diff --git a/ros/ai2vcu/CMakeLists.txt b/ros/ai2vcu/CMakeLists.txt new file mode 100644 index 0000000..fa4cbdc --- /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 std_msgs ackermann_msgs message_generation +) + +## 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) +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 +) +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 + VCU2AI.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 +) + +################################################ +## 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/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 new file mode 100644 index 0000000..be5f789 --- /dev/null +++ b/ros/ai2vcu/package.xml @@ -0,0 +1,59 @@ + + + ai2vcu + 0.0.0 + The ai2vcu package + + + + + root + + + + + + TODO + + + + + + + + + + + + + + + + + + + roscpp + + + + + message_generation + + + + + + message_runtime + + + + + catkin + + + + + + + + diff --git a/ros/ai2vcu/src/ai2vcu_node.cpp b/ros/ai2vcu/src/ai2vcu_node.cpp new file mode 100644 index 0000000..40280c7 --- /dev/null +++ b/ros/ai2vcu/src/ai2vcu_node.cpp @@ -0,0 +1,199 @@ +#include +#include +#include + +#include + +char can_id[] = "can0"; + +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; + 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; + 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; + } + + 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() { + while (ros::ok()) { + m_fs_ai_api.transmit(); + m_fs_ai_api.receive(); + + m_publisher.publish(get_message()); + + m_rate.sleep(); + ros::spinOnce(); + } + } +}; + +int main(int argc, char* argv[]) { + ros::init(argc, argv, "ai2vcu"); + AI2VCUNode node(can_id); + node.spin(); +}