From 68509f71f91fe6f702f7df700e8b18cc59bb6d23 Mon Sep 17 00:00:00 2001 From: Frank Plowman Date: Sat, 8 Jun 2024 14:02:43 +0100 Subject: [PATCH 1/5] ai2vcu: Create project skeleton Initialise project. Additionally write some CMake for the dependency on the FS AI API. At the moment, this is done using CMake's ExternalProject. --- ros/ai2vcu/CMakeLists.txt | 223 +++++++++++++++++++++++++++++++++ ros/ai2vcu/package.xml | 59 +++++++++ ros/ai2vcu/src/ai2vcu_node.cpp | 15 +++ 3 files changed, 297 insertions(+) create mode 100644 ros/ai2vcu/CMakeLists.txt create mode 100644 ros/ai2vcu/package.xml create mode 100644 ros/ai2vcu/src/ai2vcu_node.cpp diff --git a/ros/ai2vcu/CMakeLists.txt b/ros/ai2vcu/CMakeLists.txt new file mode 100644 index 0000000..4e27b44 --- /dev/null +++ b/ros/ai2vcu/CMakeLists.txt @@ -0,0 +1,223 @@ +cmake_minimum_required(VERSION 3.7) +project(ai2vcu) + +include(ExternalProject) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +## System dependencies are found with CMake's conventions +ExternalProject_Add(fs_ai_api + GIT_REPOSITORY https://github.com/FS-AI/FS-AI_API.git + GIT_TAG main + SOURCE_SUBDIR FS-AI_API + CONFIGURE_COMMAND "" + BUILD_IN_SOURCE 1 + BUILD_COMMAND $(MAKE) -C / + INSTALL_COMMAND ${CMAKE_COMMAND} -E copy_directory /lib && ${CMAKE_COMMAND} -E copy_directory / /include +) +ExternalProject_Get_Property(fs_ai_api INSTALL_DIR) +add_library(fs_ai_api_lib STATIC IMPORTED GLOBAL) +set_property(TARGET fs_ai_api_lib PROPERTY + IMPORTED_LOCATION ${INSTALL_DIR}/lib/fs-ai_api.a +) +file(MAKE_DIRECTORY ${INSTALL_DIR}/include) +set_property(TARGET fs_ai_api_lib PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${INSTALL_DIR}/include +) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ai2vcu + CATKIN_DEPENDS roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/ai2vcu.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +# Declare a C++ executable +# With catkin_make all packages are built within a single CMake context +# The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/ai2vcu_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +# Add cmake target dependencies of the executable +# same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +# Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + fs_ai_api_lib +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ai2vcu.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ros/ai2vcu/package.xml b/ros/ai2vcu/package.xml new file mode 100644 index 0000000..91c01f8 --- /dev/null +++ b/ros/ai2vcu/package.xml @@ -0,0 +1,59 @@ + + + ai2vcu + 0.0.0 + The ai2vcu package + + + + + root + + + + + + TODO + + + + + + + + + + + + + + + + + + + roscpp + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/ros/ai2vcu/src/ai2vcu_node.cpp b/ros/ai2vcu/src/ai2vcu_node.cpp new file mode 100644 index 0000000..19038ff --- /dev/null +++ b/ros/ai2vcu/src/ai2vcu_node.cpp @@ -0,0 +1,15 @@ +#include +#include + +char can_id[] = "can0"; + +int main(int argc, char** argv) +{ + int err; + + ros::init(argc, argv, "ai2vcu"); + err = fs_ai_api_init(can_id, 0, 0); + if (err != EXIT_SUCCESS) + return err; + ros::spin(); +} From 774a56d6e8f71c09714ab84f49408e990882c1f1 Mon Sep 17 00:00:00 2001 From: Frank Plowman Date: Sat, 8 Jun 2024 16:42:38 +0100 Subject: [PATCH 2/5] ai2vcu: Create FS AI API abstraction --- ros/ai2vcu/src/ai2vcu_node.cpp | 147 +++++++++++++++++++++++++++++++-- 1 file changed, 141 insertions(+), 6 deletions(-) diff --git a/ros/ai2vcu/src/ai2vcu_node.cpp b/ros/ai2vcu/src/ai2vcu_node.cpp index 19038ff..6c01d16 100644 --- a/ros/ai2vcu/src/ai2vcu_node.cpp +++ b/ros/ai2vcu/src/ai2vcu_node.cpp @@ -3,13 +3,148 @@ char can_id[] = "can0"; -int main(int argc, char** argv) -{ - int err; +class FSAIAPI { +private: + fs_ai_api_ai2vcu m_tx_data; + fs_ai_api_vcu2ai m_rx_data; + static fs_ai_api_ai2vcu_struct tx_data_init() { + fs_ai_api_ai2vcu_struct ret; + ret.AI2VCU_MISSION_STATUS = MISSION_NOT_SELECTED; + ret.AI2VCU_DIRECTION_REQUEST = DIRECTION_NEUTRAL; + ret.AI2VCU_ESTOP_REQUEST = ESTOP_NO; + ret.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF; + ret.AI2VCU_STEER_ANGLE_REQUEST_deg = 0.0; + ret.AI2VCU_AXLE_SPEED_REQUEST_rpm = 0.0; + ret.AI2VCU_AXLE_TORQUE_REQUEST_Nm = 0.0; + ret.AI2VCU_BRAKE_PRESS_REQUEST_pct = 0.0; + return ret; + } + +public: + FSAIAPI(char *can_id, bool debug=false, bool simulate=false) + : m_tx_data(tx_data_init()) { + int err; + err = fs_ai_api_init(can_id, debug, simulate); + if (err != EXIT_SUCCESS) + throw err; + + transmit(); + receive(); + } + + // Ideally, this function should be called every 10ms. + void transmit() { + fs_ai_api_ai2vcu_set_data(&m_tx_data); + } + + void receive() { + fs_ai_api_vcu2ai_get_data(&m_rx_data); + } + + void set_mission_status(fs_ai_api_mission_status_e mission_status) { + m_tx_data.AI2VCU_MISSION_STATUS = mission_status; + } + + void set_direction_request(fs_ai_api_direction_request_e direction_request) { + m_tx_data.AI2VCU_DIRECTION_REQUEST = direction_request; + } + + void set_estop_request(fs_ai_api_estop_request_e estop_request) { + m_tx_data.AI2VCU_ESTOP_REQUEST = estop_request; + } + + void set_handshake_send_bit(fs_ai_api_handshake_send_bit_e handshake_send_bit) { + m_tx_data.AI2VCU_HANDSHAKE_SEND_BIT = handshake_send_bit; + } + + void set_steer_angle_request(float steer_angle_request) { + m_tx_data.AI2VCU_STEER_ANGLE_REQUEST_deg = steer_angle_request; + } + + void set_axle_speed_request(float axle_speed_request) { + m_tx_data.AI2VCU_AXLE_SPEED_REQUEST_rpm = axle_speed_request; + } + + void set_axle_torque_request(float axle_torque_request) { + m_tx_data.AI2VCU_AXLE_TORQUE_REQUEST_Nm = axle_torque_request; + } + + void set_brake_press_request(float brake_press_request) { + m_tx_data.AI2VCU_BRAKE_PRESS_REQUEST_pct = brake_press_request; + } + + fs_ai_api_handshake_receive_bit_e get_handshake_receive_bit() { + return m_rx_data.VCU2AI_HANDSHAKE_RECEIVE_BIT; + } + + fs_ai_api_res_go_signal_bit_e get_res_go_signal_bit() { + return m_rx_data.VCU2AI_RES_GO_SIGNAL; + } + + fs_ai_api_as_state_e get_as_state() { + return m_rx_data.VCU2AI_AS_STATE; + } + + fs_ai_api_ami_state_e get_ami_state() { + return m_rx_data.VCU2AI_AMI_STATE; + } + + float get_steer_angle() { + return m_rx_data.VCU2AI_STEER_ANGLE_deg; + } + + float get_brake_press_front() { + return m_rx_data.VCU2AI_BRAKE_PRESS_F_pct; + } + + float get_brake_press_rear() { + return m_rx_data.VCU2AI_BRAKE_PRESS_R_pct; + } + + float get_wheel_speed_front_left() { + return m_rx_data.VCU2AI_FL_WHEEL_SPEED_rpm; + } + + float get_wheel_speed_front_right() { + return m_rx_data.VCU2AI_FR_WHEEL_SPEED_rpm; + } + + float get_wheel_speed_rear_left() { + return m_rx_data.VCU2AI_RL_WHEEL_SPEED_rpm; + } + + float get_wheel_speed_rear_right() { + return m_rx_data.VCU2AI_RR_WHEEL_SPEED_rpm; + } + + uint16_t get_pulse_count_front_left() { + return m_rx_data.VCU2AI_FL_PULSE_COUNT; + } + + uint16_t get_pulse_count_front_right() { + return m_rx_data.VCU2AI_FR_PULSE_COUNT; + } + + uint16_t get_pulse_count_rear_left() { + return m_rx_data.VCU2AI_RL_PULSE_COUNT; + } + + uint16_t get_pulse_count_rear_right() { + return m_rx_data.VCU2AI_RR_PULSE_COUNT; + } +}; + +class AI2VCUNode { +private: + FSAIAPI m_fs_ai_api; + +public: + AI2VCUNode(char *can_id): m_fs_ai_api(can_id) {} +}; + +int main(int argc, char* argv[]) { ros::init(argc, argv, "ai2vcu"); - err = fs_ai_api_init(can_id, 0, 0); - if (err != EXIT_SUCCESS) - return err; + AI2VCUNode node(can_id); ros::spin(); } From 4c080dd212a55303598c4e2ed73115ccc89c6805 Mon Sep 17 00:00:00 2001 From: Frank Plowman Date: Sat, 8 Jun 2024 18:23:32 +0100 Subject: [PATCH 3/5] ai2vcu: Add publisher for VCU2AI messages --- ros/ai2vcu/CMakeLists.txt | 23 +++++++++--------- ros/ai2vcu/msg/VCU2AI.msg | 42 +++++++++++++++++++++++++++++++++ ros/ai2vcu/package.xml | 4 ++-- ros/ai2vcu/src/ai2vcu_node.cpp | 43 ++++++++++++++++++++++++++++++++-- 4 files changed, 96 insertions(+), 16 deletions(-) create mode 100644 ros/ai2vcu/msg/VCU2AI.msg diff --git a/ros/ai2vcu/CMakeLists.txt b/ros/ai2vcu/CMakeLists.txt index 4e27b44..e07f00e 100644 --- a/ros/ai2vcu/CMakeLists.txt +++ b/ros/ai2vcu/CMakeLists.txt @@ -10,7 +10,7 @@ include(ExternalProject) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - roscpp + roscpp std_msgs message_generation ) ## System dependencies are found with CMake's conventions @@ -62,12 +62,11 @@ set_property(TARGET fs_ai_api_lib PROPERTY ## * uncomment the generate_messages entry below ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +# Generate messages in the 'msg' folder +add_message_files( + FILES + VCU2AI.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -83,11 +82,11 @@ set_property(TARGET fs_ai_api_lib PROPERTY # Action2.action # ) -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +# Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## diff --git a/ros/ai2vcu/msg/VCU2AI.msg b/ros/ai2vcu/msg/VCU2AI.msg new file mode 100644 index 0000000..801c1c5 --- /dev/null +++ b/ros/ai2vcu/msg/VCU2AI.msg @@ -0,0 +1,42 @@ +bool handshake_receive_bit + +bool res_go_signal + +uint8 AS_OFF=1 +uint8 AS_READY=2 +uint8 AS_DRIVING=3 +uint8 AS_EMERGENCY_BRAKE=4 +uint8 AS_FINISHED=5 +uint8 as_state + +uint8 AMI_NOT_SELECTED=0 +uint8 AMI_ACCELERATION=1 +uint8 AMI_SKIDPAD=2 +uint8 AMI_AUTOCROSS=3 +uint8 AMI_TRACK_DRIVE=4 +uint8 AMI_STATIC_INSPECTION_A=5 +uint8 AMI_STATIC_INSPECTION_B=6 +uint8 AMI_AUTONOMOUS_DEMO=7 +uint8 ami_state + +float32 steer_angle_deg + +float32 brake_press_f_pct + +float32 brake_press_r_pct + +float32 fl_wheel_speed_rpm + +float32 fr_wheel_speed_rpm + +float32 rl_wheel_speed_rpm + +float32 rr_wheel_speed_rpm + +uint16 fl_pulse_count + +uint16 fr_pulse_count + +uint16 rl_pulse_count + +uint16 rr_pulse_count diff --git a/ros/ai2vcu/package.xml b/ros/ai2vcu/package.xml index 91c01f8..be5f789 100644 --- a/ros/ai2vcu/package.xml +++ b/ros/ai2vcu/package.xml @@ -37,13 +37,13 @@ - + message_generation - + message_runtime diff --git a/ros/ai2vcu/src/ai2vcu_node.cpp b/ros/ai2vcu/src/ai2vcu_node.cpp index 6c01d16..a12c465 100644 --- a/ros/ai2vcu/src/ai2vcu_node.cpp +++ b/ros/ai2vcu/src/ai2vcu_node.cpp @@ -1,6 +1,8 @@ #include #include +#include + char can_id[] = "can0"; class FSAIAPI { @@ -138,13 +140,50 @@ class FSAIAPI { class AI2VCUNode { private: FSAIAPI m_fs_ai_api; + ros::NodeHandle m_node_handle; + ros::Rate m_rate; + ros::Publisher m_publisher; + + ai2vcu::VCU2AI get_message() { + ai2vcu::VCU2AI msg; + msg.handshake_receive_bit = m_fs_ai_api.get_handshake_receive_bit(); + msg.res_go_signal = m_fs_ai_api.get_res_go_signal_bit(); + msg.as_state = m_fs_ai_api.get_as_state(); + msg.ami_state = m_fs_ai_api.get_ami_state(); + msg.steer_angle_deg = m_fs_ai_api.get_steer_angle(); + msg.brake_press_f_pct = m_fs_ai_api.get_brake_press_front(); + msg.brake_press_r_pct = m_fs_ai_api.get_brake_press_rear(); + msg.fl_wheel_speed_rpm = m_fs_ai_api.get_wheel_speed_front_left(); + msg.fr_wheel_speed_rpm = m_fs_ai_api.get_wheel_speed_front_right(); + msg.rl_wheel_speed_rpm = m_fs_ai_api.get_wheel_speed_rear_left(); + msg.rr_wheel_speed_rpm = m_fs_ai_api.get_wheel_speed_rear_right(); + msg.fl_pulse_count = m_fs_ai_api.get_pulse_count_front_left(); + msg.fr_pulse_count = m_fs_ai_api.get_pulse_count_front_right(); + msg.rl_pulse_count = m_fs_ai_api.get_pulse_count_rear_left(); + msg.rr_pulse_count = m_fs_ai_api.get_pulse_count_rear_right(); + return msg; + } public: - AI2VCUNode(char *can_id): m_fs_ai_api(can_id) {} + AI2VCUNode(char *can_id): m_fs_ai_api(can_id), m_rate(100) { + m_publisher = m_node_handle.advertise("vcu2ai", 100); + } + + void spin() { + while (ros::ok()) { + m_fs_ai_api.transmit(); + m_fs_ai_api.receive(); + + m_publisher.publish(get_message()); + + ros::spinOnce(); + m_rate.sleep(); + } + } }; int main(int argc, char* argv[]) { ros::init(argc, argv, "ai2vcu"); AI2VCUNode node(can_id); - ros::spin(); + node.spin(); } From 31c4479c1ed1cbedec72515910c1745566e652e7 Mon Sep 17 00:00:00 2001 From: Frank Plowman Date: Sun, 9 Jun 2024 10:07:26 +0100 Subject: [PATCH 4/5] ai2vcu: Ensure the FS-AI API has been built before the node --- ros/ai2vcu/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/ros/ai2vcu/CMakeLists.txt b/ros/ai2vcu/CMakeLists.txt index e07f00e..14a1b51 100644 --- a/ros/ai2vcu/CMakeLists.txt +++ b/ros/ai2vcu/CMakeLists.txt @@ -25,6 +25,7 @@ ExternalProject_Add(fs_ai_api ) ExternalProject_Get_Property(fs_ai_api INSTALL_DIR) add_library(fs_ai_api_lib STATIC IMPORTED GLOBAL) +add_dependencies(fs_ai_api_lib fs_ai_api) set_property(TARGET fs_ai_api_lib PROPERTY IMPORTED_LOCATION ${INSTALL_DIR}/lib/fs-ai_api.a ) From 400e5603ec30c3d7704df4aa7d5dca35e28cb1f7 Mon Sep 17 00:00:00 2001 From: Frank Plowman Date: Sun, 9 Jun 2024 10:21:13 +0100 Subject: [PATCH 5/5] ai2vcu: Implement AI2VCU messages --- ros/ai2vcu/CMakeLists.txt | 2 +- ros/ai2vcu/src/ai2vcu_node.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/ros/ai2vcu/CMakeLists.txt b/ros/ai2vcu/CMakeLists.txt index 14a1b51..fa4cbdc 100644 --- a/ros/ai2vcu/CMakeLists.txt +++ b/ros/ai2vcu/CMakeLists.txt @@ -10,7 +10,7 @@ include(ExternalProject) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - roscpp std_msgs message_generation + roscpp std_msgs ackermann_msgs message_generation ) ## System dependencies are found with CMake's conventions diff --git a/ros/ai2vcu/src/ai2vcu_node.cpp b/ros/ai2vcu/src/ai2vcu_node.cpp index a12c465..40280c7 100644 --- a/ros/ai2vcu/src/ai2vcu_node.cpp +++ b/ros/ai2vcu/src/ai2vcu_node.cpp @@ -1,5 +1,6 @@ #include #include +#include #include @@ -143,6 +144,7 @@ class AI2VCUNode { ros::NodeHandle m_node_handle; ros::Rate m_rate; ros::Publisher m_publisher; + ros::Subscriber m_subscriber_ackermann; ai2vcu::VCU2AI get_message() { ai2vcu::VCU2AI msg; @@ -164,9 +166,17 @@ class AI2VCUNode { return msg; } + void ackermann_callback(const ackermann_msgs::AckermannDrive::ConstPtr &msg) { + m_fs_ai_api.set_direction_request(msg->speed > 0 ? DIRECTION_FORWARD : DIRECTION_NEUTRAL); + m_fs_ai_api.set_steer_angle_request(msg->steering_angle); + m_fs_ai_api.set_axle_speed_request(msg->speed); + m_fs_ai_api.set_axle_torque_request(msg->speed > 0 ? 250 : 0); + } + public: AI2VCUNode(char *can_id): m_fs_ai_api(can_id), m_rate(100) { m_publisher = m_node_handle.advertise("vcu2ai", 100); + m_subscriber_ackermann = m_node_handle.subscribe("ackermann_cmd", 1000, &AI2VCUNode::ackermann_callback, this); } void spin() { @@ -176,8 +186,8 @@ class AI2VCUNode { m_publisher.publish(get_message()); - ros::spinOnce(); m_rate.sleep(); + ros::spinOnce(); } } };