├── .gitmodules
├── .gitignore
├── package.xml
├── .circleci
└── config.yml
├── LICENSE
├── CMakeLists.txt
├── launch
└── pacmod2.launch
├── include
└── pacmod
│ ├── pacmod_common.h
│ ├── pacmod_ros_msg_handler.h
│ └── pacmod_core.h
├── CHANGELOG.rst
├── README.md
└── src
├── pacmod_core.cpp
├── pacmod_ros_msg_handler.cpp
└── pacmod_node.cpp
/.gitmodules:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | bin/
3 | lib/
4 | lib_include/
5 | *.log
6 | *.swp
7 | polysync-viewer.config
8 | *.user
9 | *~
10 | .idea/
11 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | pacmod2
4 | 3.0.0
5 | AutonomouStuff PACMod driver package
6 | MIT
7 |
8 | AutonomouStuff Software Team
9 | Joe Driscoll
10 | Josh Whitley
11 |
12 | catkin
13 |
14 | roslint
15 |
16 | roscpp
17 | std_msgs
18 | pacmod_msgs
19 | can_msgs
20 |
21 |
--------------------------------------------------------------------------------
/.circleci/config.yml:
--------------------------------------------------------------------------------
1 | version: 2
2 | jobs:
3 | noetic:
4 | docker:
5 | - image: autonomoustuff/docker-builds:noetic-ros-base
6 | steps:
7 | - checkout
8 | - run:
9 | name: Set Up Container
10 | command: |
11 | apt-get update -qq
12 | source /opt/ros/*/setup.bash
13 | rosdep update && rosdep install --from-paths . --ignore-src -y
14 | cd ..
15 | catkin init
16 | catkin config --extend /opt/ros/$ROS_DISTRO
17 | - run:
18 | name: Build
19 | command: |
20 | cd ..
21 | catkin build
22 | - run:
23 | name: Run Tests
24 | command: |
25 | source /opt/ros/*/setup.bash
26 | cd ..
27 | catkin run_tests
28 | catkin_test_results
29 | working_directory: ~/src
30 |
31 | workflows:
32 | version: 2
33 | ros_build:
34 | jobs:
35 | - noetic
36 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | Copyright (c) 2019 AutonomouStuff, LLC
2 |
3 | Permission is hereby granted, free of charge, to any person obtaining a copy
4 | of this software and associated documentation files (the "Software"), to deal
5 | in the Software without restriction, including without limitation the rights
6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | copies of the Software, and to permit persons to whom the Software is
8 | furnished to do so, subject to the following conditions:
9 |
10 | The above copyright notice and this permission notice shall be included in all
11 | copies or substantial portions of the Software.
12 |
13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19 | SOFTWARE.
20 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(pacmod2)
3 |
4 | ## Add support for C++11, supported in ROS Kinetic and newer
5 | # For PACMod, this must be enabled to use mutexes.
6 | add_definitions(-std=c++11)
7 |
8 | find_package(catkin REQUIRED COMPONENTS
9 | roscpp
10 | std_msgs
11 | pacmod_msgs
12 | can_msgs
13 | roslint
14 | )
15 |
16 | catkin_package()
17 |
18 | ## Specify additional locations of header files
19 | ## Your package locations should be listed before other locations
20 | include_directories(
21 | include/
22 | ${catkin_INCLUDE_DIRS}
23 | )
24 |
25 | ## Declare a C++ executable
26 | add_executable(${PROJECT_NAME}
27 | src/pacmod_ros_msg_handler.cpp
28 | src/pacmod_node.cpp
29 | src/pacmod_core.cpp
30 | )
31 |
32 | ## Add cmake target dependencies of the executable
33 | ## same as for the library above
34 | add_dependencies(${PROJECT_NAME}
35 | ${${PROJECT_NAME}_EXPORTED_TARGETS}
36 | ${catkin_EXPORTED_TARGETS}
37 | )
38 |
39 | ## Specify libraries to link a library or executable target against
40 | target_link_libraries(${PROJECT_NAME}
41 | ${catkin_LIBRARIES}
42 | )
43 |
44 | set(ROSLINT_CPP_OPTS "--filter=-build/namespaces,-build/c++11")
45 | roslint_cpp()
46 |
47 | if(CATKIN_ENABLE_TESTING)
48 | roslint_add_test()
49 | endif()
50 |
51 | install(TARGETS ${PROJECT_NAME}
52 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
53 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
54 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
55 | )
56 |
57 | install(DIRECTORY launch/
58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
59 | )
60 |
--------------------------------------------------------------------------------
/launch/pacmod2.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
--------------------------------------------------------------------------------
/include/pacmod/pacmod_common.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019 AutonomouStuff, LLC
2 | //
3 | // Permission is hereby granted, free of charge, to any person obtaining a copy
4 | // of this software and associated documentation files (the "Software"), to deal
5 | // in the Software without restriction, including without limitation the rights
6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | // copies of the Software, and to permit persons to whom the Software is
8 | // furnished to do so, subject to the following conditions:
9 | //
10 | // The above copyright notice and this permission notice shall be included in
11 | // all copies or substantial portions of the Software.
12 | //
13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 | // THE SOFTWARE.
20 |
21 | #ifndef PACMOD_PACMOD_COMMON_H
22 | #define PACMOD_PACMOD_COMMON_H
23 |
24 | #include
25 | #include
26 |
27 | #include
28 | #include
29 | #include
30 | #include
31 | #include
32 | #include
33 | #include
34 | #include
35 | #include
36 | #include
37 | #include
38 | #include
39 | #include
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 |
47 | #endif // PACMOD_PACMOD_COMMON_H
48 |
--------------------------------------------------------------------------------
/include/pacmod/pacmod_ros_msg_handler.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019 AutonomouStuff, LLC
2 | //
3 | // Permission is hereby granted, free of charge, to any person obtaining a copy
4 | // of this software and associated documentation files (the "Software"), to deal
5 | // in the Software without restriction, including without limitation the rights
6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | // copies of the Software, and to permit persons to whom the Software is
8 | // furnished to do so, subject to the following conditions:
9 | //
10 | // The above copyright notice and this permission notice shall be included in
11 | // all copies or substantial portions of the Software.
12 | //
13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 | // THE SOFTWARE.
20 |
21 | #ifndef PACMOD_PACMOD_ROS_MSG_HANDLER_H
22 | #define PACMOD_PACMOD_ROS_MSG_HANDLER_H
23 |
24 | #include
25 |
26 | #include
27 | #include
28 | #include
29 | #include
30 |
31 | namespace AS
32 | {
33 | namespace Drivers
34 | {
35 | namespace PACMod
36 | {
37 | class LockedData
38 | {
39 | public:
40 | LockedData();
41 |
42 | bool isValid() const;
43 | void setIsValid(bool valid);
44 |
45 | std::vector getData() const;
46 | void setData(std::vector new_data);
47 |
48 | private:
49 | std::vector _data;
50 | bool _is_valid;
51 | mutable std::mutex _data_mut;
52 | mutable std::mutex _valid_mut;
53 | };
54 |
55 | class PacmodTxRosMsgHandler
56 | {
57 | public:
58 | void fillAndPublish(const int64_t& can_id,
59 | std::string frame_id,
60 | const ros::Publisher& pub,
61 | const std::shared_ptr& parser_class);
62 |
63 | private:
64 | void fillSystemRptInt(const std::shared_ptr& parser_class,
65 | pacmod_msgs::SystemRptInt* new_msg,
66 | std::string frame_id);
67 | void fillSystemRptFloat(const std::shared_ptr& parser_class,
68 | pacmod_msgs::SystemRptFloat* new_msg,
69 | std::string frame_id);
70 | void fillGlobalRpt(const std::shared_ptr& parser_class,
71 | pacmod_msgs::GlobalRpt* new_msg,
72 | std::string frame_id);
73 | void fillVehicleSpeedRpt(const std::shared_ptr& parser_class,
74 | pacmod_msgs::VehicleSpeedRpt* new_msg,
75 | std::string frame_id);
76 | void fillMotorRpt1(const std::shared_ptr& parser_class,
77 | pacmod_msgs::MotorRpt1* new_msg,
78 | std::string frame_id);
79 | void fillMotorRpt2(const std::shared_ptr& parser_class,
80 | pacmod_msgs::MotorRpt2* new_msg,
81 | std::string frame_id);
82 | void fillMotorRpt3(const std::shared_ptr& parser_class,
83 | pacmod_msgs::MotorRpt3* new_msg,
84 | std::string frame_id);
85 | void fillWheelSpeedRpt(const std::shared_ptr& parser_class,
86 | pacmod_msgs::WheelSpeedRpt* new_msg,
87 | std::string frame_id);
88 | void fillSteeringPIDRpt1(const std::shared_ptr& parser_class,
89 | pacmod_msgs::SteeringPIDRpt1* new_msg,
90 | std::string frame_id);
91 | void fillSteeringPIDRpt2(const std::shared_ptr& parser_class,
92 | pacmod_msgs::SteeringPIDRpt2* new_msg,
93 | std::string frame_id);
94 | void fillSteeringPIDRpt3(const std::shared_ptr& parser_class,
95 | pacmod_msgs::SteeringPIDRpt3* new_msg,
96 | std::string frame_id);
97 | void fillParkingBrakeStatusRpt(const std::shared_ptr& parser_class,
98 | pacmod_msgs::ParkingBrakeStatusRpt* new_msg,
99 | std::string frame_id);
100 | void fillYawRateRpt(const std::shared_ptr& parser_class,
101 | pacmod_msgs::YawRateRpt* new_msg,
102 | std::string frame_id);
103 | void fillLatLonHeadingRpt(const std::shared_ptr& parser_class,
104 | pacmod_msgs::LatLonHeadingRpt* new_msg,
105 | std::string frame_id);
106 | void fillDateTimeRpt(const std::shared_ptr& parser_class,
107 | pacmod_msgs::DateTimeRpt* new_msg,
108 | std::string frame_id);
109 | void fillSteeringPIDRpt4(const std::shared_ptr& parser_class,
110 | pacmod_msgs::SteeringPIDRpt4* new_msg,
111 | std::string frame_id);
112 | void fillVinRpt(const std::shared_ptr& parser_class,
113 | pacmod_msgs::VinRpt* new_msg,
114 | std::string frame_id);
115 | };
116 |
117 | class PacmodRxRosMsgHandler
118 | {
119 | public:
120 | static std::vector unpackAndEncode(const int64_t& can_id,
121 | const pacmod_msgs::PacmodCmd::ConstPtr& msg);
122 | static std::vector unpackAndEncode(const int64_t& can_id,
123 | const pacmod_msgs::PositionWithSpeed::ConstPtr& msg);
124 | };
125 | } // namespace PACMod
126 | } // namespace Drivers
127 | } // namespace AS
128 |
129 | #endif // PACMOD_PACMOD_ROS_MSG_HANDLER_H
130 |
--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2 | Changelog for package pacmod2
3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4 |
5 | 3.0.0 (2022-01-05)
6 | ------------------
7 | * Rename to pacmod2 (`#20 `_)
8 | * Update README (`#18 `_)
9 | * Noetic Updates (`#19 `_)
10 | * Contributors: icolwell-as
11 |
12 | 2.1.0 (2020-02-10)
13 | ------------------
14 | * Merge pull request `#14 `_ from astuff/fix/updated_msgs
15 | URGENT: Fix message changes from pacmod_msgs.
16 | * Merge pull request `#13 `_ from astuff/maint/code_cleanup
17 | Formatting clean-up of code and updating of license headers.
18 | * Merge pull request `#11 `_ from astuff/maint/roslint_cleanup
19 | Removing roslint exception. Cleaning up include paths.
20 | * Merge pull request `#10 `_ from astuff/maint/remove_redundant_folders
21 | Moving all files up a directory, consistent with other packages.
22 | * Moving DBC to new repo. CI: Removing Indigo.
23 | * Merge pull request `#6 `_ from astuff/maint/add_melodic_build
24 | Maint/add melodic build
25 | * Allowed_failures wasn't working before.
26 | * Forgot to remove ROSINSTALL_FILENAME from allowed failures.
27 | * Removing ROSINSTALL_FILE as it isn't necessary with a version postfix.
28 | * Fixing ROSINSTALL_FILENAME in Travis.
29 | * Adding separate rosinstall files for lunar and melodic.
30 | * Adding melodic build to Travis.
31 | * Contributors: Daniel-Stanek, Joe Kale, Joshua Whitley, Nishanth Samala, Sam Rustan, Zach Oakes
32 |
33 | 2.0.2 (2018-06-11)
34 | ------------------
35 | * Merge pull request `#3 `_ from astuff/maint/adding_roslint
36 | Maint/adding roslint
37 | * Applying all recommended changes from roslint.
38 | * Adding roslint as build requirement.
39 | * Adding DBC file to repo.
40 | * Changed 'overridden' to 'override_active' (name was changed in GlobalMsgRpt)
41 | * Updating license in package.xml.
42 | * Missing remaps for socketcan.
43 | * Only sending commands when enabled.
44 | * Listening for output instead of manual_input while disabled.
45 | * When disabled, making command match report.
46 | * Updating package.xml to format 2.
47 | * Adding MIT license flower box to all source files.
48 | * Documenting vehicle types before open-sourcing.
49 | * Adding missing loop pause on can_write.
50 | * Updating launch files for kvaser_interface.
51 | * Changing topic names to match convention.
52 | * Cleaning up launch file.
53 | * Removing can_interface requirement. Minor clean-up.
54 | * Merging in core pre-open-sourcing.
55 | * Removing pause on CAN handle re-open.
56 | * Fixing parsing bug in VinRpt.
57 | * Fixing segfault with bad pointer.
58 | * New encoding method.
59 | * Making PacmodRosMsgHandler into PacmodTxRosMsgHandler.
60 | * Changing read to new parsing method.
61 | * Moving headers to pacmod_common.h.
62 | * Adding VinRpt.
63 | * Minor change to order of operations on connect.
64 | * Making reader and writer connect to CAN the same way.
65 | * Initializing CAN_IDs the right way.
66 | * Moving all CAN_IDs to class properties - similar to other drivers.
67 | * Cleaning up launch file formatting.
68 | * Cleaning up package docs.
69 | * Removing install of README which doesn't exist anymore.
70 | * Keeping reader open.
71 | * Adding a check for ROS while trying to connect.
72 | * Add populate timestamps in messages
73 | * Change steering pid 3 message, populate time stamp in rx echo, disable tx echo so tx_can doesn't include rx messages, add wheel speed and steer pid4 messages
74 | * Fix scaling on steering PID messages
75 | * Use sleep_until instead of sleep_for in can_send loop so it will send closer to the desired rate
76 | * Adding license.
77 | * Updating README.
78 | * Fixing bug that causes thread to need killing if CAN channel is unavailable.
79 | * More error handling. Now repeatedly attempts to connect.
80 | Added further error handling to canSend.
81 | Now attempts to connect both can_reader and can_writer with 1s delay
82 | time.
83 | * Adding more error reporting. Shutting down on CAN open errors.
84 | Added more extensive error-reporting messages.
85 | On CAN open errors in either the reader or the writer, shut down (will
86 | be changed later).
87 | * Matching changes made to can_interface.
88 | * Fixing data types on DateTime message.
89 | * Updating value types for LatLonHeading message.
90 | * Added headlight and horn subscribers.
91 | * Setting up more vehicle-specific publishing and subscribing. Updates to core.
92 | * Only parsing headlights if vehicle is Lexus.
93 | * Changes consistent with can_interface cleanup.
94 | * Changing topic names to be more consistent with existing topics.
95 | * Making some CAN publishing optional.
96 | * Fixing missing yaw_rate_rpt_pub.
97 | * Added parameter for vehicle_type. Added conditional publishers.
98 | Also added needed messages which include timestamps. Completed conditional logic
99 | for POLARIS_GEM, POLARIS_RANGER, INTERNATIONAL_PROSTAR_122, and LEXUS_RX_450H.
100 | * Adding parsing support for messages from additional supported vehicles.
101 | * Added code to handle semi windshield wipers
102 | * Making canSend more efficient.
103 | * Reset all values to default when PACMod is disabled.
104 | * Parsing error messages on general report.
105 | * Fixed scale value (10->1000) in SystemRptFloatMsg
106 | * Setting clear_override to always be true.
107 | * Changing to periodic message burst instead of ad-hoc transmission.
108 | * Minor code cleanup
109 | * Changing can_rx_forward to can_rx.
110 | * Migrating from can_interface/CanFrame to can_msgs/Frame.
111 | * added new topic as_tx/vehicle_speed for m/s speed
112 | * changed how vehicle speed is calculated
113 | * Fixes for heartbeat and vehicle speed.
114 | * Removing initial enable/disable.
115 | * Adding override debounce.
116 | * Sends heartbeat. Listens for override on PACMod and adjusts heartbeat signal accordingly.
117 | * Creating separate messages package. Cannot remove C++11 requirement - need mutexes.
118 | * Contributors: Christopher Vigna, Daniel Stanek, Joe Driscoll, Joe Kale, Joshua Whitley, Sam Rustan, Nathan Imig
119 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # PACMod2 (Platform Actuation and Control MODule) ROS Driver #
2 |
3 | [](https://circleci.com/gh/astuff/pacmod/tree/master)
4 |
5 | This ROS node is designed to allow the user to control a vehicle (see Supported Vehicles below) with the PACMod drive-by-wire system, board revision 2.
6 | Note that this driver is meant for PACMod 2 systems and will not work for newer PACMod 3 systems.
7 | See the [pacmod3 repo](https://github.com/astuff/pacmod3) for a ROS driver for PACMod 3 systems.
8 |
9 | For access to the DBC file which defines the CAN interface for the PACMod 2, see the [pacmod1_2_dbc](https://github.com/astuff/pacmod1_2_dbc) repo.
10 |
11 | ## Installation
12 |
13 | Install pacmod using our debian repository:
14 |
15 | ```sh
16 | sudo apt install apt-transport-https
17 | sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list'
18 | sudo apt update
19 | sudo apt install ros-$ROS_DISTRO-pacmod2
20 | ```
21 |
22 | Note: Previously the pacmod driver was released via the ROS buildfarm.
23 | This has changed as of Ubuntu 20.04 (ROS2 Foxy and ROS1 Noetic) to keep old package versions available for download, which gives users greater control over their installed software and also allows downgrades if an upgrade breaks software dependencies.
24 |
25 | ## ROS API
26 |
27 | ### Launch Arguments
28 |
29 | - **pacmod_vehicle_type**: This should be set to match the vehicle you are using.
30 | - **use_kvaser**: Set this to true if a Kvaser CAN device is being used with Kvaser canlib drivers to connect to the PACMod. Defaults to `false`.
31 | - **kvaser_hardware_id**: The hardware id of the kvaser device, only applies if `use_kvaser` is true.
32 | - **kvaser_circuit_id**: The circuit/channel id that the PACMod is plugged into on the kvaser device, only applies if `use_kvaser` is true.
33 | - **use_socketcan**: Set this to true if Linux SocketCAN drivers are being used to connect to the PACMod. Defaults to `false`.
34 | - **socketcan_device**: The device id of the SocketCAN channel the PACMod is plugged into, only applies if `use_socketcan` is true.
35 | - **namespace**: The namespace of the PACMod driver, topics will be namespaced accordingly. Defaults to `pacmod`.
36 |
37 | ### Published Topics
38 |
39 | Topics published on all platforms:
40 |
41 | - `parsed_tx/accel_rpt` ([pacmod_msgs/SystemRptFloat](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptFloat.msg))
42 | - `parsed_tx/brake_rpt` ([pacmod_msgs/SystemRptFloat](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptFloat.msg))
43 | - `parsed_tx/steer_rpt` ([pacmod_msgs/SystemRptFloat](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptFloat.msg))
44 | - `parsed_tx/shift_rpt` ([pacmod_msgs/SystemRptInt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptInt.msg))
45 | - `parsed_tx/turn_rpt` ([pacmod_msgs/SystemRptInt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptInt.msg))
46 | - `parsed_tx/vehicle_speed_rpt` ([pacmod_msgs/VehicleSpeedRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/VehicleSpeedRpt.msg))
47 | - `parsed_tx/vin_rpt` ([pacmod_msgs/VinRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/VinRpt.msg))
48 | - `parsed_tx/global_rpt` ([pacmod_msgs/GlobalRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/GlobalRpt.msg))
49 | - `as_tx/vehicle_speed` ([std_msgs/Float64](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float64.html))
50 | - `as_tx/enable` ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html))
51 | - `can_rx` ([can_msgs/Frame](http://docs.ros.org/en/noetic/api/can_msgs/html/msg/Frame.html))
52 |
53 | Topics published on supported platforms only:
54 |
55 | - `parsed_tx/brake_rpt_detail_1` ([pacmod_msgs/MotorRpt1](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/MotorRpt1.msg))
56 | - `parsed_tx/brake_rpt_detail_2` ([pacmod_msgs/MotorRpt2](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/MotorRpt2.msg))
57 | - `parsed_tx/brake_rpt_detail_3` ([pacmod_msgs/MotorRpt3](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/MotorRpt3.msg))
58 | - `parsed_tx/steer_rpt_2` ([pacmod_msgs/SystemRptFloat](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptFloat.msg))
59 | - `parsed_tx/steer_rpt_3` ([pacmod_msgs/SystemRptFloat](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptFloat.msg))
60 | - `parsed_tx/steer_rpt_detail_1` ([pacmod_msgs/MotorRpt1](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/MotorRpt1.msg))
61 | - `parsed_tx/steer_rpt_detail_2` ([pacmod_msgs/MotorRpt2](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/MotorRpt2.msg))
62 | - `parsed_tx/steer_rpt_detail_3` ([pacmod_msgs/MotorRpt3](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/MotorRpt3.msg))
63 | - `parsed_tx/steer_pid_rpt_1` ([pacmod_msgs/SteeringPIDRpt1](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SteeringPIDRpt1.msg))
64 | - `parsed_tx/steer_pid_rpt_2` ([pacmod_msgs/SteeringPIDRpt2](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SteeringPIDRpt2.msg))
65 | - `parsed_tx/steer_pid_rpt_3` ([pacmod_msgs/SteeringPIDRpt3](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SteeringPIDRpt3.msg))
66 | - `parsed_tx/steer_pid_rpt_4` ([pacmod_msgs/SteeringPIDRpt4](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SteeringPIDRpt4.msg))
67 | - `parsed_tx/wiper_rpt` ([pacmod_msgs/SystemRptInt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptInt.msg))
68 | - `parsed_tx/horn_rpt` ([pacmod_msgs/SystemRptInt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/SystemRptInt.msg))
69 | - `parsed_tx/yaw_rate_rpt` ([pacmod_msgs/YawRateRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/YawRateRpt.msg))
70 | - `parsed_tx/lat_lon_heading_rpt` ([pacmod_msgs/LatLonHeadingRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/LatLonHeadingRpt.msg))
71 | - `parsed_tx/wheel_speed_rpt` ([pacmod_msgs/WheelSpeedRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/WheelSpeedRpt.msg))
72 | - `parsed_tx/date_time_rpt` ([pacmod_msgs/DateTimeRpt](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/DateTimeRpt.msg))
73 |
74 | ### Subscribed Topics
75 |
76 | Topics subscribed on all platforms:
77 |
78 | - `as_rx/accel_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
79 | - `as_rx/brake_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
80 | - `as_rx/steer_cmd` ([pacmod_msgs/PositionWithSpeed](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PositionWithSpeed.msg))
81 | - `as_rx/shift_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
82 | - `as_rx/turn_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
83 | - `as_rx/enable` ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html))
84 | - `can_tx` ([can_msgs/Frame](http://docs.ros.org/en/noetic/api/can_msgs/html/msg/Frame.html))
85 |
86 | Topics subscribed on supported platforms only:
87 |
88 | - `as_rx/wiper_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
89 | - `as_rx/horn_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
90 | - `as_rx/headlight_cmd` ([pacmod_msgs/PacmodCmd](https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg))
91 |
92 | ## Supported Vehicles ##
93 |
94 | - Polaris GEM Series (e2/e4/e6) MY 2016+
95 | - Polaris eLXD MY 2016+
96 | - Polaris Ranger X900
97 | - International Prostar+ 122
98 | - Lexus RX-450h MY 2016+
99 |
100 | Note: If you have a pacmod system and don't see your vehicle listed above, you likely have a [PACMod3 system](https://github.com/astuff/pacmod3).
101 |
--------------------------------------------------------------------------------
/include/pacmod/pacmod_core.h:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019 AutonomouStuff, LLC
2 | //
3 | // Permission is hereby granted, free of charge, to any person obtaining a copy
4 | // of this software and associated documentation files (the "Software"), to deal
5 | // in the Software without restriction, including without limitation the rights
6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | // copies of the Software, and to permit persons to whom the Software is
8 | // furnished to do so, subject to the following conditions:
9 | //
10 | // The above copyright notice and this permission notice shall be included in
11 | // all copies or substantial portions of the Software.
12 | //
13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 | // THE SOFTWARE.
20 |
21 | #ifndef PACMOD_PACMOD_CORE_H
22 | #define PACMOD_PACMOD_CORE_H
23 |
24 | #include
25 | #include
26 | #include
27 | #include
28 | #include
29 |
30 | namespace AS
31 | {
32 | namespace Drivers
33 | {
34 | namespace PACMod
35 | {
36 | enum VehicleType
37 | {
38 | POLARIS_GEM,
39 | POLARIS_RANGER,
40 | INTERNATIONAL_PROSTAR_122,
41 | LEXUS_RX_450H
42 | };
43 |
44 | class PacmodTxMsg
45 | {
46 | public:
47 | static std::shared_ptr make_message(const int64_t& can_id);
48 | virtual void parse(uint8_t *in) = 0;
49 | };
50 |
51 | // TX Messages
52 | class GlobalRptMsg :
53 | public PacmodTxMsg
54 | {
55 | public:
56 | static const int64_t CAN_ID;
57 |
58 | bool enabled;
59 | bool override_active;
60 | bool user_can_timeout;
61 | bool brake_can_timeout;
62 | bool steering_can_timeout;
63 | bool vehicle_can_timeout;
64 | uint16_t user_can_read_errors;
65 |
66 | void parse(uint8_t *in);
67 | };
68 |
69 | class VinRptMsg :
70 | public PacmodTxMsg
71 | {
72 | public:
73 | static const int64_t CAN_ID;
74 |
75 | std::string mfg_code;
76 | std::string mfg;
77 | char model_year_code;
78 | uint32_t model_year;
79 | uint32_t serial;
80 |
81 | void parse(uint8_t *in);
82 | };
83 |
84 | class SystemRptIntMsg :
85 | public PacmodTxMsg
86 | {
87 | public:
88 | uint32_t manual_input;
89 | uint32_t command;
90 | uint32_t output;
91 |
92 | void parse(uint8_t *in);
93 | };
94 |
95 | class TurnSignalRptMsg :
96 | public SystemRptIntMsg
97 | {
98 | public:
99 | static const int64_t CAN_ID;
100 | };
101 |
102 | class HeadlightRptMsg :
103 | public SystemRptIntMsg
104 | {
105 | public:
106 | static const int64_t CAN_ID;
107 | };
108 |
109 | class HornRptMsg :
110 | public SystemRptIntMsg
111 | {
112 | public:
113 | static const int64_t CAN_ID;
114 | };
115 |
116 | class WiperRptMsg :
117 | public SystemRptIntMsg
118 | {
119 | public:
120 | static const int64_t CAN_ID;
121 | };
122 |
123 | class ShiftRptMsg :
124 | public SystemRptIntMsg
125 | {
126 | public:
127 | static const int64_t CAN_ID;
128 | };
129 |
130 | class SystemRptFloatMsg :
131 | public PacmodTxMsg
132 | {
133 | public:
134 | double manual_input;
135 | double command;
136 | double output;
137 |
138 | void parse(uint8_t *in);
139 | };
140 |
141 | class AccelRptMsg :
142 | public SystemRptFloatMsg
143 | {
144 | public:
145 | static const int64_t CAN_ID;
146 | };
147 |
148 | class SteerRptMsg :
149 | public SystemRptFloatMsg
150 | {
151 | public:
152 | static const int64_t CAN_ID;
153 | };
154 |
155 | class SteerRpt2Msg :
156 | public SystemRptFloatMsg
157 | {
158 | public:
159 | static const int64_t CAN_ID;
160 | };
161 |
162 | class SteerRpt3Msg :
163 | public SystemRptFloatMsg
164 | {
165 | public:
166 | static const int64_t CAN_ID;
167 | };
168 |
169 | class BrakeRptMsg :
170 | public SystemRptFloatMsg
171 | {
172 | public:
173 | static const int64_t CAN_ID;
174 | };
175 |
176 | class VehicleSpeedRptMsg :
177 | public PacmodTxMsg
178 | {
179 | public:
180 | static const int64_t CAN_ID;
181 |
182 | double vehicle_speed;
183 | bool vehicle_speed_valid;
184 | uint8_t vehicle_speed_raw[2];
185 |
186 | void parse(uint8_t *in);
187 | };
188 |
189 | class MotorRpt1Msg :
190 | public PacmodTxMsg
191 | {
192 | public:
193 | double current;
194 | double position;
195 |
196 | void parse(uint8_t *in);
197 | };
198 |
199 | class BrakeMotorRpt1Msg :
200 | public MotorRpt1Msg
201 | {
202 | public:
203 | static const int64_t CAN_ID;
204 | };
205 |
206 | class SteerMotorRpt1Msg :
207 | public MotorRpt1Msg
208 | {
209 | public:
210 | static const int64_t CAN_ID;
211 | };
212 |
213 | class MotorRpt2Msg :
214 | public PacmodTxMsg
215 | {
216 | public:
217 | double encoder_temp;
218 | double motor_temp;
219 | double velocity;
220 |
221 | void parse(uint8_t *in);
222 | };
223 |
224 | class BrakeMotorRpt2Msg :
225 | public MotorRpt2Msg
226 | {
227 | public:
228 | static const int64_t CAN_ID;
229 | };
230 |
231 | class SteerMotorRpt2Msg :
232 | public MotorRpt2Msg
233 | {
234 | public:
235 | static const int64_t CAN_ID;
236 | };
237 |
238 | class MotorRpt3Msg :
239 | public PacmodTxMsg
240 | {
241 | public:
242 | double torque_output;
243 | double torque_input;
244 |
245 | void parse(uint8_t *in);
246 | };
247 |
248 | class BrakeMotorRpt3Msg :
249 | public MotorRpt3Msg
250 | {
251 | public:
252 | static const int64_t CAN_ID;
253 | };
254 |
255 | class SteerMotorRpt3Msg :
256 | public MotorRpt3Msg
257 | {
258 | public:
259 | static const int64_t CAN_ID;
260 | };
261 |
262 | class YawRateRptMsg :
263 | public PacmodTxMsg
264 | {
265 | public:
266 | static const int64_t CAN_ID;
267 |
268 | double yaw_rate;
269 |
270 | void parse(uint8_t *in);
271 | };
272 |
273 | class LatLonHeadingRptMsg :
274 | public PacmodTxMsg
275 | {
276 | public:
277 | static const int64_t CAN_ID;
278 |
279 | int latitude_degrees;
280 | uint32_t latitude_minutes;
281 | uint32_t latitude_seconds;
282 | int longitude_degrees;
283 | uint32_t longitude_minutes;
284 | uint32_t longitude_seconds;
285 | double heading;
286 |
287 | void parse(uint8_t *in);
288 | };
289 |
290 | class DateTimeRptMsg :
291 | public PacmodTxMsg
292 | {
293 | public:
294 | static const int64_t CAN_ID;
295 |
296 | uint32_t year;
297 | uint8_t month;
298 | uint8_t day;
299 | uint8_t hour;
300 | uint8_t minute;
301 | uint8_t second;
302 |
303 | void parse(uint8_t *in);
304 | };
305 |
306 | class WheelSpeedRptMsg :
307 | public PacmodTxMsg
308 | {
309 | public:
310 | static const int64_t CAN_ID;
311 |
312 | double front_left_wheel_speed;
313 | double front_right_wheel_speed;
314 | double rear_left_wheel_speed;
315 | double rear_right_wheel_speed;
316 |
317 | void parse(uint8_t *in);
318 | };
319 |
320 | class SteeringPIDRpt1Msg :
321 | public PacmodTxMsg
322 | {
323 | public:
324 | static const int64_t CAN_ID;
325 |
326 | double dt;
327 | double kp;
328 | double ki;
329 | double kd;
330 |
331 | void parse(uint8_t *in);
332 | };
333 |
334 | class SteeringPIDRpt2Msg :
335 | public PacmodTxMsg
336 | {
337 | public:
338 | static const int64_t CAN_ID;
339 |
340 | double p_term;
341 | double i_term;
342 | double d_term;
343 | double all_terms;
344 |
345 | void parse(uint8_t *in);
346 | };
347 |
348 | class SteeringPIDRpt3Msg :
349 | public PacmodTxMsg
350 | {
351 | public:
352 | static const int64_t CAN_ID;
353 |
354 | double new_torque;
355 | double str_angle_desired;
356 | double str_angle_actual;
357 | double error;
358 |
359 | void parse(uint8_t *in);
360 | };
361 |
362 | class SteeringPIDRpt4Msg :
363 | public PacmodTxMsg
364 | {
365 | public:
366 | static const int64_t CAN_ID;
367 |
368 | double angular_velocity;
369 | double angular_acceleration;
370 |
371 | void parse(uint8_t *in);
372 | };
373 |
374 | class ParkingBrakeStatusRptMsg :
375 | public PacmodTxMsg
376 | {
377 | public:
378 | static const int64_t CAN_ID;
379 |
380 | bool parking_brake_engaged;
381 |
382 | void parse(uint8_t *in);
383 | };
384 |
385 | // RX Messages
386 | class PacmodRxMsg
387 | {
388 | public:
389 | std::vector data;
390 | };
391 |
392 | class GlobalCmdMsg :
393 | public PacmodRxMsg
394 | {
395 | public:
396 | static const int64_t CAN_ID;
397 |
398 | void encode(bool enable, bool clear_override, bool ignore_overide);
399 | };
400 |
401 | class TurnSignalCmdMsg :
402 | public PacmodRxMsg
403 | {
404 | public:
405 | static const int64_t CAN_ID;
406 |
407 | void encode(uint8_t turn_signal_cmd);
408 | };
409 |
410 | class HeadlightCmdMsg :
411 | public PacmodRxMsg
412 | {
413 | public:
414 | static const int64_t CAN_ID;
415 |
416 | void encode(uint8_t headlight_cmd);
417 | };
418 |
419 | class HornCmdMsg :
420 | public PacmodRxMsg
421 | {
422 | public:
423 | static const int64_t CAN_ID;
424 |
425 | void encode(uint8_t horn_cmd);
426 | };
427 |
428 | class WiperCmdMsg :
429 | public PacmodRxMsg
430 | {
431 | public:
432 | static const int64_t CAN_ID;
433 |
434 | void encode(uint8_t wiper_cmd);
435 | };
436 |
437 | class ShiftCmdMsg :
438 | public PacmodRxMsg
439 | {
440 | public:
441 | static const int64_t CAN_ID;
442 |
443 | void encode(uint8_t shift_cmd);
444 | };
445 |
446 | class AccelCmdMsg :
447 | public PacmodRxMsg
448 | {
449 | public:
450 | static const int64_t CAN_ID;
451 |
452 | void encode(double accel_cmd);
453 | };
454 |
455 | class SteerCmdMsg :
456 | public PacmodRxMsg
457 | {
458 | public:
459 | static const int64_t CAN_ID;
460 |
461 | void encode(double steer_pos, double steer_spd);
462 | };
463 |
464 | class BrakeCmdMsg :
465 | public PacmodRxMsg
466 | {
467 | public:
468 | static const int64_t CAN_ID;
469 |
470 | void encode(double brake_cmd);
471 | };
472 | } // namespace PACMod
473 | } // namespace Drivers
474 | } // namespace AS
475 |
476 | #endif // PACMOD_PACMOD_CORE_H
477 |
--------------------------------------------------------------------------------
/src/pacmod_core.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019 AutonomouStuff, LLC
2 | //
3 | // Permission is hereby granted, free of charge, to any person obtaining a copy
4 | // of this software and associated documentation files (the "Software"), to deal
5 | // in the Software without restriction, including without limitation the rights
6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | // copies of the Software, and to permit persons to whom the Software is
8 | // furnished to do so, subject to the following conditions:
9 | //
10 | // The above copyright notice and this permission notice shall be included in
11 | // all copies or substantial portions of the Software.
12 | //
13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 | // THE SOFTWARE.
20 |
21 | #include
22 |
23 | #include
24 |
25 | namespace AS
26 | {
27 | namespace Drivers
28 | {
29 | namespace PACMod
30 | {
31 |
32 | const int64_t TurnSignalCmdMsg::CAN_ID = 0x63;
33 | const int64_t TurnSignalRptMsg::CAN_ID = 0x64;
34 | const int64_t ShiftCmdMsg::CAN_ID = 0x65;
35 | const int64_t ShiftRptMsg::CAN_ID = 0x66;
36 | const int64_t AccelCmdMsg::CAN_ID = 0x67;
37 | const int64_t AccelRptMsg::CAN_ID = 0x68;
38 | const int64_t GlobalCmdMsg::CAN_ID = 0x69;
39 | const int64_t GlobalRptMsg::CAN_ID = 0x6A;
40 | const int64_t BrakeCmdMsg::CAN_ID = 0x6B;
41 | const int64_t SteerCmdMsg::CAN_ID = 0x6D;
42 | const int64_t BrakeRptMsg::CAN_ID = 0x6C;
43 | const int64_t SteerRptMsg::CAN_ID = 0x6E;
44 | const int64_t VehicleSpeedRptMsg::CAN_ID = 0x6F;
45 | const int64_t BrakeMotorRpt1Msg::CAN_ID = 0x70;
46 | const int64_t BrakeMotorRpt2Msg::CAN_ID = 0x71;
47 | const int64_t BrakeMotorRpt3Msg::CAN_ID = 0x72;
48 | const int64_t SteerMotorRpt1Msg::CAN_ID = 0x73;
49 | const int64_t SteerMotorRpt2Msg::CAN_ID = 0x74;
50 | const int64_t SteerMotorRpt3Msg::CAN_ID = 0x75;
51 | const int64_t HeadlightCmdMsg::CAN_ID = 0x76;
52 | const int64_t HeadlightRptMsg::CAN_ID = 0x77;
53 | const int64_t HornCmdMsg::CAN_ID = 0x78;
54 | const int64_t HornRptMsg::CAN_ID = 0x79;
55 | const int64_t WheelSpeedRptMsg::CAN_ID = 0x7A;
56 | const int64_t SteeringPIDRpt1Msg::CAN_ID = 0x7B;
57 | const int64_t SteeringPIDRpt2Msg::CAN_ID = 0x7C;
58 | const int64_t SteeringPIDRpt3Msg::CAN_ID = 0x7D;
59 | const int64_t SteerRpt2Msg::CAN_ID = 0x7E;
60 | const int64_t SteerRpt3Msg::CAN_ID = 0x7F;
61 | const int64_t ParkingBrakeStatusRptMsg::CAN_ID = 0x80;
62 | const int64_t YawRateRptMsg::CAN_ID = 0x81;
63 | const int64_t LatLonHeadingRptMsg::CAN_ID = 0x82;
64 | const int64_t DateTimeRptMsg::CAN_ID = 0x83;
65 | const int64_t SteeringPIDRpt4Msg::CAN_ID = 0x84;
66 | const int64_t WiperCmdMsg::CAN_ID = 0x90;
67 | const int64_t WiperRptMsg::CAN_ID = 0x91;
68 | const int64_t VinRptMsg::CAN_ID = 0xFF;
69 |
70 | std::shared_ptr PacmodTxMsg::make_message(const int64_t& can_id)
71 | {
72 | switch (can_id)
73 | {
74 | case TurnSignalRptMsg::CAN_ID:
75 | return std::shared_ptr(new TurnSignalRptMsg);
76 | break;
77 | case ShiftRptMsg::CAN_ID:
78 | return std::shared_ptr(new ShiftRptMsg);
79 | break;
80 | case AccelRptMsg::CAN_ID:
81 | return std::shared_ptr(new AccelRptMsg);
82 | break;
83 | case GlobalRptMsg::CAN_ID:
84 | return std::shared_ptr(new GlobalRptMsg);
85 | break;
86 | case BrakeRptMsg::CAN_ID:
87 | return std::shared_ptr(new BrakeRptMsg);
88 | break;
89 | case SteerRptMsg::CAN_ID:
90 | return std::shared_ptr(new SteerRptMsg);
91 | break;
92 | case VehicleSpeedRptMsg::CAN_ID:
93 | return std::shared_ptr(new VehicleSpeedRptMsg);
94 | break;
95 | case BrakeMotorRpt1Msg::CAN_ID:
96 | return std::shared_ptr(new BrakeMotorRpt1Msg);
97 | break;
98 | case BrakeMotorRpt2Msg::CAN_ID:
99 | return std::shared_ptr(new BrakeMotorRpt2Msg);
100 | break;
101 | case BrakeMotorRpt3Msg::CAN_ID:
102 | return std::shared_ptr(new BrakeMotorRpt3Msg);
103 | break;
104 | case SteerMotorRpt1Msg::CAN_ID:
105 | return std::shared_ptr(new SteerMotorRpt1Msg);
106 | break;
107 | case SteerMotorRpt2Msg::CAN_ID:
108 | return std::shared_ptr(new SteerMotorRpt2Msg);
109 | break;
110 | case SteerMotorRpt3Msg::CAN_ID:
111 | return std::shared_ptr(new SteerMotorRpt3Msg);
112 | break;
113 | case HeadlightRptMsg::CAN_ID:
114 | return std::shared_ptr(new HeadlightRptMsg);
115 | break;
116 | case HornRptMsg::CAN_ID:
117 | return std::shared_ptr(new HornRptMsg);
118 | break;
119 | case WheelSpeedRptMsg::CAN_ID:
120 | return std::shared_ptr(new WheelSpeedRptMsg);
121 | break;
122 | case SteeringPIDRpt1Msg::CAN_ID:
123 | return std::shared_ptr(new SteeringPIDRpt1Msg);
124 | break;
125 | case SteeringPIDRpt2Msg::CAN_ID:
126 | return std::shared_ptr(new SteeringPIDRpt2Msg);
127 | break;
128 | case SteeringPIDRpt3Msg::CAN_ID:
129 | return std::shared_ptr(new SteeringPIDRpt3Msg);
130 | break;
131 | case SteerRpt2Msg::CAN_ID:
132 | return std::shared_ptr(new SteerRpt2Msg);
133 | break;
134 | case SteerRpt3Msg::CAN_ID:
135 | return std::shared_ptr(new SteerRpt3Msg);
136 | break;
137 | case ParkingBrakeStatusRptMsg::CAN_ID:
138 | return std::shared_ptr(new ParkingBrakeStatusRptMsg);
139 | break;
140 | case YawRateRptMsg::CAN_ID:
141 | return std::shared_ptr(new YawRateRptMsg);
142 | break;
143 | case LatLonHeadingRptMsg::CAN_ID:
144 | return std::shared_ptr(new LatLonHeadingRptMsg);
145 | break;
146 | case DateTimeRptMsg::CAN_ID:
147 | return std::shared_ptr(new DateTimeRptMsg);
148 | break;
149 | case SteeringPIDRpt4Msg::CAN_ID:
150 | return std::shared_ptr(new SteeringPIDRpt4Msg);
151 | break;
152 | case WiperRptMsg::CAN_ID:
153 | return std::shared_ptr(new WiperRptMsg);
154 | break;
155 | case VinRptMsg::CAN_ID:
156 | return std::shared_ptr(new VinRptMsg);
157 | break;
158 | default:
159 | return NULL;
160 | }
161 | }
162 |
163 | // TX Messages
164 | void GlobalRptMsg::parse(uint8_t *in)
165 | {
166 | enabled = in[0] & 0x01;
167 | override_active = ((in[0] & 0x02) >> 1) != 0;
168 | user_can_timeout = ((in[0] & 0x20) >> 5) != 0;
169 | brake_can_timeout = ((in[0] & 0x10) >> 4) != 0;
170 | steering_can_timeout = ((in[0] & 0x08) >> 3) != 0;
171 | vehicle_can_timeout = ((in[0] & 0x04) >> 2) != 0;
172 | user_can_read_errors = ((in[6] << 8) | in[7]);
173 | }
174 |
175 | void VinRptMsg::parse(uint8_t *in)
176 | {
177 | std::ostringstream oss;
178 | oss << in[0] << in[1] << in[2];
179 | mfg_code = oss.str();
180 |
181 | if (mfg_code == "52C")
182 | mfg = "POLARIS INDUSTRIES INC.";
183 | else if (mfg_code == "3HS")
184 | mfg = "NAVISTAR, INC.";
185 | else if (mfg_code == "2T2")
186 | mfg = "TOYOTA MOTOR MANUFACTURING CANADA";
187 |
188 | model_year_code = in[3];
189 |
190 | if (model_year_code >= '1' && model_year_code <= '9')
191 | {
192 | model_year = 2000 + model_year_code;
193 | }
194 | else if (model_year_code >= 'A' && model_year_code < 'Z')
195 | {
196 | switch (model_year_code)
197 | {
198 | case 'A':
199 | model_year = 2010;
200 | break;
201 | case 'B':
202 | model_year = 2011;
203 | break;
204 | case 'C':
205 | model_year = 2012;
206 | break;
207 | case 'D':
208 | model_year = 2013;
209 | break;
210 | case 'E':
211 | model_year = 2014;
212 | break;
213 | case 'F':
214 | model_year = 2015;
215 | break;
216 | case 'G':
217 | model_year = 2016;
218 | break;
219 | case 'H':
220 | model_year = 2017;
221 | break;
222 | case 'J':
223 | model_year = 2018;
224 | break;
225 | case 'K':
226 | model_year = 2019;
227 | break;
228 | case 'L':
229 | model_year = 2020;
230 | break;
231 | case 'M':
232 | model_year = 2021;
233 | break;
234 | case 'N':
235 | model_year = 2022;
236 | break;
237 | case 'P':
238 | model_year = 2023;
239 | break;
240 | case 'R':
241 | model_year = 2024;
242 | break;
243 | case 'S':
244 | model_year = 2025;
245 | break;
246 | case 'T':
247 | model_year = 2026;
248 | break;
249 | case 'V':
250 | model_year = 2027;
251 | break;
252 | case 'W':
253 | model_year = 2028;
254 | break;
255 | case 'X':
256 | model_year = 2029;
257 | break;
258 | case 'Y':
259 | model_year = 2030;
260 | break;
261 | }
262 | }
263 |
264 | serial = (in[4] & 0x0F);
265 | serial = (serial << 8) | in[5];
266 | serial = (serial << 8) | in[6];
267 | }
268 |
269 | void SystemRptIntMsg::parse(uint8_t *in)
270 | {
271 | manual_input = in[0];
272 | command = in[1];
273 | output = in[2];
274 | }
275 |
276 | void SystemRptFloatMsg::parse(uint8_t *in)
277 | {
278 | int16_t temp;
279 |
280 | temp = (static_cast(in[0]) << 8) | in[1];
281 | manual_input = static_cast(temp / 1000.0);
282 |
283 | temp = (static_cast(in[2]) << 8) | in[3];
284 | command = static_cast(temp / 1000.0);
285 |
286 | temp = (static_cast(in[4]) << 8) | in[5];
287 | output = static_cast(temp / 1000.0);
288 | }
289 |
290 | void VehicleSpeedRptMsg::parse(uint8_t *in)
291 | {
292 | int16_t temp;
293 |
294 | temp = (static_cast(in[0]) << 8) | in[1];
295 | vehicle_speed = static_cast(temp / 100.0);
296 |
297 | vehicle_speed_valid = (in[2] == 1);
298 | vehicle_speed_raw[0] = in[3];
299 | vehicle_speed_raw[1] = in[4];
300 | }
301 |
302 | void YawRateRptMsg::parse(uint8_t *in)
303 | {
304 | int16_t temp;
305 |
306 | temp = (static_cast(in[0]) << 8) | in[1];
307 | yaw_rate = static_cast(temp / 100.0);
308 | }
309 |
310 | void LatLonHeadingRptMsg::parse(uint8_t *in)
311 | {
312 | latitude_degrees = static_cast(in[0]);
313 | latitude_minutes = in[1];
314 | latitude_seconds = in[2];
315 | longitude_degrees = static_cast(in[3]);
316 | longitude_minutes = in[4];
317 | longitude_seconds = in[5];
318 | heading = ((static_cast(in[6]) << 8) | in[7]) / 100.0;
319 | }
320 |
321 | void DateTimeRptMsg::parse(uint8_t *in)
322 | {
323 | year = in[0];
324 | month = in[1];
325 | day = in[2];
326 | hour = in[3];
327 | minute = in[4];
328 | second = in[5];
329 | }
330 |
331 | void WheelSpeedRptMsg::parse(uint8_t *in)
332 | {
333 | int16_t temp;
334 |
335 | temp = (static_cast(in[0]) << 8) | in[1];
336 | front_left_wheel_speed = static_cast(temp / 100.0);
337 |
338 | temp = (static_cast(in[2]) << 8) | in[3];
339 | front_right_wheel_speed = static_cast(temp / 100.0);
340 |
341 | temp = (static_cast(in[4]) << 8) | in[5];
342 | rear_left_wheel_speed = static_cast(temp / 100.0);
343 |
344 | temp = (static_cast(in[6]) << 8) | in[7];
345 | rear_right_wheel_speed = static_cast(temp / 100.0);
346 | }
347 |
348 | void MotorRpt1Msg::parse(uint8_t *in)
349 | {
350 | int32_t temp;
351 |
352 | temp = (static_cast(in[0]) << 24) |
353 | (static_cast(in[1]) << 16) |
354 | (static_cast(in[2]) << 8) | in[3];
355 | current = static_cast(temp / 1000.0);
356 |
357 | temp = (static_cast(in[4]) << 24) |
358 | (static_cast(in[5]) << 16) |
359 | (static_cast(in[6]) << 8) | in[7];
360 | position = static_cast(temp / 1000.0);
361 | }
362 |
363 | void MotorRpt2Msg::parse(uint8_t *in)
364 | {
365 | int16_t temp16;
366 | int32_t temp32;
367 |
368 | temp16 = (static_cast(in[0]) << 8) | in[1];
369 | encoder_temp = static_cast(temp16);
370 |
371 | temp16 = (static_cast(in[2]) << 8) | in[3];
372 | motor_temp = static_cast(temp16);
373 |
374 | temp32 = (static_cast(in[7]) << 24) |
375 | (static_cast(in[6]) << 16) |
376 | (static_cast(in[5]) << 8) | in[4];
377 | velocity = static_cast(temp32 / 1000.0);
378 | }
379 |
380 | void MotorRpt3Msg::parse(uint8_t *in)
381 | {
382 | int32_t temp;
383 |
384 | temp = (static_cast(in[0]) << 24) |
385 | (static_cast(in[1]) << 16) |
386 | (static_cast(in[2]) << 8) | in[3];
387 | torque_output = static_cast(temp / 1000.0);
388 |
389 | temp = (static_cast(in[4]) << 24) |
390 | (static_cast(in[5]) << 16) |
391 | (static_cast(in[6]) << 8) | in[7];
392 | torque_input = static_cast(temp / 1000.0);
393 | }
394 |
395 | void SteeringPIDRpt1Msg::parse(uint8_t *in)
396 | {
397 | int16_t temp;
398 |
399 | temp = (static_cast(in[0]) << 8) | in[1];
400 | dt = static_cast(temp / 1000.0);
401 |
402 | temp = (static_cast(in[2]) << 8) | in[3];
403 | kp = static_cast(temp / 1000.0);
404 |
405 | temp = (static_cast(in[4]) << 8) | in[5];
406 | ki = static_cast(temp / 1000.0);
407 |
408 | temp = (static_cast(in[6]) << 8) | in[7];
409 | kd = static_cast(temp / 1000.0);
410 | }
411 |
412 | void SteeringPIDRpt2Msg::parse(uint8_t *in)
413 | {
414 | int16_t temp;
415 |
416 | temp = (static_cast(in[0]) << 8) | in[1];
417 | p_term = static_cast(temp / 1000.0);
418 |
419 | temp = (static_cast(in[2]) << 8) | in[3];
420 | i_term = static_cast(temp / 1000.0);
421 |
422 | temp = (static_cast(in[4]) << 8) | in[5];
423 | d_term = static_cast(temp / 1000.0);
424 |
425 | temp = (static_cast(in[6]) << 8) | in[7];
426 | all_terms = static_cast(temp / 1000.0);
427 | }
428 |
429 | void SteeringPIDRpt3Msg::parse(uint8_t *in)
430 | {
431 | int16_t temp;
432 |
433 | temp = (static_cast(in[0]) << 8) | in[1];
434 | new_torque = static_cast(temp / 1000.0);
435 |
436 | temp = (static_cast(in[2]) << 8) | in[3];
437 | str_angle_desired = static_cast(temp / 1000.0);
438 |
439 | temp = (static_cast(in[4]) << 8) | in[5];
440 | str_angle_actual = static_cast(temp / 1000.0);
441 |
442 | temp = (static_cast(in[6]) << 8) | in[7];
443 | error = static_cast(temp / 1000.0);
444 | }
445 |
446 | void SteeringPIDRpt4Msg::parse(uint8_t *in)
447 | {
448 | int16_t temp;
449 |
450 | temp = (static_cast(in[0]) << 8) | in[1];
451 | angular_velocity = static_cast(temp / 1000.0);
452 |
453 | temp = (static_cast(in[2]) << 8) | in[3];
454 | angular_acceleration = static_cast(temp / 1000.0);
455 | }
456 |
457 | void ParkingBrakeStatusRptMsg::parse(uint8_t *in)
458 | {
459 | parking_brake_engaged = (in[0] == 0x01);
460 | }
461 |
462 | // RX Messages
463 | void GlobalCmdMsg::encode(bool enable, bool clear_override, bool ignore_overide)
464 | {
465 | data.assign(8, 0);
466 |
467 | if (enable)
468 | data[0] |= 0x01;
469 | if (clear_override)
470 | data[0] |= 0x02;
471 | if (ignore_overide)
472 | data[0] |= 0x04;
473 | }
474 |
475 | void TurnSignalCmdMsg::encode(uint8_t turn_signal_cmd)
476 | {
477 | data.assign(8, 0);
478 | data[0] = turn_signal_cmd;
479 | }
480 |
481 | void HeadlightCmdMsg::encode(uint8_t headlight_cmd)
482 | {
483 | data.assign(8, 0);
484 | data[0] = headlight_cmd;
485 | }
486 |
487 | void HornCmdMsg::encode(uint8_t horn_cmd)
488 | {
489 | data.assign(8, 0);
490 | data[0] = horn_cmd;
491 | }
492 |
493 | void WiperCmdMsg::encode(uint8_t wiper_cmd)
494 | {
495 | data.assign(8, 0);
496 | data[0] = wiper_cmd;
497 | }
498 |
499 | void ShiftCmdMsg::encode(uint8_t shift_cmd)
500 | {
501 | data.assign(8, 0);
502 | data[0] = shift_cmd;
503 | }
504 |
505 | void AccelCmdMsg::encode(double accel_cmd)
506 | {
507 | data.assign(8, 0);
508 | uint16_t cmdInt = static_cast(accel_cmd * 1000.0);
509 | data[0] = (cmdInt & 0xFF00) >> 8;
510 | data[1] = cmdInt & 0x00FF;
511 | }
512 |
513 | void SteerCmdMsg::encode(double steer_pos, double steer_spd)
514 | {
515 | data.assign(8, 0);
516 | int32_t raw_pos = static_cast(1000.0 * steer_pos);
517 | uint32_t raw_spd = (uint32_t)(1000.0 * steer_spd);
518 |
519 | data[0] = (raw_pos & 0xFF000000) >> 24;
520 | data[1] = (raw_pos & 0x00FF0000) >> 16;
521 | data[2] = (raw_pos & 0x0000FF00) >> 8;
522 | data[3] = raw_pos & 0x000000FF;
523 | data[4] = (raw_spd & 0xFF000000) >> 24;
524 | data[5] = (raw_spd & 0x00FF0000) >> 16;
525 | data[6] = (raw_spd & 0x0000FF00) >> 8;
526 | data[7] = raw_spd & 0x000000FF;
527 | }
528 |
529 | void BrakeCmdMsg::encode(double brake_pct)
530 | {
531 | data.assign(8, 0);
532 | uint16_t raw_pct = static_cast(1000.0 * brake_pct);
533 |
534 | data[0] = (raw_pct & 0xFF00) >> 8;
535 | data[1] = (raw_pct & 0x00FF);
536 | }
537 |
538 | } // namespace PACMod
539 | } // namespace Drivers
540 | } // namespace AS
541 |
--------------------------------------------------------------------------------
/src/pacmod_ros_msg_handler.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019 AutonomouStuff, LLC
2 | //
3 | // Permission is hereby granted, free of charge, to any person obtaining a copy
4 | // of this software and associated documentation files (the "Software"), to deal
5 | // in the Software without restriction, including without limitation the rights
6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | // copies of the Software, and to permit persons to whom the Software is
8 | // furnished to do so, subject to the following conditions:
9 | //
10 | // The above copyright notice and this permission notice shall be included in
11 | // all copies or substantial portions of the Software.
12 | //
13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 | // THE SOFTWARE.
20 |
21 | #include
22 |
23 | #include
24 | #include
25 | #include
26 |
27 | namespace AS
28 | {
29 | namespace Drivers
30 | {
31 | namespace PACMod
32 | {
33 |
34 | LockedData::LockedData() :
35 | _data(),
36 | _data_mut(),
37 | _is_valid(false),
38 | _valid_mut()
39 | {
40 | }
41 |
42 | bool LockedData::isValid() const
43 | {
44 | std::lock_guard lck(_valid_mut);
45 | return _is_valid;
46 | }
47 |
48 | void LockedData::setIsValid(bool valid)
49 | {
50 | std::lock_guard lck(_valid_mut);
51 | _is_valid = valid;
52 | }
53 |
54 | std::vector LockedData::getData() const
55 | {
56 | std::lock_guard lck(_data_mut);
57 | return _data;
58 | }
59 |
60 | void LockedData::setData(std::vector new_data)
61 | {
62 | std::lock_guard lck(_data_mut);
63 | _data = new_data;
64 | }
65 |
66 | void PacmodTxRosMsgHandler::fillAndPublish(const int64_t& can_id,
67 | std::string frame_id,
68 | const ros::Publisher& pub,
69 | const std::shared_ptr& parser_class)
70 | {
71 | if (can_id == TurnSignalRptMsg::CAN_ID ||
72 | can_id == ShiftRptMsg::CAN_ID ||
73 | can_id == HeadlightRptMsg::CAN_ID ||
74 | can_id == HornRptMsg::CAN_ID ||
75 | can_id == WiperRptMsg::CAN_ID)
76 | {
77 | pacmod_msgs::SystemRptInt new_msg;
78 | fillSystemRptInt(parser_class, &new_msg, frame_id);
79 | pub.publish(new_msg);
80 | }
81 | else if (can_id == AccelRptMsg::CAN_ID ||
82 | can_id == BrakeRptMsg::CAN_ID ||
83 | can_id == SteerRptMsg::CAN_ID ||
84 | can_id == SteerRpt2Msg::CAN_ID ||
85 | can_id == SteerRpt3Msg::CAN_ID)
86 | {
87 | pacmod_msgs::SystemRptFloat new_msg;
88 | fillSystemRptFloat(parser_class, &new_msg, frame_id);
89 | pub.publish(new_msg);
90 | }
91 | else if (can_id == GlobalRptMsg::CAN_ID)
92 | {
93 | pacmod_msgs::GlobalRpt new_msg;
94 | fillGlobalRpt(parser_class, &new_msg, frame_id);
95 | pub.publish(new_msg);
96 | }
97 | else if (can_id == VehicleSpeedRptMsg::CAN_ID)
98 | {
99 | pacmod_msgs::VehicleSpeedRpt new_msg;
100 | fillVehicleSpeedRpt(parser_class, &new_msg, frame_id);
101 | pub.publish(new_msg);
102 | }
103 | else if (can_id == BrakeMotorRpt1Msg::CAN_ID ||
104 | can_id == SteerMotorRpt1Msg::CAN_ID)
105 | {
106 | pacmod_msgs::MotorRpt1 new_msg;
107 | fillMotorRpt1(parser_class, &new_msg, frame_id);
108 | pub.publish(new_msg);
109 | }
110 | else if (can_id == BrakeMotorRpt2Msg::CAN_ID ||
111 | can_id == SteerMotorRpt2Msg::CAN_ID)
112 | {
113 | pacmod_msgs::MotorRpt2 new_msg;
114 | fillMotorRpt2(parser_class, &new_msg, frame_id);
115 | pub.publish(new_msg);
116 | }
117 | else if (can_id == BrakeMotorRpt3Msg::CAN_ID ||
118 | can_id == SteerMotorRpt3Msg::CAN_ID)
119 | {
120 | pacmod_msgs::MotorRpt3 new_msg;
121 | fillMotorRpt3(parser_class, &new_msg, frame_id);
122 | pub.publish(new_msg);
123 | }
124 | else if (can_id == WheelSpeedRptMsg::CAN_ID)
125 | {
126 | pacmod_msgs::WheelSpeedRpt new_msg;
127 | fillWheelSpeedRpt(parser_class, &new_msg, frame_id);
128 | pub.publish(new_msg);
129 | }
130 | else if (can_id == SteeringPIDRpt1Msg::CAN_ID)
131 | {
132 | pacmod_msgs::SteeringPIDRpt1 new_msg;
133 | fillSteeringPIDRpt1(parser_class, &new_msg, frame_id);
134 | pub.publish(new_msg);
135 | }
136 | else if (can_id == SteeringPIDRpt2Msg::CAN_ID)
137 | {
138 | pacmod_msgs::SteeringPIDRpt2 new_msg;
139 | fillSteeringPIDRpt2(parser_class, &new_msg, frame_id);
140 | pub.publish(new_msg);
141 | }
142 | else if (can_id == SteeringPIDRpt3Msg::CAN_ID)
143 | {
144 | pacmod_msgs::SteeringPIDRpt3 new_msg;
145 | fillSteeringPIDRpt3(parser_class, &new_msg, frame_id);
146 | pub.publish(new_msg);
147 | }
148 | else if (can_id == ParkingBrakeStatusRptMsg::CAN_ID)
149 | {
150 | pacmod_msgs::ParkingBrakeStatusRpt new_msg;
151 | fillParkingBrakeStatusRpt(parser_class, &new_msg, frame_id);
152 | pub.publish(new_msg);
153 | }
154 | else if (can_id == YawRateRptMsg::CAN_ID)
155 | {
156 | pacmod_msgs::YawRateRpt new_msg;
157 | fillYawRateRpt(parser_class, &new_msg, frame_id);
158 | pub.publish(new_msg);
159 | }
160 | else if (can_id == LatLonHeadingRptMsg::CAN_ID)
161 | {
162 | pacmod_msgs::LatLonHeadingRpt new_msg;
163 | fillLatLonHeadingRpt(parser_class, &new_msg, frame_id);
164 | pub.publish(new_msg);
165 | }
166 | else if (can_id == DateTimeRptMsg::CAN_ID)
167 | {
168 | pacmod_msgs::DateTimeRpt new_msg;
169 | fillDateTimeRpt(parser_class, &new_msg, frame_id);
170 | pub.publish(new_msg);
171 | }
172 | else if (can_id == SteeringPIDRpt4Msg::CAN_ID)
173 | {
174 | pacmod_msgs::SteeringPIDRpt4 new_msg;
175 | fillSteeringPIDRpt4(parser_class, &new_msg, frame_id);
176 | pub.publish(new_msg);
177 | }
178 | else if (can_id == VinRptMsg::CAN_ID)
179 | {
180 | pacmod_msgs::VinRpt new_msg;
181 | fillVinRpt(parser_class, &new_msg, frame_id);
182 | pub.publish(new_msg);
183 | }
184 | }
185 |
186 | void PacmodTxRosMsgHandler::fillSystemRptInt(const std::shared_ptr& parser_class,
187 | pacmod_msgs::SystemRptInt* new_msg,
188 | std::string frame_id)
189 | {
190 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
191 |
192 | new_msg->manual_input = dc_parser->manual_input;
193 | new_msg->command = dc_parser->command;
194 | new_msg->output = dc_parser->output;
195 |
196 | new_msg->header.frame_id = frame_id;
197 | new_msg->header.stamp = ros::Time::now();
198 | }
199 |
200 | void PacmodTxRosMsgHandler::fillSystemRptFloat(const std::shared_ptr& parser_class,
201 | pacmod_msgs::SystemRptFloat* new_msg,
202 | std::string frame_id)
203 | {
204 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
205 |
206 | new_msg->manual_input = dc_parser->manual_input;
207 | new_msg->command = dc_parser->command;
208 | new_msg->output = dc_parser->output;
209 |
210 | new_msg->header.frame_id = frame_id;
211 | new_msg->header.stamp = ros::Time::now();
212 | }
213 |
214 | void PacmodTxRosMsgHandler::fillGlobalRpt(const std::shared_ptr& parser_class,
215 | pacmod_msgs::GlobalRpt* new_msg,
216 | std::string frame_id)
217 | {
218 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
219 |
220 | new_msg->enabled = dc_parser->enabled;
221 | new_msg->override_active = dc_parser->override_active;
222 | new_msg->user_can_timeout = dc_parser->user_can_timeout;
223 | new_msg->brake_can_timeout = dc_parser->brake_can_timeout;
224 | new_msg->steering_can_timeout = dc_parser->steering_can_timeout;
225 | new_msg->vehicle_can_timeout = dc_parser->vehicle_can_timeout;
226 | new_msg->user_can_read_errors = dc_parser->user_can_read_errors;
227 |
228 | new_msg->header.frame_id = frame_id;
229 | new_msg->header.stamp = ros::Time::now();
230 | }
231 |
232 | void PacmodTxRosMsgHandler::fillVehicleSpeedRpt(const std::shared_ptr& parser_class,
233 | pacmod_msgs::VehicleSpeedRpt* new_msg,
234 | std::string frame_id)
235 | {
236 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
237 |
238 | new_msg->vehicle_speed = dc_parser->vehicle_speed;
239 | new_msg->vehicle_speed_valid = dc_parser->vehicle_speed_valid;
240 | new_msg->vehicle_speed_raw[0] = dc_parser->vehicle_speed_raw[0];
241 | new_msg->vehicle_speed_raw[1] = dc_parser->vehicle_speed_raw[1];
242 |
243 | new_msg->header.frame_id = frame_id;
244 | new_msg->header.stamp = ros::Time::now();
245 | }
246 |
247 | void PacmodTxRosMsgHandler::fillMotorRpt1(const std::shared_ptr& parser_class,
248 | pacmod_msgs::MotorRpt1* new_msg,
249 | std::string frame_id)
250 | {
251 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
252 |
253 | new_msg->current = dc_parser->current;
254 | new_msg->position = dc_parser->position;
255 |
256 | new_msg->header.frame_id = frame_id;
257 | new_msg->header.stamp = ros::Time::now();
258 | }
259 |
260 | void PacmodTxRosMsgHandler::fillMotorRpt2(const std::shared_ptr& parser_class,
261 | pacmod_msgs::MotorRpt2* new_msg,
262 | std::string frame_id)
263 | {
264 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
265 |
266 | new_msg->encoder_temp = dc_parser->encoder_temp;
267 | new_msg->motor_temp = dc_parser->motor_temp;
268 | new_msg->angular_velocity = dc_parser->velocity;
269 |
270 | new_msg->header.frame_id = frame_id;
271 | new_msg->header.stamp = ros::Time::now();
272 | }
273 |
274 | void PacmodTxRosMsgHandler::fillMotorRpt3(const std::shared_ptr& parser_class,
275 | pacmod_msgs::MotorRpt3* new_msg,
276 | std::string frame_id)
277 | {
278 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
279 |
280 | new_msg->torque_output = dc_parser->torque_output;
281 | new_msg->torque_input = dc_parser->torque_input;
282 |
283 | new_msg->header.frame_id = frame_id;
284 | new_msg->header.stamp = ros::Time::now();
285 | }
286 |
287 | void PacmodTxRosMsgHandler::fillWheelSpeedRpt(const std::shared_ptr& parser_class,
288 | pacmod_msgs::WheelSpeedRpt* new_msg,
289 | std::string frame_id)
290 | {
291 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
292 |
293 | new_msg->front_left_wheel_speed = dc_parser->front_left_wheel_speed;
294 | new_msg->front_right_wheel_speed = dc_parser->front_right_wheel_speed;
295 | new_msg->rear_left_wheel_speed = dc_parser->rear_left_wheel_speed;
296 | new_msg->rear_right_wheel_speed = dc_parser->rear_right_wheel_speed;
297 |
298 | new_msg->header.frame_id = frame_id;
299 | new_msg->header.stamp = ros::Time::now();
300 | }
301 |
302 | void PacmodTxRosMsgHandler::fillSteeringPIDRpt1(const std::shared_ptr& parser_class,
303 | pacmod_msgs::SteeringPIDRpt1* new_msg,
304 | std::string frame_id)
305 | {
306 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
307 |
308 | new_msg->dt = dc_parser->dt;
309 | new_msg->kp = dc_parser->kp;
310 | new_msg->ki = dc_parser->ki;
311 | new_msg->kd = dc_parser->kd;
312 |
313 | new_msg->header.frame_id = frame_id;
314 | new_msg->header.stamp = ros::Time::now();
315 | }
316 |
317 | void PacmodTxRosMsgHandler::fillSteeringPIDRpt2(const std::shared_ptr& parser_class,
318 | pacmod_msgs::SteeringPIDRpt2* new_msg,
319 | std::string frame_id)
320 | {
321 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
322 |
323 | new_msg->p_term = dc_parser->p_term;
324 | new_msg->i_term = dc_parser->i_term;
325 | new_msg->d_term = dc_parser->d_term;
326 | new_msg->all_terms = dc_parser->all_terms;
327 |
328 | new_msg->header.frame_id = frame_id;
329 | new_msg->header.stamp = ros::Time::now();
330 | }
331 |
332 | void PacmodTxRosMsgHandler::fillSteeringPIDRpt3(const std::shared_ptr& parser_class,
333 | pacmod_msgs::SteeringPIDRpt3* new_msg,
334 | std::string frame_id)
335 | {
336 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
337 |
338 | new_msg->new_torque = dc_parser->new_torque;
339 | new_msg->str_angle_desired = dc_parser->str_angle_desired;
340 | new_msg->str_angle_actual = dc_parser->str_angle_actual;
341 | new_msg->error = dc_parser->error;
342 |
343 | new_msg->header.frame_id = frame_id;
344 | new_msg->header.stamp = ros::Time::now();
345 | }
346 |
347 | void PacmodTxRosMsgHandler::fillParkingBrakeStatusRpt(const std::shared_ptr& parser_class,
348 | pacmod_msgs::ParkingBrakeStatusRpt* new_msg,
349 | std::string frame_id)
350 | {
351 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
352 |
353 | new_msg->parking_brake_engaged = dc_parser->parking_brake_engaged;
354 |
355 | new_msg->header.frame_id = frame_id;
356 | new_msg->header.stamp = ros::Time::now();
357 | }
358 |
359 | void PacmodTxRosMsgHandler::fillYawRateRpt(const std::shared_ptr& parser_class,
360 | pacmod_msgs::YawRateRpt* new_msg,
361 | std::string frame_id)
362 | {
363 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
364 |
365 | new_msg->yaw_rate = dc_parser->yaw_rate;
366 |
367 | new_msg->header.frame_id = frame_id;
368 | new_msg->header.stamp = ros::Time::now();
369 | }
370 |
371 | void PacmodTxRosMsgHandler::fillLatLonHeadingRpt(const std::shared_ptr& parser_class,
372 | pacmod_msgs::LatLonHeadingRpt* new_msg,
373 | std::string frame_id)
374 | {
375 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
376 |
377 | new_msg->latitude_degrees = dc_parser->latitude_degrees;
378 | new_msg->latitude_minutes = dc_parser->latitude_minutes;
379 | new_msg->latitude_seconds = dc_parser->latitude_seconds;
380 | new_msg->longitude_degrees = dc_parser->longitude_degrees;
381 | new_msg->longitude_minutes = dc_parser->longitude_minutes;
382 | new_msg->longitude_seconds = dc_parser->longitude_seconds;
383 | new_msg->heading = dc_parser->heading;
384 |
385 | new_msg->header.frame_id = frame_id;
386 | new_msg->header.stamp = ros::Time::now();
387 | }
388 |
389 | void PacmodTxRosMsgHandler::fillDateTimeRpt(const std::shared_ptr& parser_class,
390 | pacmod_msgs::DateTimeRpt* new_msg,
391 | std::string frame_id)
392 | {
393 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
394 |
395 | new_msg->year = dc_parser->year;
396 | new_msg->month = dc_parser->month;
397 | new_msg->day = dc_parser->day;
398 | new_msg->hour = dc_parser->hour;
399 | new_msg->minute = dc_parser->minute;
400 | new_msg->second = dc_parser->second;
401 |
402 | new_msg->header.frame_id = frame_id;
403 | new_msg->header.stamp = ros::Time::now();
404 | }
405 |
406 | void PacmodTxRosMsgHandler::fillSteeringPIDRpt4(const std::shared_ptr& parser_class,
407 | pacmod_msgs::SteeringPIDRpt4* new_msg,
408 | std::string frame_id)
409 | {
410 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
411 |
412 | new_msg->angular_velocity = dc_parser->angular_velocity;
413 | new_msg->angular_acceleration = dc_parser->angular_acceleration;
414 |
415 | new_msg->header.frame_id = frame_id;
416 | new_msg->header.stamp = ros::Time::now();
417 | }
418 |
419 | void PacmodTxRosMsgHandler::fillVinRpt(const std::shared_ptr& parser_class,
420 | pacmod_msgs::VinRpt* new_msg,
421 | std::string frame_id)
422 | {
423 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
424 |
425 | new_msg->mfg_code = dc_parser->mfg_code;
426 | new_msg->mfg = dc_parser->mfg;
427 | new_msg->model_year_code = dc_parser->model_year_code;
428 | new_msg->model_year = dc_parser->model_year;
429 | new_msg->serial = dc_parser->serial;
430 |
431 | new_msg->header.frame_id = frame_id;
432 | new_msg->header.stamp = ros::Time::now();
433 | }
434 |
435 | std::vector PacmodRxRosMsgHandler::unpackAndEncode(const int64_t& can_id,
436 | const pacmod_msgs::PacmodCmd::ConstPtr& msg)
437 | {
438 | if (can_id == TurnSignalCmdMsg::CAN_ID)
439 | {
440 | TurnSignalCmdMsg encoder;
441 | encoder.encode(msg->ui16_cmd);
442 | return encoder.data;
443 | }
444 | else if (can_id == ShiftCmdMsg::CAN_ID)
445 | {
446 | ShiftCmdMsg encoder;
447 | encoder.encode(msg->ui16_cmd);
448 | return encoder.data;
449 | }
450 | else if (can_id == AccelCmdMsg::CAN_ID)
451 | {
452 | AccelCmdMsg encoder;
453 | encoder.encode(msg->f64_cmd);
454 | return encoder.data;
455 | }
456 | else if (can_id == GlobalCmdMsg::CAN_ID)
457 | {
458 | GlobalCmdMsg encoder;
459 | encoder.encode(msg->enable, msg->clear, msg->ignore);
460 | return encoder.data;
461 | }
462 | else if (can_id == BrakeCmdMsg::CAN_ID)
463 | {
464 | BrakeCmdMsg encoder;
465 | encoder.encode(msg->f64_cmd);
466 | return encoder.data;
467 | }
468 | else if (can_id == HeadlightCmdMsg::CAN_ID)
469 | {
470 | HeadlightCmdMsg encoder;
471 | encoder.encode(msg->ui16_cmd);
472 | return encoder.data;
473 | }
474 | else if (can_id == HornCmdMsg::CAN_ID)
475 | {
476 | HornCmdMsg encoder;
477 | encoder.encode(msg->ui16_cmd);
478 | return encoder.data;
479 | }
480 | else if (can_id == WiperCmdMsg::CAN_ID)
481 | {
482 | WiperCmdMsg encoder;
483 | encoder.encode(msg->ui16_cmd);
484 | return encoder.data;
485 | }
486 | }
487 |
488 | std::vector PacmodRxRosMsgHandler::unpackAndEncode(const int64_t& can_id,
489 | const pacmod_msgs::PositionWithSpeed::ConstPtr& msg)
490 | {
491 | std::vector ret_vec;
492 |
493 | if (can_id == SteerCmdMsg::CAN_ID)
494 | {
495 | SteerCmdMsg encoder;
496 | encoder.encode(msg->angular_position, msg->angular_velocity_limit);
497 | return encoder.data;
498 | }
499 | }
500 |
501 | } // namespace PACMod
502 | } // namespace Drivers
503 | } // namespace AS
504 |
--------------------------------------------------------------------------------
/src/pacmod_node.cpp:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2019 AutonomouStuff, LLC
2 | //
3 | // Permission is hereby granted, free of charge, to any person obtaining a copy
4 | // of this software and associated documentation files (the "Software"), to deal
5 | // in the Software without restriction, including without limitation the rights
6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 | // copies of the Software, and to permit persons to whom the Software is
8 | // furnished to do so, subject to the following conditions:
9 | //
10 | // The above copyright notice and this permission notice shall be included in
11 | // all copies or substantial portions of the Software.
12 | //
13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 | // THE SOFTWARE.
20 |
21 | #include
22 | #include
23 | #include
24 |
25 | #include
26 | #include
27 | #include
28 | #include
29 | #include
30 | #include
31 | #include
32 | #include
33 |
34 | #include
35 | #include
36 | #include
37 | #include
38 |
39 | using namespace AS::Drivers::PACMod; // NOLINT
40 |
41 | double last_global_rpt_msg_received = 0.0;
42 | const double watchdog_timeout = 0.3;
43 | std::string veh_type_string = "POLARIS_GEM"; // NOLINT
44 | VehicleType veh_type = VehicleType::POLARIS_GEM;
45 | std::unordered_map pub_tx_list;
46 | PacmodTxRosMsgHandler handler;
47 |
48 | // Vehicle-Specific Publishers
49 | ros::Publisher wiper_rpt_pub;
50 | ros::Publisher headlight_rpt_pub;
51 | ros::Publisher horn_rpt_pub;
52 | ros::Publisher steer_rpt_2_pub;
53 | ros::Publisher steer_rpt_3_pub;
54 | ros::Publisher wheel_speed_rpt_pub;
55 | ros::Publisher steering_pid_rpt_1_pub;
56 | ros::Publisher steering_pid_rpt_2_pub;
57 | ros::Publisher steering_pid_rpt_3_pub;
58 | ros::Publisher steering_pid_rpt_4_pub;
59 | ros::Publisher lat_lon_heading_rpt_pub;
60 | ros::Publisher date_time_rpt_pub;
61 | ros::Publisher parking_brake_status_rpt_pub;
62 | ros::Publisher yaw_rate_rpt_pub;
63 | ros::Publisher steering_rpt_detail_1_pub;
64 | ros::Publisher steering_rpt_detail_2_pub;
65 | ros::Publisher steering_rpt_detail_3_pub;
66 | ros::Publisher brake_rpt_detail_1_pub;
67 | ros::Publisher brake_rpt_detail_2_pub;
68 | ros::Publisher brake_rpt_detail_3_pub;
69 |
70 | // Vehicle-Specific Subscribers
71 | std::shared_ptr wiper_set_cmd_sub,
72 | headlight_set_cmd_sub,
73 | horn_set_cmd_sub;
74 |
75 | // Advertise published messages
76 | ros::Publisher global_rpt_pub;
77 | ros::Publisher vin_rpt_pub;
78 | ros::Publisher turn_rpt_pub;
79 | ros::Publisher shift_rpt_pub;
80 | ros::Publisher accel_rpt_pub;
81 | ros::Publisher steer_rpt_pub;
82 | ros::Publisher brake_rpt_pub;
83 | ros::Publisher vehicle_speed_pub;
84 | ros::Publisher vehicle_speed_ms_pub;
85 | ros::Publisher enable_pub;
86 | ros::Publisher can_rx_pub;
87 |
88 | std::unordered_map> rx_list;
89 | std::unordered_map rpt_cmd_list;
90 |
91 | bool enable_state = false;
92 | std::mutex enable_mut;
93 | bool override_state = false;
94 | std::mutex override_mut;
95 | bool global_keep_going = true;
96 | std::mutex keep_going_mut;
97 |
98 | /*
99 | pacmod_msgs::PacmodCmd global_cmd_msg;
100 | pacmod_msgs::PacmodCmd::ConstPtr global_cmd_msg_cpr(&global_cmd_msg);
101 | */
102 | std::chrono::milliseconds can_error_pause = std::chrono::milliseconds(1000);
103 |
104 | // Sets the PACMod enable flag through CAN.
105 | void set_enable(bool val)
106 | {
107 | std::lock_guard lck(enable_mut);
108 | enable_state = val;
109 | }
110 |
111 | // Listens for incoming requests to enable the PACMod
112 | void callback_pacmod_enable(const std_msgs::Bool::ConstPtr& msg)
113 | {
114 | set_enable(msg->data);
115 | }
116 |
117 | // Listens for incoming requests to change the state of the turn signals
118 | void callback_turn_signal_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
119 | {
120 | int64_t can_id = TurnSignalCmdMsg::CAN_ID;
121 | auto rx_it = rx_list.find(can_id);
122 |
123 | if (rx_it != rx_list.end())
124 | {
125 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
126 | rx_it->second->setIsValid(true);
127 | }
128 | else
129 | {
130 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
131 | }
132 | }
133 |
134 | // Listens for incoming requests to change the state of the headlights
135 | void callback_headlight_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
136 | {
137 | int64_t can_id = HeadlightCmdMsg::CAN_ID;
138 | auto rx_it = rx_list.find(can_id);
139 |
140 | if (rx_it != rx_list.end())
141 | {
142 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
143 | rx_it->second->setIsValid(true);
144 | }
145 | else
146 | {
147 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
148 | }
149 | }
150 |
151 | // Listens for incoming requests to change the state of the horn
152 | void callback_horn_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
153 | {
154 | int64_t can_id = HornCmdMsg::CAN_ID;
155 | auto rx_it = rx_list.find(can_id);
156 |
157 | if (rx_it != rx_list.end())
158 | {
159 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
160 | rx_it->second->setIsValid(true);
161 | }
162 | else
163 | {
164 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
165 | }
166 | }
167 |
168 | // Listens for incoming requests to change the state of the windshield wipers
169 | void callback_wiper_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
170 | {
171 | int64_t can_id = WiperCmdMsg::CAN_ID;
172 | auto rx_it = rx_list.find(can_id);
173 |
174 | if (rx_it != rx_list.end())
175 | {
176 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
177 | rx_it->second->setIsValid(true);
178 | }
179 | else
180 | {
181 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
182 | }
183 | }
184 |
185 | // Listens for incoming requests to change the gear shifter state
186 | void callback_shift_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
187 | {
188 | int64_t can_id = ShiftCmdMsg::CAN_ID;
189 | auto rx_it = rx_list.find(can_id);
190 |
191 | if (rx_it != rx_list.end())
192 | {
193 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
194 | rx_it->second->setIsValid(true);
195 | }
196 | else
197 | {
198 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
199 | }
200 | }
201 |
202 | // Listens for incoming requests to change the position of the throttle pedal
203 | void callback_accelerator_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
204 | {
205 | int64_t can_id = AccelCmdMsg::CAN_ID;
206 | auto rx_it = rx_list.find(can_id);
207 |
208 | if (rx_it != rx_list.end())
209 | {
210 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
211 | rx_it->second->setIsValid(true);
212 | }
213 | else
214 | {
215 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
216 | }
217 | }
218 |
219 | // Listens for incoming requests to change the position of the steering wheel with a speed limit
220 | void callback_steering_set_cmd(const pacmod_msgs::PositionWithSpeed::ConstPtr& msg)
221 | {
222 | int64_t can_id = SteerCmdMsg::CAN_ID;
223 | auto rx_it = rx_list.find(can_id);
224 |
225 | if (rx_it != rx_list.end())
226 | {
227 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
228 | rx_it->second->setIsValid(true);
229 | }
230 | else
231 | {
232 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
233 | }
234 | }
235 |
236 | // Listens for incoming requests to change the position of the brake pedal
237 | void callback_brake_set_cmd(const pacmod_msgs::PacmodCmd::ConstPtr& msg)
238 | {
239 | int64_t can_id = BrakeCmdMsg::CAN_ID;
240 | auto rx_it = rx_list.find(can_id);
241 |
242 | if (rx_it != rx_list.end())
243 | {
244 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(can_id, msg));
245 | rx_it->second->setIsValid(true);
246 | }
247 | else
248 | {
249 | ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
250 | }
251 | }
252 |
253 | void send_can(int32_t id, const std::vector& vec)
254 | {
255 | can_msgs::Frame frame;
256 | frame.id = id;
257 | frame.is_rtr = false;
258 | frame.is_extended = false;
259 | frame.is_error = false;
260 | frame.dlc = 8;
261 | std::copy(vec.begin(), vec.end(), frame.data.begin());
262 |
263 | frame.header.stamp = ros::Time::now();
264 |
265 | can_rx_pub.publish(frame);
266 | }
267 |
268 | void can_write()
269 | {
270 | std::vector data;
271 |
272 | const std::chrono::milliseconds loop_pause = std::chrono::milliseconds(33);
273 | const std::chrono::milliseconds inter_msg_pause = std::chrono::milliseconds(1);
274 | bool keep_going = true;
275 |
276 | // Set local to global value before looping.
277 | keep_going_mut.lock();
278 | keep_going = global_keep_going;
279 | keep_going_mut.unlock();
280 |
281 | while (keep_going)
282 | {
283 | /*
284 | // Create Global Command
285 | enable_mut.lock();
286 | global_cmd_msg.enable = enable_state;
287 | enable_mut.unlock();
288 |
289 | global_cmd_msg.clear = true;
290 | global_cmd_msg.ignore = false;
291 |
292 | auto rx_it = rx_list.find(GlobalCmdMsg::CAN_ID);
293 | rx_it->second->setData(PacmodRxRosMsgHandler::unpackAndEncode(GlobalCmdMsg::CAN_ID, global_cmd_msg_cpr));
294 | rx_it->second->setIsValid(true);
295 | */
296 |
297 | // Temporarily write the Global message separately.
298 | GlobalCmdMsg global_obj;
299 | bool local_enable;
300 |
301 | enable_mut.lock();
302 | local_enable = enable_state;
303 | enable_mut.unlock();
304 |
305 | global_obj.encode(local_enable, true, false);
306 |
307 | // ret = can_writer.write(GlobalCmdMsg::CAN_ID, &global_obj.data[0], 8, true);
308 | send_can(GlobalCmdMsg::CAN_ID, global_obj.data);
309 |
310 | std::this_thread::sleep_for(inter_msg_pause);
311 |
312 | if (local_enable)
313 | {
314 | // Write all the data that we have received.
315 | for (const auto& element : rx_list)
316 | {
317 | // Make sure the data are valid.
318 | if (element.second->isValid())
319 | {
320 | send_can(element.first, element.second->getData());
321 | std::this_thread::sleep_for(inter_msg_pause);
322 | }
323 | }
324 | }
325 |
326 | std::chrono::system_clock::time_point next_time = std::chrono::system_clock::now();
327 | next_time += loop_pause;
328 | std::this_thread::sleep_until(next_time);
329 |
330 | // Set local to global immediately before next loop.
331 | keep_going_mut.lock();
332 | keep_going = global_keep_going;
333 | keep_going_mut.unlock();
334 | }
335 | }
336 |
337 | void can_read(const can_msgs::Frame::ConstPtr &msg)
338 | {
339 | std_msgs::Bool bool_pub_msg;
340 | auto parser_class = PacmodTxMsg::make_message(msg->id);
341 | auto pub = pub_tx_list.find(msg->id);
342 |
343 | // Only parse messages for which we have a parser and a publisher.
344 | if (parser_class != NULL && pub != pub_tx_list.end())
345 | {
346 | parser_class->parse(const_cast(&msg->data[0]));
347 | handler.fillAndPublish(msg->id, "pacmod", pub->second, parser_class);
348 |
349 | bool local_enable = false;
350 |
351 | enable_mut.lock();
352 | local_enable = enable_state;
353 | enable_mut.unlock();
354 |
355 | if (!local_enable)
356 | {
357 | // If we're disabled, set all of the system commands
358 | // to be the current report values. This ensures that
359 | // when we enable, we are in the same state as the vehicle.
360 |
361 | // Find the cmd value for this rpt.
362 | auto cmd = rpt_cmd_list.find(msg->id);
363 |
364 | if (cmd != rpt_cmd_list.end())
365 | {
366 | // Find the data we need to set.
367 | auto rx_it = rx_list.find(cmd->second);
368 |
369 | if (rx_it != rx_list.end())
370 | {
371 | if (msg->id == TurnSignalRptMsg::CAN_ID)
372 | {
373 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
374 | TurnSignalCmdMsg encoder;
375 |
376 | encoder.encode(dc_parser->output);
377 | rx_it->second->setData(encoder.data);
378 | }
379 | else if (msg->id == ShiftRptMsg::CAN_ID)
380 | {
381 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
382 | ShiftCmdMsg encoder;
383 |
384 | encoder.encode(dc_parser->output);
385 | rx_it->second->setData(encoder.data);
386 | }
387 | else if (msg->id == AccelRptMsg::CAN_ID)
388 | {
389 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
390 | AccelCmdMsg encoder;
391 |
392 | encoder.encode(dc_parser->output);
393 | rx_it->second->setData(encoder.data);
394 | }
395 | else if (msg->id == SteerRptMsg::CAN_ID)
396 | {
397 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
398 | SteerCmdMsg encoder;
399 |
400 | encoder.encode(dc_parser->output, 2.0);
401 | rx_it->second->setData(encoder.data);
402 | }
403 | else if (msg->id == BrakeRptMsg::CAN_ID)
404 | {
405 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
406 | BrakeCmdMsg encoder;
407 |
408 | encoder.encode(dc_parser->output);
409 | rx_it->second->setData(encoder.data);
410 | }
411 | else if (msg->id == WiperRptMsg::CAN_ID)
412 | {
413 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
414 | WiperCmdMsg encoder;
415 |
416 | encoder.encode(dc_parser->output);
417 | rx_it->second->setData(encoder.data);
418 | }
419 | else if (msg->id == HornRptMsg::CAN_ID)
420 | {
421 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
422 | HornCmdMsg encoder;
423 |
424 | encoder.encode(dc_parser->output);
425 | rx_it->second->setData(encoder.data);
426 | }
427 |
428 | rx_it->second->setIsValid(true);
429 | }
430 | }
431 | }
432 |
433 | if (msg->id == GlobalRptMsg::CAN_ID)
434 | {
435 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
436 |
437 | bool_pub_msg.data = (dc_parser->enabled);
438 | enable_pub.publish(bool_pub_msg);
439 |
440 | if (dc_parser->override_active)
441 | {
442 | set_enable(false);
443 | }
444 | }
445 | else if (msg->id == VehicleSpeedRptMsg::CAN_ID)
446 | {
447 | auto dc_parser = std::dynamic_pointer_cast(parser_class);
448 |
449 | // Now publish in m/s
450 | std_msgs::Float64 veh_spd_ms_msg;
451 | veh_spd_ms_msg.data = (dc_parser->vehicle_speed) * 0.44704;
452 | vehicle_speed_ms_pub.publish(veh_spd_ms_msg);
453 | }
454 | }
455 | }
456 |
457 | int main(int argc, char *argv[])
458 | {
459 | ros::init(argc, argv, "pacmod");
460 | ros::AsyncSpinner spinner(2);
461 | ros::NodeHandle n;
462 | ros::NodeHandle priv("~");
463 | ros::Rate loop_rate(1.0); // PACMod is sending at ~30Hz.
464 |
465 | // Wait for time to be valid
466 | while (ros::Time::now().nsec == 0) {}
467 |
468 | // Get and validate parameters
469 | if (priv.getParam("vehicle_type", veh_type_string))
470 | {
471 | ROS_INFO("PACMod - Got vehicle type of: %s", veh_type_string.c_str());
472 |
473 | if (veh_type_string == "POLARIS_GEM")
474 | {
475 | veh_type = VehicleType::POLARIS_GEM;
476 | }
477 | else if (veh_type_string == "POLARIS_RANGER")
478 | {
479 | veh_type = VehicleType::POLARIS_RANGER;
480 | }
481 | else if (veh_type_string == "INTERNATIONAL_PROSTAR_122")
482 | {
483 | veh_type = VehicleType::INTERNATIONAL_PROSTAR_122;
484 | }
485 | else if (veh_type_string == "LEXUS_RX_450H")
486 | {
487 | veh_type = VehicleType::LEXUS_RX_450H;
488 | }
489 | else
490 | {
491 | veh_type = VehicleType::POLARIS_GEM;
492 | ROS_WARN("PACMod - An invalid vehicle type was entered. Assuming POLARIS_GEM.");
493 | }
494 | }
495 |
496 | // Advertise published messages
497 | can_rx_pub = n.advertise("can_rx", 20);
498 | global_rpt_pub = n.advertise