From 50314f76ff8887c00838224e2ad98f420b18f876 Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:49:40 +1000 Subject: [PATCH 1/8] deprecate old unused msgs. add float32stamped to include header data --- .../driverless_common/marker.py | 4 ++-- src/common/driverless_msgs/CMakeLists.txt | 19 +++++++------------ src/common/driverless_msgs/msg/BrickData.msg | 3 --- src/common/driverless_msgs/msg/CarStatus.msg | 2 -- .../{WSSVelocity.msg => Float32Stamped.msg} | 2 +- .../driverless_msgs/msg/Int32Stamped.msg | 2 ++ src/common/driverless_msgs/msg/MotorRPM.msg | 2 -- src/common/driverless_msgs/msg/Reset.msg | 1 - .../driverless_msgs/msg/SteeringReading.msg | 3 --- .../msg/TrackDetectionStamped.msg | 2 -- 10 files changed, 12 insertions(+), 28 deletions(-) delete mode 100644 src/common/driverless_msgs/msg/BrickData.msg delete mode 100644 src/common/driverless_msgs/msg/CarStatus.msg rename src/common/driverless_msgs/msg/{WSSVelocity.msg => Float32Stamped.msg} (57%) create mode 100644 src/common/driverless_msgs/msg/Int32Stamped.msg delete mode 100644 src/common/driverless_msgs/msg/MotorRPM.msg delete mode 100644 src/common/driverless_msgs/msg/Reset.msg delete mode 100644 src/common/driverless_msgs/msg/SteeringReading.msg delete mode 100644 src/common/driverless_msgs/msg/TrackDetectionStamped.msg diff --git a/src/common/driverless_common/driverless_common/marker.py b/src/common/driverless_common/driverless_common/marker.py index 83c1ce5ba..01265ddb2 100644 --- a/src/common/driverless_common/driverless_common/marker.py +++ b/src/common/driverless_common/driverless_common/marker.py @@ -1,7 +1,7 @@ from math import sqrt from builtin_interfaces.msg import Duration -from driverless_msgs.msg import Cone, ConeDetectionStamped, ConeWithCovariance, TrackDetectionStamped +from driverless_msgs.msg import Cone, ConeDetectionStamped, ConeWithCovariance from geometry_msgs.msg import Point, Pose, Quaternion, Vector3 from std_msgs.msg import ColorRGBA, Header from visualization_msgs.msg import Marker, MarkerArray @@ -13,7 +13,7 @@ MAX_NUM_CONES = 50 -def marker_array_from_map(detection: TrackDetectionStamped, ground_truth: bool = False) -> MarkerArray: +def marker_array_from_map(detection: ConeDetectionStamped, ground_truth: bool = False) -> MarkerArray: cones: List[ConeWithCovariance] = detection.cones if ground_truth: # ground truth markers out of the sim are translucent diff --git a/src/common/driverless_msgs/CMakeLists.txt b/src/common/driverless_msgs/CMakeLists.txt index e648a895e..fb849723e 100644 --- a/src/common/driverless_msgs/CMakeLists.txt +++ b/src/common/driverless_msgs/CMakeLists.txt @@ -18,26 +18,21 @@ find_package(geometry_msgs REQUIRED) # messages set(msg_files - "msg/BrickData.msg" + "msg/Can.msg" "msg/Cone.msg" "msg/ConeDetectionStamped.msg" - "msg/CarStatus.msg" - "msg/MotorRPM.msg" + "msg/ConeWithCovariance.msg" + "msg/DebugMsg.msg" + "msg/DoubleMatrix.msg" + "msg/DrivingDynamics1.msg" + "msg/Float32Stamped.msg" + "msg/Int32Stamped.msg" "msg/PathPoint.msg" "msg/PathStamped.msg" - "msg/Can.msg" "msg/RES.msg" - "msg/Reset.msg" "msg/Shutdown.msg" "msg/State.msg" - "msg/ConeWithCovariance.msg" - "msg/TrackDetectionStamped.msg" - "msg/SteeringReading.msg" "msg/SystemStatus.msg" - "msg/DrivingDynamics1.msg" - "msg/WSSVelocity.msg" - "msg/DebugMsg.msg" - "msg/DoubleMatrix.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/src/common/driverless_msgs/msg/BrickData.msg b/src/common/driverless_msgs/msg/BrickData.msg deleted file mode 100644 index b435c671b..000000000 --- a/src/common/driverless_msgs/msg/BrickData.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 id -uint16[] voltages -uint8[] temperatures diff --git a/src/common/driverless_msgs/msg/CarStatus.msg b/src/common/driverless_msgs/msg/CarStatus.msg deleted file mode 100644 index 7c9a71ad7..000000000 --- a/src/common/driverless_msgs/msg/CarStatus.msg +++ /dev/null @@ -1,2 +0,0 @@ -bool voltage_error -BrickData[] brick_data diff --git a/src/common/driverless_msgs/msg/WSSVelocity.msg b/src/common/driverless_msgs/msg/Float32Stamped.msg similarity index 57% rename from src/common/driverless_msgs/msg/WSSVelocity.msg rename to src/common/driverless_msgs/msg/Float32Stamped.msg index 2f8b6fff9..207241975 100644 --- a/src/common/driverless_msgs/msg/WSSVelocity.msg +++ b/src/common/driverless_msgs/msg/Float32Stamped.msg @@ -1,2 +1,2 @@ std_msgs/Header header -float32 velocity +float32 data diff --git a/src/common/driverless_msgs/msg/Int32Stamped.msg b/src/common/driverless_msgs/msg/Int32Stamped.msg new file mode 100644 index 000000000..bf748e857 --- /dev/null +++ b/src/common/driverless_msgs/msg/Int32Stamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +int32 data diff --git a/src/common/driverless_msgs/msg/MotorRPM.msg b/src/common/driverless_msgs/msg/MotorRPM.msg deleted file mode 100644 index de1b77635..000000000 --- a/src/common/driverless_msgs/msg/MotorRPM.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 index -int32 rpm diff --git a/src/common/driverless_msgs/msg/Reset.msg b/src/common/driverless_msgs/msg/Reset.msg deleted file mode 100644 index 26cb10e23..000000000 --- a/src/common/driverless_msgs/msg/Reset.msg +++ /dev/null @@ -1 +0,0 @@ -bool reset diff --git a/src/common/driverless_msgs/msg/SteeringReading.msg b/src/common/driverless_msgs/msg/SteeringReading.msg deleted file mode 100644 index b74bd2c15..000000000 --- a/src/common/driverless_msgs/msg/SteeringReading.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 steering_angle -uint16 adc_0 -uint16 adc_1 diff --git a/src/common/driverless_msgs/msg/TrackDetectionStamped.msg b/src/common/driverless_msgs/msg/TrackDetectionStamped.msg deleted file mode 100644 index a79647ca1..000000000 --- a/src/common/driverless_msgs/msg/TrackDetectionStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -ConeWithCovariance[] cones From ebc582537150aa0108381c9cd84fa72f6f8fd211 Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:50:13 +1000 Subject: [PATCH 2/8] compose velodyne nodes in launch. fix view range --- .../sensors/config/VLP32C-Onboard.yaml | 4 +- src/hardware/sensors/config/VLP32C.yaml | 6 +- src/hardware/sensors/launch/vlp32.launch.py | 58 +++++++++++++------ 3 files changed, 47 insertions(+), 21 deletions(-) diff --git a/src/hardware/sensors/config/VLP32C-Onboard.yaml b/src/hardware/sensors/config/VLP32C-Onboard.yaml index e6ce32867..583042f1d 100644 --- a/src/hardware/sensors/config/VLP32C-Onboard.yaml +++ b/src/hardware/sensors/config/VLP32C-Onboard.yaml @@ -1,5 +1,5 @@ rpm: 600 returns: Strongest fov: - start: 300 - end: 60 + start: 270 + end: 90 diff --git a/src/hardware/sensors/config/VLP32C.yaml b/src/hardware/sensors/config/VLP32C.yaml index 33c775ce2..25394b4a4 100644 --- a/src/hardware/sensors/config/VLP32C.yaml +++ b/src/hardware/sensors/config/VLP32C.yaml @@ -17,11 +17,13 @@ velodyne_laserscan_node: ring: -1 resolution: 0.01 -velodyne_convert_node: +velodyne_transform_node: ros__parameters: calibration: VeloView-VLP32C.yaml min_range: 0.5 max_range: 130.0 view_direction: 0.0 - view_width: 3.141592 + view_width: 6.282185 organize_cloud: true + fixed_frame: "" + target_frame: "" diff --git a/src/hardware/sensors/launch/vlp32.launch.py b/src/hardware/sensors/launch/vlp32.launch.py index 516d83b36..746b9858e 100644 --- a/src/hardware/sensors/launch/vlp32.launch.py +++ b/src/hardware/sensors/launch/vlp32.launch.py @@ -3,6 +3,8 @@ from ament_index_python.packages import get_package_share_path import launch import launch_ros.actions +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode import yaml @@ -13,12 +15,15 @@ def generate_launch_description(): package="velodyne_driver", executable="velodyne_driver_node", output="both", parameters=[driver_params_file] ) - convert_params_file = os.path.join(get_package_share_path("sensors") / "config" / "VLP32C.yaml") - with open(convert_params_file, "r") as f: - convert_params = yaml.safe_load(f)["velodyne_convert_node"]["ros__parameters"] - convert_params["calibration"] = str(get_package_share_path("sensors") / "config" / "VLP32C-calibration.yaml") - velodyne_convert_node = launch_ros.actions.Node( - package="velodyne_pointcloud", executable="velodyne_convert_node", output="both", parameters=[convert_params] + transform_params_file = os.path.join(get_package_share_path("sensors") / "config" / "VLP32C.yaml") + with open(transform_params_file, "r") as f: + transform_params = yaml.safe_load(f)["velodyne_transform_node"]["ros__parameters"] + transform_params["calibration"] = str(get_package_share_path("sensors") / "config" / "VLP32C-calibration.yaml") + velodyne_transform_node = launch_ros.actions.Node( + package="velodyne_pointcloud", + executable="velodyne_transform_node", + output="both", + parameters=[transform_params], ) laserscan_params_file = str(get_package_share_path("sensors") / "config" / "VLP32C.yaml") @@ -29,16 +34,35 @@ def generate_launch_description(): parameters=[laserscan_params_file], ) - return launch.LaunchDescription( - [ - velodyne_driver_node, - velodyne_convert_node, - velodyne_laserscan_node, - launch.actions.RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=velodyne_driver_node, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], - ) + container = ComposableNodeContainer( + name="velodyne_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="velodyne_driver", + plugin="velodyne_driver::VelodyneDriver", + name="velodyne_driver_node", + parameters=[driver_params_file], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="velodyne_pointcloud", + plugin="velodyne_pointcloud::Convert", # Transform', + name="velodyne_transform_node", + parameters=[transform_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="velodyne_laserscan", + plugin="velodyne_laserscan::VelodyneLaserScan", + name="velodyne_laserscan_node", + parameters=[laserscan_params_file], + extra_arguments=[{"use_intra_process_comms": True}], ), - ] + ], + output="both", ) + + return launch.LaunchDescription([container]) From f6fa4551516a70c12634539d249393c769d77e98 Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:51:30 +1000 Subject: [PATCH 3/8] compose vehicle supervisor. split into .hpp. inheriting supervisor for launching version --- .../vehicle_supervisor/CMakeLists.txt | 67 +- .../include/component_supervisor.hpp | 98 +++ .../include/component_supervisor_launch.hpp | 21 + .../src/component_supervisor.cpp | 477 +++++++++++++++ .../src/component_supervisor_launch.cpp | 201 ++++++ .../src/node_supervisor.cpp | 578 +----------------- .../src/node_supervisor_launch.cpp | 17 + .../src/node_supervisor_slim.cpp | 559 ----------------- 8 files changed, 867 insertions(+), 1151 deletions(-) create mode 100644 src/hardware/vehicle_supervisor/include/component_supervisor.hpp create mode 100644 src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp create mode 100644 src/hardware/vehicle_supervisor/src/component_supervisor.cpp create mode 100644 src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp create mode 100644 src/hardware/vehicle_supervisor/src/node_supervisor_launch.cpp delete mode 100644 src/hardware/vehicle_supervisor/src/node_supervisor_slim.cpp diff --git a/src/hardware/vehicle_supervisor/CMakeLists.txt b/src/hardware/vehicle_supervisor/CMakeLists.txt index 377fcf272..92fff093f 100644 --- a/src/hardware/vehicle_supervisor/CMakeLists.txt +++ b/src/hardware/vehicle_supervisor/CMakeLists.txt @@ -7,28 +7,28 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. find_package(ackermann_msgs REQUIRED) find_package(driverless_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) -add_compile_definitions(QUTMS_CAN_VESC) add_compile_definitions(QUTMS_CAN_VCU) add_compile_definitions(QUTMS_CAN_DVL) add_compile_definitions(QUTMS_CAN_RES) add_compile_definitions(QUTMS_CAN_EBS) add_compile_definitions(QUTMS_CAN_SW) -include_directories(include) -include_directories(${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Inc) -include_directories(${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/include/) +include_directories( + include + ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Inc + ${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/include + ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include +) set (SOURCES ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_VCU.c - ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_VESC.c ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_DVL.c ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_EBS_CTRL.c ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_RES.c @@ -39,22 +39,49 @@ set (SOURCES SET_SOURCE_FILES_PROPERTIES(${SOURCES} PROPERTIES LANGUAGE CXX ) -add_executable(vehicle_supervisor_node src/node_supervisor.cpp ${SOURCES}) -add_executable(vehicle_supervisor_slim_node src/node_supervisor_slim.cpp ${SOURCES}) -ament_target_dependencies(vehicle_supervisor_node rclcpp driverless_msgs ackermann_msgs std_msgs nav_msgs) -ament_target_dependencies(vehicle_supervisor_slim_node rclcpp driverless_msgs ackermann_msgs std_msgs nav_msgs) -target_include_directories(vehicle_supervisor_node PUBLIC ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include) -target_include_directories(vehicle_supervisor_slim_node PUBLIC ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include) +# ROS 2 components +add_library(supervisor_component SHARED src/component_supervisor.cpp ${SOURCES}) +ament_target_dependencies(supervisor_component + "rclcpp" + "rclcpp_components" + "std_msgs" + "nav_msgs" + "driverless_msgs" + "ackermann_msgs" +) +rclcpp_components_register_nodes(supervisor_component "vehicle_supervisor::ASSupervisor") + +add_library(supervisor_launch_component SHARED src/component_supervisor_launch.cpp src/component_supervisor.cpp ${SOURCES}) +ament_target_dependencies(supervisor_launch_component + "rclcpp" + "rclcpp_components" + "std_msgs" + "nav_msgs" + "driverless_msgs" + "ackermann_msgs") +rclcpp_components_register_nodes(supervisor_launch_component "vehicle_supervisor::ASSupervisorLaunch") -target_include_directories(vehicle_supervisor_node PUBLIC - $ - $) +# ROS 2 nodes +add_executable(supervisor_node src/node_supervisor.cpp) +target_link_libraries(supervisor_node supervisor_component) +ament_target_dependencies(supervisor_node + "rclcpp" +) + +add_executable(supervisor_launch_node src/node_supervisor_launch.cpp) +target_link_libraries(supervisor_launch_node supervisor_launch_component supervisor_component) +ament_target_dependencies(supervisor_launch_node + "rclcpp" +) -target_include_directories(vehicle_supervisor_slim_node PUBLIC - $ - $) +install(TARGETS + supervisor_component supervisor_launch_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) -install(TARGETS vehicle_supervisor_slim_node vehicle_supervisor_node +install(TARGETS + supervisor_node supervisor_launch_node DESTINATION lib/${PROJECT_NAME}) ament_package() diff --git a/src/hardware/vehicle_supervisor/include/component_supervisor.hpp b/src/hardware/vehicle_supervisor/include/component_supervisor.hpp new file mode 100644 index 000000000..328dc0df1 --- /dev/null +++ b/src/hardware/vehicle_supervisor/include/component_supervisor.hpp @@ -0,0 +1,98 @@ +#ifndef VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_HPP_ +#define VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_HPP_ + +#include + +#include "CAN_DVL.h" +#include "CAN_EBS_CTRL.h" +#include "CAN_RES.h" +#include "CAN_SW.h" +#include "CAN_VCU.h" +#include "QUTMS_can.h" +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "can_interface.hpp" +#include "driverless_common/common.hpp" +#include "driverless_msgs/msg/can.hpp" +#include "driverless_msgs/msg/driving_dynamics1.hpp" +#include "driverless_msgs/msg/res.hpp" +#include "driverless_msgs/msg/shutdown.hpp" +#include "driverless_msgs/msg/state.hpp" +#include "driverless_msgs/msg/system_status.hpp" +#include "driverless_msgs/msg/float32_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/u_int8.hpp" + +using std::placeholders::_1; + +namespace vehicle_supervisor { + +class ASSupervisor : public rclcpp::Node, public CanInterface { + protected: + DVL_HeartbeatState_t DVL_heartbeat; + VCU_HeartbeatState_t CTRL_VCU_heartbeat; + EBS_CTRL_HeartbeatState_t EBS_heartbeat; + SW_HeartbeatState_t SW_heartbeat; + DVL_DrivingDynamics1_Data_u DVL_drivingDynamics1; + DVL_SystemStatus_Data_u DVL_systemStatus; + RES_Status_t RES_status; + + driverless_msgs::msg::State ros_state; + + // callback timers + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::TimerBase::SharedPtr res_alive_timer_; + rclcpp::TimerBase::SharedPtr dataLogger_timer_; + + // subscribers + rclcpp::Subscription::SharedPtr can_sub_; + rclcpp::Subscription::SharedPtr canopen_sub_; + rclcpp::Subscription::SharedPtr control_sub_; + rclcpp::Subscription::SharedPtr shutdown_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + rclcpp::Subscription::SharedPtr lap_sub_; + rclcpp::Subscription::SharedPtr steering_ready_sub_; + + // publishers + rclcpp::Publisher::SharedPtr can_pub_; + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Publisher::SharedPtr res_status_pub_; + rclcpp::Publisher::SharedPtr logging_drivingDynamics1_pub_; + rclcpp::Publisher::SharedPtr logging_systemStatus_pub_; + + rclcpp::CallbackGroup::SharedPtr sensor_cb_group_; + rclcpp::CallbackGroup::SharedPtr ctrl_cb_group_; + rclcpp::CallbackGroup::SharedPtr timer_cb_group_; + + bool res_alive = 0; + float last_torque = 0; + float last_steering_angle = 0; + float last_velocity = 0; + + void canopen_callback(const driverless_msgs::msg::Can::SharedPtr msg); + void can_callback(const driverless_msgs::msg::Can::SharedPtr msg); + void velocity_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg); + void steering_angle_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg); + void control_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg); + void lap_counter_callback(const std_msgs::msg::UInt8::SharedPtr msg); + void steering_state_callback(const std_msgs::msg::Bool::SharedPtr msg); + void shutdown_callback(const driverless_msgs::msg::Shutdown::SharedPtr msg); + + void dvl_heartbeat_timer_callback(); + void res_alive_timer_callback(); + void dataLogger_timer_callback(); + + void run_fsm(); + void publish_heartbeart(); + void reset_dataLogger(); + + public: + ASSupervisor(const rclcpp::NodeOptions & options, std::string name="vehicle_supervisor_node"); + +}; + +} // namespace vehicle_supervisor + +#endif // VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_HPP_ \ No newline at end of file diff --git a/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp b/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp new file mode 100644 index 000000000..343454f92 --- /dev/null +++ b/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp @@ -0,0 +1,21 @@ +#ifndef VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_LAUNCH_HPP_ +#define VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_LAUNCH_HPP_ + +#include "component_supervisor.hpp" + +namespace vehicle_supervisor { + +class ASSupervisorLaunch : public ASSupervisor { + private: + void run_fsm(); + void launch_mission(); + + void dvl_heartbeat_timer_callback(); + + public: + ASSupervisorLaunch(const rclcpp::NodeOptions & options); +}; + +} // namespace vehicle_supervisor + +#endif // VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_LAUNCH_HPP_ \ No newline at end of file diff --git a/src/hardware/vehicle_supervisor/src/component_supervisor.cpp b/src/hardware/vehicle_supervisor/src/component_supervisor.cpp new file mode 100644 index 000000000..2ea9396bb --- /dev/null +++ b/src/hardware/vehicle_supervisor/src/component_supervisor.cpp @@ -0,0 +1,477 @@ +#include "component_supervisor.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace vehicle_supervisor { + +void ASSupervisor::canopen_callback(const driverless_msgs::msg::Can::SharedPtr msg) { + // RES boot up + if (msg->id == (RES_BOOT_UP_ID)) { + /* + RES has reported in, request state change to enable it + + Doing so will result in the RES reporting PDOs + 2000 - 20007 every 30ms with the ID 0x180 + RES_NODE_ID + + Byte 0 = state command (start up) + Byte 1 = Node ID (0x00 = All Nodes) + */ + RCLCPP_INFO(this->get_logger(), "RES Booted"); + uint8_t p[8] = {0x01, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + this->can_pub_->publish(this->_d_2_f(0x00, false, p, sizeof(p))); + this->res_alive = 1; + } + // RES heartbeat + else if (msg->id == (RES_HEARTBEAT_ID)) { + // RES Reciever Status Packet + Parse_RES_Heartbeat((uint8_t *)&msg->data[0], &this->RES_status); + // Log RES state + RCLCPP_DEBUG(this->get_logger(), "RES Status: [SW, BT]: %i, %i -- [EST]: %i, -- [RAD_QUAL]: %i", + this->RES_status.sw_k2, this->RES_status.bt_k3, this->RES_status.estop, + this->RES_status.radio_quality); + // // publish RES status + // driverless_msgs::msg::RES res_msg; + // res_msg.sw_k2 = this->RES_status.sw_k2; + // res_msg.bt_k3 = this->RES_status.bt_k3; + // res_msg.estop = this->RES_status.estop; + // res_msg.radio_quality = this->RES_status.radio_quality; + // res_msg.loss_of_signal_shutdown_notice = this->RES_status.loss_of_signal_shutdown_notice; + // this->res_status_pub_->publish(res_msg); + this->res_alive = 1; + } +} + +void ASSupervisor::can_callback(const driverless_msgs::msg::Can::SharedPtr msg) { + uint32_t qutms_masked_id = msg->id & ~0xF; + + // CAN hearbeats + if (qutms_masked_id == VCU_Heartbeat_ID) { + uint8_t VCU_ID = msg->id & 0xF; + RCLCPP_DEBUG(this->get_logger(), "VCU ID: %u STATE: %02x", VCU_ID, msg->data[0]); + + // data vector to uint8_t array + uint8_t data[8]; + copy_data(msg->data, data, 8); + } else if (msg->id == EBS_CTRL_Heartbeat_ID) { + uint8_t data[8]; + copy_data(msg->data, data, 8); + + Parse_EBS_CTRL_Heartbeat(data, &this->EBS_heartbeat); + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE_DRIVE) { + this->DVL_systemStatus._fields.EBS_state = DVL_EBS_STATE_ARMED; + } else { + this->DVL_systemStatus._fields.EBS_state = DVL_EBS_STATE_ACTIVATED; + } + } else if (qutms_masked_id == SW_Heartbeat_ID) { + // data vector to uint8_t array + uint8_t data[4]; + copy_data(msg->data, data, 4); + + Parse_SW_Heartbeat(data, &this->SW_heartbeat); + RCLCPP_DEBUG(this->get_logger(), "SW State: %02x Mission Id: %d", this->SW_heartbeat.stateID, + this->SW_heartbeat.missionID); + } +} + +void ASSupervisor::velocity_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg) { + this->last_velocity = msg->data; + this->DVL_drivingDynamics1._fields.speed_actual = (int8_t)msg->data; +} + +void ASSupervisor::steering_angle_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg) { + this->last_steering_angle = msg->data; + this->DVL_drivingDynamics1._fields.steering_angle_actual = (int8_t)msg->data; +} + +void ASSupervisor::control_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg) { + // input range: -1 to 1, convert to -100 to 100 + this->last_torque = 100 * msg->drive.acceleration; + // convert requested accel to estimated motor torque + float torqueValue = msg->drive.acceleration * 9 * 4.5 * 4; + + this->DVL_drivingDynamics1._fields.steering_angle_target = (int8_t)msg->drive.steering_angle; + this->DVL_drivingDynamics1._fields.speed_target = (int8_t)msg->drive.speed; + this->DVL_drivingDynamics1._fields.motor_moment_target = (int8_t)torqueValue; + this->DVL_drivingDynamics1._fields.motor_moment_actual = (int8_t)torqueValue; +} + +void ASSupervisor::lap_counter_callback(const std_msgs::msg::UInt8::SharedPtr msg) { + this->ros_state.lap_count = msg->data; + this->DVL_systemStatus._fields.lap_counter = msg->data; +} + +void ASSupervisor::steering_state_callback(const std_msgs::msg::Bool::SharedPtr msg) { + this->ros_state.navigation_ready = msg->data; +} + +void ASSupervisor::shutdown_callback(const driverless_msgs::msg::Shutdown::SharedPtr msg) { + if (msg->emergency_shutdown) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } else if (msg->finished_engage_ebs) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_ACTIVATE_EBS; + } else if (msg->finished) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; + } +} + +void ASSupervisor::publish_heartbeart() { + if (this->get_parameter("manual_override").as_bool()) { + // override straight to driving trackdrive + DVL_HeartbeatState_t or_DVL_heartbeat; + or_DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; + or_DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; + auto heartbeat = Compose_DVL_Heartbeat(&or_DVL_heartbeat); + this->can_pub_->publish(std::move( + this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data)))); + + driverless_msgs::msg::State::UniquePtr or_ros_state(new driverless_msgs::msg::State()); + or_ros_state->state = or_DVL_heartbeat.stateID; + or_ros_state->mission = DRIVERLESS_MISSIONS::MISSION_TRACK; + or_ros_state->lap_count = this->ros_state.lap_count; + or_ros_state->navigation_ready = this->ros_state.navigation_ready; + or_ros_state->header.stamp = this->now(); + this->state_pub_->publish(std::move(or_ros_state)); + + return; + } + // CAN publisher + auto heartbeat = Compose_DVL_Heartbeat(&this->DVL_heartbeat); + this->can_pub_->publish(std::move( + this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data)))); + + // ROS publisher + driverless_msgs::msg::State::UniquePtr state_msg(new driverless_msgs::msg::State()); + state_msg->header.stamp = this->now(); + state_msg->state = this->DVL_heartbeat.stateID; + state_msg->mission = this->ros_state.mission; + state_msg->lap_count = this->ros_state.lap_count; + state_msg->navigation_ready = this->ros_state.navigation_ready; + this->state_pub_->publish(std::move(state_msg)); +} + +void ASSupervisor::dvl_heartbeat_timer_callback() { + run_fsm(); + publish_heartbeart(); +} + +void ASSupervisor::res_alive_timer_callback() { + if (!this->res_alive) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + RCLCPP_WARN(this->get_logger(), "RES TIMEOUT: Attemping to start RES"); + uint8_t p[8] = {0x80, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + this->can_pub_->publish(std::move(this->_d_2_f(0x00, false, p, sizeof(p)))); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + uint8_t p2[8] = {0x01, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + this->can_pub_->publish(std::move(this->_d_2_f(0x00, false, p2, sizeof(p2)))); + } + this->res_alive = 0; +} + +void ASSupervisor::dataLogger_timer_callback() { + // system status + auto systemStatusMsg = Compose_DVL_SystemStatus(&this->DVL_systemStatus); + this->can_pub_->publish(std::move( + this->_d_2_f(systemStatusMsg.id, false, systemStatusMsg.data, sizeof(systemStatusMsg.data)))); + + driverless_msgs::msg::SystemStatus systemStatus_ROSmsg; + systemStatus_ROSmsg.as_state = this->DVL_systemStatus._fields.AS_state; + systemStatus_ROSmsg.ebs_state = this->DVL_systemStatus._fields.EBS_state; + systemStatus_ROSmsg.ami_state = this->DVL_systemStatus._fields.AMI_state; + systemStatus_ROSmsg.steering_state = this->DVL_systemStatus._fields.steering_state; + systemStatus_ROSmsg.service_brake_state = this->DVL_systemStatus._fields.service_brake_state; + systemStatus_ROSmsg.lap_counter = this->DVL_systemStatus._fields.lap_counter; + systemStatus_ROSmsg.cones_count_actual = this->DVL_systemStatus._fields.cones_count_actual; + systemStatus_ROSmsg.cones_count_all = this->DVL_systemStatus._fields.cones_count_all; + + this->logging_systemStatus_pub_->publish(systemStatus_ROSmsg); + + // very small delay between messages + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + // driving dynamics 1 + auto drivingDynamics1Msg = Compose_DVL_DrivingDynamics1(&this->DVL_drivingDynamics1); + this->can_pub_->publish(std::move( + this->_d_2_f(drivingDynamics1Msg.id, false, drivingDynamics1Msg.data, sizeof(drivingDynamics1Msg.data)))); + + driverless_msgs::msg::DrivingDynamics1 drivingDynamics1_ROSmsg; + drivingDynamics1_ROSmsg.speed_actual = this->DVL_drivingDynamics1._fields.speed_actual; + drivingDynamics1_ROSmsg.speed_target = this->DVL_drivingDynamics1._fields.speed_target; + drivingDynamics1_ROSmsg.steering_angle_actual = this->DVL_drivingDynamics1._fields.steering_angle_actual; + drivingDynamics1_ROSmsg.steering_angle_target = this->DVL_drivingDynamics1._fields.steering_angle_target; + drivingDynamics1_ROSmsg.brake_hydr_actual = this->DVL_drivingDynamics1._fields.brake_hydr_actual; + drivingDynamics1_ROSmsg.brake_hydr_target = this->DVL_drivingDynamics1._fields.brake_hydr_target; + drivingDynamics1_ROSmsg.motor_moment_actual = this->DVL_drivingDynamics1._fields.motor_moment_actual; + drivingDynamics1_ROSmsg.motor_moment_target = this->DVL_drivingDynamics1._fields.motor_moment_target; + + this->logging_drivingDynamics1_pub_->publish(drivingDynamics1_ROSmsg); +} + +void ASSupervisor::run_fsm() { + // by default, no torque + this->DVL_heartbeat.torqueRequest = 0.0; + + // Starting state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_START) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + + // reset mission selections + this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; + this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_NONE; + + // transition to select mission when res switch is backwards + if (!this->RES_status.sw_k2) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_SELECT_MISSION; + } + } + + // Select mission state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_SELECT_MISSION) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_SELECT_MISSION || + this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_WAIT_FOR_MISSION; + } + } + + // Wait for mission state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_WAIT_FOR_MISSION) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { + this->ros_state.mission = this->SW_heartbeat.missionID; + } + + if (this->ros_state.mission != DVL_MISSION::DVL_MISSION_NONE) { + // set mission for logging + if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_INSPECTION) { + this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_INSPECTION; + } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_EBS) { + this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_BRAKETEST; + } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_TRACK) { + this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_TRACKDRIVE; + } else { + this->DVL_systemStatus._fields.AMI_state = 0; + } + + // transition to Check EBS state when mission is selected + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_CHECK_EBS; + + // set the DVL mission IDs according to selection + if (this->ros_state.mission == DVL_MISSION::DVL_MISSION_MANUAL) { + this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_MANUAL; + } else { + this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; + } + } + } + + // Check EBS state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_CHECK_EBS) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_READY && this->RES_status.sw_k2) { + // transition to Ready state when EBS reports EBS checks complete and res switch is forward + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_READY; + } + } + + // Ready state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_READY) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_READY; + + if (this->RES_status.bt_k3) { + // transition to Driving state when RES R2D button is pressed + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_RELEASE_EBS; + } + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { + // transition to E-Stop state when EBS reports shutdown + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } + } + + // Release brake state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_RELEASE_EBS) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_DRIVE) { + // transition to Driving state when brakes r good + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; + } + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { + // transition to E-Stop state when EBS reports shutdown + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } + } + + // Driving state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_DRIVING) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; + + // update torque with last saved value + this->DVL_heartbeat.torqueRequest = this->last_torque; + + // if (this->RES_status.bt_k3) { + // // transition to finished state when RES R2D button is pressed + // this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; + // } + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { + // transition to E-Stop state when EBS reports shutdown + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } + } + + // EBS Activated state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_ACTIVATE_EBS) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; + + // this state activates the EBS without tripping shutdown + // used at end of missions + + // if the EBS says its braking, we go to finished + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_BRAKE_ACTIVATE) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; + } + } + + // Finished state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_FINISHED) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; + + this->DVL_heartbeat.torqueRequest = 0; + if (!this->RES_status.sw_k2) { + // transition to start when RES swtiched back + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; + } + } + + // Emergency state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_EMERGENCY) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_EBRAKE; + + this->DVL_heartbeat.torqueRequest = 0; + if (!this->RES_status.estop && !this->RES_status.sw_k2) { + // transition to start when RES swtiched back + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; + } + } + + // E-stop overrides - these are checked no matter what state we are in + if (this->RES_status.estop) { + // transition to E-Stop state when RES reports E-Stop or loss of signal + this->DVL_heartbeat.torqueRequest = 0; + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } +} + +void ASSupervisor::reset_dataLogger() { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + this->DVL_systemStatus._fields.EBS_state = DVL_EBS_State::DVL_EBS_STATE_UNAVAILABLE; + this->DVL_systemStatus._fields.AMI_state = 0; + this->DVL_systemStatus._fields.steering_state = 0; + this->DVL_systemStatus._fields.service_brake_state = DVL_SERVICE_BRAKE_State::DVL_SERVICE_BRAKE_STATE_AVAILABLE; + this->DVL_systemStatus._fields.lap_counter = 0; + this->DVL_systemStatus._fields.cones_count_actual = 0; + this->DVL_systemStatus._fields.cones_count_all = 0; + + this->DVL_drivingDynamics1._fields.speed_actual = 0; + this->DVL_drivingDynamics1._fields.speed_target = 0; + this->DVL_drivingDynamics1._fields.steering_angle_actual = 0; + this->DVL_drivingDynamics1._fields.steering_angle_target = 0; + this->DVL_drivingDynamics1._fields.brake_hydr_actual = 0; + this->DVL_drivingDynamics1._fields.brake_hydr_target = 0; + this->DVL_drivingDynamics1._fields.motor_moment_actual = 0; + this->DVL_drivingDynamics1._fields.motor_moment_target = 0; +} + +ASSupervisor::ASSupervisor(const rclcpp::NodeOptions & options, std::string name) : Node(name, options) { + // Setup inital states + this->ros_state.state = DVL_STATES::DVL_STATE_START; + this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; + this->ros_state.lap_count = 0; + this->ros_state.navigation_ready = false; + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; + + this->declare_parameter("manual_override", false); + + this->reset_dataLogger(); + + sensor_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + ctrl_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto sensor_opt = rclcpp::SubscriptionOptions(); + sensor_opt.callback_group = sensor_cb_group_; + auto ctrl_opt = rclcpp::SubscriptionOptions(); + ctrl_opt.callback_group = ctrl_cb_group_; + auto timer_opt = rclcpp::SubscriptionOptions(); + timer_opt.callback_group = timer_cb_group_; + + // CAN + this->can_sub_ = this->create_subscription( + "/can/canbus_rosbound", QOS_ALL, std::bind(&ASSupervisor::can_callback, this, _1), sensor_opt); + this->canopen_sub_ = this->create_subscription( + "/can/canopen_rosbound", QOS_ALL, std::bind(&ASSupervisor::canopen_callback, this, _1), sensor_opt); + + // Steering sub + this->steering_angle_sub_ = this->create_subscription( + "/vehicle/steering_angle", QOS_LATEST, std::bind(&ASSupervisor::steering_angle_callback, this, _1), + sensor_opt); + + // Velocity sub + this->velocity_sub_ = this->create_subscription( + "/vehicle/velocity", QOS_LATEST, std::bind(&ASSupervisor::velocity_callback, this, _1), sensor_opt); + + // Control -> sub to acceleration command + this->control_sub_ = this->create_subscription( + "/control/accel_command", QOS_LATEST, std::bind(&ASSupervisor::control_callback, this, _1), ctrl_opt); + + // Lap counter sub + this->lap_sub_ = this->create_subscription( + "/system/laps_completed", QOS_LATEST, std::bind(&ASSupervisor::lap_counter_callback, this, _1), ctrl_opt); + + // steering ready sub + this->steering_ready_sub_ = this->create_subscription( + "/system/steering_ready", QOS_LATEST, std::bind(&ASSupervisor::steering_state_callback, this, _1), + ctrl_opt); + + // Shutdown emergency + this->shutdown_sub_ = this->create_subscription( + "/system/shutdown", 10, std::bind(&ASSupervisor::shutdown_callback, this, _1), timer_opt); + + // AS Heartbeat + this->heartbeat_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), + std::bind(&ASSupervisor::dvl_heartbeat_timer_callback, this), + timer_cb_group_); + + // RES Alive + this->res_alive_timer_ = + this->create_wall_timer(std::chrono::milliseconds(4000), std::bind(&ASSupervisor::res_alive_timer_callback, this), + timer_cb_group_); + + // Data Logger + this->dataLogger_timer_ = + this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&ASSupervisor::dataLogger_timer_callback, this), + timer_cb_group_); + + // Publishers + this->logging_drivingDynamics1_pub_ = + this->create_publisher("/data_logger/drivingDynamics1", 10); + this->logging_systemStatus_pub_ = + this->create_publisher("/data_logger/systemStatus", 10); + + // Outgoing CAN + this->can_pub_ = this->create_publisher("/can/canbus_carbound", 10); + + // State pub + this->state_pub_ = this->create_publisher("/system/as_status", 10); + + // RES status pub + // this->res_status_pub_ = this->create_publisher("/system/res_status", 10); + + RCLCPP_INFO(this->get_logger(), "---Vehicle Supervisor Node Initialised---"); +} + +} // namespace vehicle_supervisor + +RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_supervisor::ASSupervisor); diff --git a/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp b/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp new file mode 100644 index 000000000..b36338119 --- /dev/null +++ b/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp @@ -0,0 +1,201 @@ +#include "component_supervisor_launch.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace vehicle_supervisor { + +void ASSupervisorLaunch::launch_mission() { + // run the mission program based on the mission selected + // command: ros2 run mission_controller MISSION_handler_node + std::string command = "ros2 run mission_controller "; + if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_INSPECTION) { + RCLCPP_INFO(this->get_logger(), "Launching Inspection Mission"); + command += "inspection"; + } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_EBS) { + RCLCPP_INFO(this->get_logger(), "Launching EBS Mission"); + command += "ebs"; + } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_TRACK) { + RCLCPP_INFO(this->get_logger(), "Launching Trackdrive Mission"); + command += "trackdrive"; + } else { + RCLCPP_INFO(this->get_logger(), "Manual driving, no action required"); + } + command += "_handler_node &"; + // run command without blocking (ampersand at end) + system(command.c_str()); + + RCLCPP_INFO(this->get_logger(), "Mission Launched"); +} + +void ASSupervisorLaunch::run_fsm() { + // by default, no torque + this->DVL_heartbeat.torqueRequest = 0.0; + + // Starting state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_START) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + + // reset mission selections + this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; + this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_NONE; + + // transition to select mission when res switch is backwards + if (!this->RES_status.sw_k2) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_SELECT_MISSION; + } + } + + // Select mission state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_SELECT_MISSION) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_SELECT_MISSION || + this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_WAIT_FOR_MISSION; + } + } + + // Wait for mission state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_WAIT_FOR_MISSION) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { + this->ros_state.mission = this->SW_heartbeat.missionID; + ASSupervisorLaunch::launch_mission(); + } + + if (this->ros_state.mission != DVL_MISSION::DVL_MISSION_NONE) { + // set mission for logging + if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_INSPECTION) { + this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_INSPECTION; + } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_EBS) { + this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_BRAKETEST; + } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_TRACK) { + this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_TRACKDRIVE; + } else { + this->DVL_systemStatus._fields.AMI_state = 0; + } + + // transition to Check EBS state when mission is selected + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_CHECK_EBS; + + // set the DVL mission IDs according to selection + if (this->ros_state.mission == DVL_MISSION::DVL_MISSION_MANUAL) { + this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_MANUAL; + } else { + this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; + } + } + } + + // Check EBS state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_CHECK_EBS) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_READY && this->RES_status.sw_k2) { + // transition to Ready state when EBS reports EBS checks complete and res switch is forward + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_READY; + } + } + + // Ready state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_READY) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_READY; + + if (this->RES_status.bt_k3) { + // transition to Driving state when RES R2D button is pressed + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_RELEASE_EBS; + } + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { + // transition to E-Stop state when EBS reports shutdown + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } + } + + // Release brake state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_RELEASE_EBS) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_DRIVE) { + // transition to Driving state when brakes r good + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; + } + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { + // transition to E-Stop state when EBS reports shutdown + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } + } + + // Driving state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_DRIVING) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; + + // update torque with last saved value + this->DVL_heartbeat.torqueRequest = this->last_torque; + + // if (this->RES_status.bt_k3) { + // // transition to finished state when RES R2D button is pressed + // this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; + // } + + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { + // transition to E-Stop state when EBS reports shutdown + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } + } + + // EBS Activated state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_ACTIVATE_EBS) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; + + // this state activates the EBS without tripping shutdown + // used at end of missions + + // if the EBS says its braking, we go to finished + if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_BRAKE_ACTIVATE) { + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; + } + } + + // Finished state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_FINISHED) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; + + this->DVL_heartbeat.torqueRequest = 0; + if (!this->RES_status.sw_k2) { + // transition to start when RES swtiched back + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; + } + } + + // Emergency state + if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_EMERGENCY) { + this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_EBRAKE; + + this->DVL_heartbeat.torqueRequest = 0; + if (!this->RES_status.estop && !this->RES_status.sw_k2) { + // transition to start when RES swtiched back + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; + } + } + + // E-stop overrides - these are checked no matter what state we are in + if (this->RES_status.estop) { + // transition to E-Stop state when RES reports E-Stop or loss of signal + this->DVL_heartbeat.torqueRequest = 0; + this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; + } +} + +void ASSupervisorLaunch::dvl_heartbeat_timer_callback() { + run_fsm(); + publish_heartbeart(); +} + +ASSupervisorLaunch::ASSupervisorLaunch(const rclcpp::NodeOptions & options) : ASSupervisor(options, "vehicle_supervisor_launch_node") { + // override the heartbeat timer + this->heartbeat_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&ASSupervisorLaunch::dvl_heartbeat_timer_callback, this)); +} + +} // namespace vehicle_supervisor + +RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_supervisor::ASSupervisorLaunch) diff --git a/src/hardware/vehicle_supervisor/src/node_supervisor.cpp b/src/hardware/vehicle_supervisor/src/node_supervisor.cpp index ca03fe848..1e8c2e981 100644 --- a/src/hardware/vehicle_supervisor/src/node_supervisor.cpp +++ b/src/hardware/vehicle_supervisor/src/node_supervisor.cpp @@ -1,583 +1,17 @@ -#include - -#include "CAN_DVL.h" -#include "CAN_EBS_CTRL.h" -#include "CAN_RES.h" -#include "CAN_SW.h" -#include "CAN_VCU.h" -#include "QUTMS_can.h" -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" -#include "can_interface.hpp" -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" -#include "driverless_msgs/msg/driving_dynamics1.hpp" -#include "driverless_msgs/msg/res.hpp" -#include "driverless_msgs/msg/shutdown.hpp" -#include "driverless_msgs/msg/state.hpp" -#include "driverless_msgs/msg/system_status.hpp" -#include "nav_msgs/msg/odometry.hpp" +#include "component_supervisor.hpp" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/u_int8.hpp" - -using std::placeholders::_1; - -void copy_data(const std::vector &vec, uint8_t *dest, size_t n) { - for (size_t i = 0; i < n; i++) { - dest[i] = vec[i]; - } -} - -class ASSupervisor : public rclcpp::Node, public CanInterface { - private: - DVL_HeartbeatState_t DVL_heartbeat; - VCU_HeartbeatState_t CTRL_VCU_heartbeat; - EBS_CTRL_HeartbeatState_t EBS_heartbeat; - SW_HeartbeatState_t SW_heartbeat; - DVL_DrivingDynamics1_Data_u DVL_drivingDynamics1; - DVL_SystemStatus_Data_u DVL_systemStatus; - RES_Status_t RES_status; - - driverless_msgs::msg::State ros_state; - - // callback timers - rclcpp::TimerBase::SharedPtr heartbeat_timer_; - rclcpp::TimerBase::SharedPtr res_alive_timer_; - rclcpp::TimerBase::SharedPtr dataLogger_timer_; - - // subscribers - rclcpp::Subscription::SharedPtr can_sub_; - rclcpp::Subscription::SharedPtr canopen_sub_; - rclcpp::Subscription::SharedPtr control_sub_; - rclcpp::Subscription::SharedPtr shutdown_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr lap_sub_; - rclcpp::Subscription::SharedPtr steering_ready_sub_; - - // publishers - rclcpp::Publisher::SharedPtr can_pub_; - rclcpp::Publisher::SharedPtr state_pub_; - rclcpp::Publisher::SharedPtr res_status_pub_; - rclcpp::Publisher::SharedPtr logging_drivingDynamics1_pub_; - rclcpp::Publisher::SharedPtr logging_systemStatus_pub_; - - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_; - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_; - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber3_; - - bool res_alive = 0; - float last_torque = 0; - float last_steering_angle = 0; - float last_velocity = 0; - - void canopen_callback(const driverless_msgs::msg::Can msg) { - // RES boot up - if (msg.id == (RES_BOOT_UP_ID)) { - /* - RES has reported in, request state change to enable it - - Doing so will result in the RES reporting PDOs - 2000 - 20007 every 30ms with the ID 0x180 + RES_NODE_ID - - Byte 0 = state command (start up) - Byte 1 = Node ID (0x00 = All Nodes) - */ - RCLCPP_INFO(this->get_logger(), "RES Booted"); - uint8_t p[8] = {0x01, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - this->can_pub_->publish(this->_d_2_f(0x00, false, p, sizeof(p))); - this->res_alive = 1; - } - // RES heartbeat - else if (msg.id == (RES_HEARTBEAT_ID)) { - // RES Reciever Status Packet - Parse_RES_Heartbeat((uint8_t *)&msg.data[0], &this->RES_status); - // Log RES state - RCLCPP_DEBUG(this->get_logger(), "RES Status: [SW, BT]: %i, %i -- [EST]: %i, -- [RAD_QUAL]: %i", - this->RES_status.sw_k2, this->RES_status.bt_k3, this->RES_status.estop, - this->RES_status.radio_quality); - // publish RES status - driverless_msgs::msg::RES res_msg; - res_msg.sw_k2 = this->RES_status.sw_k2; - res_msg.bt_k3 = this->RES_status.bt_k3; - res_msg.estop = this->RES_status.estop; - res_msg.radio_quality = this->RES_status.radio_quality; - res_msg.loss_of_signal_shutdown_notice = this->RES_status.loss_of_signal_shutdown_notice; - this->res_status_pub_->publish(res_msg); - this->res_alive = 1; - } - } - - void can_callback(const driverless_msgs::msg::Can msg) { - uint32_t qutms_masked_id = msg.id & ~0xF; - - // VCU hearbeat - if (qutms_masked_id == VCU_Heartbeat_ID) { - uint8_t VCU_ID = msg.id & 0xF; - RCLCPP_DEBUG(this->get_logger(), "VCU ID: %u STATE: %02x", VCU_ID, msg.data[0]); - - // data vector to uint8_t array - uint8_t data[8]; - copy_data(msg.data, data, 8); - - // update heartbeat data for this specific VCU - if (VCU_ID == VCU_ID_CTRL) { - Parse_VCU_Heartbeat(data, &this->CTRL_VCU_heartbeat); - this->run_fsm(); - } - } else if (msg.id == EBS_CTRL_Heartbeat_ID) { - uint8_t data[8]; - copy_data(msg.data, data, 8); - - Parse_EBS_CTRL_Heartbeat(data, &this->EBS_heartbeat); - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_DRIVE) { - this->DVL_systemStatus._fields.EBS_state = DVL_EBS_State::DVL_EBS_STATE_ARMED; - } else { - this->DVL_systemStatus._fields.EBS_state = DVL_EBS_State::DVL_EBS_STATE_ACTIVATED; - } - } - // Steering wheel heartbeat - else if (qutms_masked_id == SW_Heartbeat_ID) { - // data vector to uint8_t array - uint8_t data[4]; - copy_data(msg.data, data, 4); - - Parse_SW_Heartbeat(data, &this->SW_heartbeat); - RCLCPP_DEBUG(this->get_logger(), "SW State: %02x Mission Id: %d", this->SW_heartbeat.stateID, - this->SW_heartbeat.missionID); - } - // update state machine following any changes - this->run_fsm(); - } - - void velocity_callback(const std_msgs::msg::Float32 msg) { - this->last_velocity = msg.data; - this->DVL_drivingDynamics1._fields.speed_actual = (int8_t)msg.data; - } - - void steering_angle_callback(const std_msgs::msg::Float32 msg) { - this->last_steering_angle = msg.data; - this->DVL_drivingDynamics1._fields.steering_angle_actual = (int8_t)msg.data; - this->run_fsm(); - } - - void control_callback(const ackermann_msgs::msg::AckermannDriveStamped msg) { - // input range: -1 to 0, 0 to 1 - // torque to car range: 50 to 100 for accel, 40 to 0 for regen - - // compute torque from accel and braking - this->last_torque = 100 * msg.drive.acceleration; - // convert requested accel to estimated motor torque - float torqueValue = msg.drive.acceleration * 9 * 4.5 * 4; - - this->DVL_drivingDynamics1._fields.steering_angle_target = (int8_t)msg.drive.steering_angle; - this->DVL_drivingDynamics1._fields.speed_target = (int8_t)msg.drive.speed; - this->DVL_drivingDynamics1._fields.motor_moment_target = (int8_t)torqueValue; - this->DVL_drivingDynamics1._fields.motor_moment_actual = (int8_t)torqueValue; - } - - void lap_counter_callback(const std_msgs::msg::UInt8 msg) { - this->ros_state.lap_count = msg.data; - this->DVL_systemStatus._fields.lap_counter = msg.data; - } - - void steering_state_callback(const std_msgs::msg::Bool msg) { this->ros_state.navigation_ready = msg.data; } - - void dvl_heartbeat_callback() { - if (this->get_parameter("manual_override").as_bool()) { - // override straight to driving trackdrive - DVL_HeartbeatState_t or_DVL_heartbeat; - or_DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; - or_DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; - auto heartbeat = Compose_DVL_Heartbeat(&or_DVL_heartbeat); - this->can_pub_->publish(this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data))); - - driverless_msgs::msg::State or_ros_state; - or_ros_state.state = or_DVL_heartbeat.stateID; - or_ros_state.mission = DRIVERLESS_MISSIONS::MISSION_TRACK; - or_ros_state.lap_count = this->ros_state.lap_count; - or_ros_state.navigation_ready = this->ros_state.navigation_ready; - or_ros_state.header.stamp = this->now(); - this->state_pub_->publish(or_ros_state); - - return; - } - // CAN publisher - auto heartbeat = Compose_DVL_Heartbeat(&this->DVL_heartbeat); - this->can_pub_->publish(this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data))); - - // ROScube publisher - this->ros_state.header.stamp = this->now(); - this->ros_state.state = this->DVL_heartbeat.stateID; - this->state_pub_->publish(this->ros_state); - } - - void res_alive_callback() { - if (!this->res_alive) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - RCLCPP_WARN(this->get_logger(), "RES TIMEOUT: Attemping to start RES"); - uint8_t p[8] = {0x80, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - this->can_pub_->publish(this->_d_2_f(0x00, false, p, sizeof(p))); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - uint8_t p2[8] = {0x01, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - this->can_pub_->publish(this->_d_2_f(0x00, false, p2, sizeof(p2))); - } - this->res_alive = 0; - this->run_fsm(); - } - - void dataLogger_callback() { - // system status - auto systemStatusMsg = Compose_DVL_SystemStatus(&this->DVL_systemStatus); - this->can_pub_->publish( - this->_d_2_f(systemStatusMsg.id, false, systemStatusMsg.data, sizeof(systemStatusMsg.data))); - - driverless_msgs::msg::SystemStatus systemStatus_ROSmsg; - systemStatus_ROSmsg.as_state = this->DVL_systemStatus._fields.AS_state; - systemStatus_ROSmsg.ebs_state = this->DVL_systemStatus._fields.EBS_state; - systemStatus_ROSmsg.ami_state = this->DVL_systemStatus._fields.AMI_state; - systemStatus_ROSmsg.steering_state = this->DVL_systemStatus._fields.steering_state; - systemStatus_ROSmsg.service_brake_state = this->DVL_systemStatus._fields.service_brake_state; - systemStatus_ROSmsg.lap_counter = this->DVL_systemStatus._fields.lap_counter; - systemStatus_ROSmsg.cones_count_actual = this->DVL_systemStatus._fields.cones_count_actual; - systemStatus_ROSmsg.cones_count_all = this->DVL_systemStatus._fields.cones_count_all; - - this->logging_systemStatus_pub_->publish(systemStatus_ROSmsg); - - // very small delay between messages - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - // driving dynamics 1 - auto drivingDynamics1Msg = Compose_DVL_DrivingDynamics1(&this->DVL_drivingDynamics1); - this->can_pub_->publish( - this->_d_2_f(drivingDynamics1Msg.id, false, drivingDynamics1Msg.data, sizeof(drivingDynamics1Msg.data))); - - driverless_msgs::msg::DrivingDynamics1 drivingDynamics1_ROSmsg; - drivingDynamics1_ROSmsg.speed_actual = this->DVL_drivingDynamics1._fields.speed_actual; - drivingDynamics1_ROSmsg.speed_target = this->DVL_drivingDynamics1._fields.speed_target; - drivingDynamics1_ROSmsg.steering_angle_actual = this->DVL_drivingDynamics1._fields.steering_angle_actual; - drivingDynamics1_ROSmsg.steering_angle_target = this->DVL_drivingDynamics1._fields.steering_angle_target; - drivingDynamics1_ROSmsg.brake_hydr_actual = this->DVL_drivingDynamics1._fields.brake_hydr_actual; - drivingDynamics1_ROSmsg.brake_hydr_target = this->DVL_drivingDynamics1._fields.brake_hydr_target; - drivingDynamics1_ROSmsg.motor_moment_actual = this->DVL_drivingDynamics1._fields.motor_moment_actual; - drivingDynamics1_ROSmsg.motor_moment_target = this->DVL_drivingDynamics1._fields.motor_moment_target; - - this->logging_drivingDynamics1_pub_->publish(drivingDynamics1_ROSmsg); - } - - void shutdown_callback(const driverless_msgs::msg::Shutdown msg) { - if (msg.emergency_shutdown) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - run_fsm(); - } else if (msg.finished_engage_ebs) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_ACTIVATE_EBS; - run_fsm(); - } else if (msg.finished) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; - run_fsm(); - } - } - - void launch_mission() { - // run the mission program based on the mission selected - // command: ros2 run mission_controller MISSION_handler_node - std::string command = "ros2 run mission_controller "; - if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_INSPECTION) { - RCLCPP_INFO(this->get_logger(), "Launching Inspection Mission"); - command += "inspection"; - } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_EBS) { - RCLCPP_INFO(this->get_logger(), "Launching EBS Mission"); - command += "ebs"; - } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_TRACK) { - RCLCPP_INFO(this->get_logger(), "Launching Trackdrive Mission"); - command += "trackdrive"; - } else { - RCLCPP_INFO(this->get_logger(), "Manual driving, no action required"); - } - command += "_handler_node &"; - // run command without blocking (ampersand at end) - system(command.c_str()); - - RCLCPP_INFO(this->get_logger(), "Mission Launched"); - } - - void run_fsm() { - // by default, no torque - this->DVL_heartbeat.torqueRequest = 0.0; - - // Starting state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_START) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - - // reset mission selections - this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; - this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_NONE; - - // transition to select mission when res switch is backwards - if (!this->RES_status.sw_k2) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_SELECT_MISSION; - } - } - - // Select mission state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_SELECT_MISSION) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_SELECT_MISSION || - this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_WAIT_FOR_MISSION; - } - } - - // Wait for mission state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_WAIT_FOR_MISSION) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { - this->ros_state.mission = this->SW_heartbeat.missionID; - launch_mission(); - } - - if (this->ros_state.mission != DVL_MISSION::DVL_MISSION_NONE) { - // set mission for logging - if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_INSPECTION) { - this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_INSPECTION; - } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_EBS) { - this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_BRAKETEST; - } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_TRACK) { - this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_TRACKDRIVE; - } else { - this->DVL_systemStatus._fields.AMI_state = 0; - } - - // transition to Check EBS state when mission is selected - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_CHECK_EBS; - - // set the DVL mission IDs according to selection - if (this->ros_state.mission == DVL_MISSION::DVL_MISSION_MANUAL) { - this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_MANUAL; - } else { - this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; - } - } - } - - // Check EBS state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_CHECK_EBS) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_READY && this->RES_status.sw_k2) { - // transition to Ready state when VCU reports EBS checks complete and res switch is forward - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_READY; - } - } - - // Ready state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_READY) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_READY; - - if (this->RES_status.bt_k3) { - // transition to Driving state when RES R2D button is pressed - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_RELEASE_EBS; - } - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { - // transition to E-Stop state when EBS VCU reports shutdown - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - // Release brake state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_RELEASE_EBS) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_DRIVE) { - // transition to Driving state when brakes r good - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; - } - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { - // transition to E-Stop state when EBS VCU reports shutdown - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - // Driving state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_DRIVING) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; - - // update torque with last saved value - this->DVL_heartbeat.torqueRequest = this->last_torque; - - // if (this->RES_status.bt_k3) { - // // transition to finished state when RES R2D button is pressed - // this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; - // } - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { - // transition to E-Stop state when EBS VCU reports shutdown - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - // EBS Activated state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_ACTIVATE_EBS) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; - - // this state activates the EBS without tripping shutdown - // used at end of missions - - // if the vcu says its braking, we go to finished - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_BRAKE_ACTIVATE) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; - } - } - - // Finished state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_FINISHED) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; - - this->DVL_heartbeat.torqueRequest = 0; - if (!this->RES_status.sw_k2) { - // transition to start when RES swtiched back - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; - } - } - - // Emergency state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_EMERGENCY) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_EBRAKE; - - this->DVL_heartbeat.torqueRequest = 0; - if (!this->RES_status.estop && !this->RES_status.sw_k2) { - // transition to start when RES swtiched back - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; - } - } - - // E-stop overrides - these are checked no matter what state we are in - if (this->RES_status.estop) { - // transition to E-Stop state when RES reports E-Stop or loss of signal - this->DVL_heartbeat.torqueRequest = 0; - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - void reset_dataLogger() { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - this->DVL_systemStatus._fields.EBS_state = DVL_EBS_State::DVL_EBS_STATE_UNAVAILABLE; - this->DVL_systemStatus._fields.AMI_state = 0; - this->DVL_systemStatus._fields.steering_state = 0; - this->DVL_systemStatus._fields.service_brake_state = DVL_SERVICE_BRAKE_State::DVL_SERVICE_BRAKE_STATE_AVAILABLE; - this->DVL_systemStatus._fields.lap_counter = 0; - this->DVL_systemStatus._fields.cones_count_actual = 0; - this->DVL_systemStatus._fields.cones_count_all = 0; - - this->DVL_drivingDynamics1._fields.speed_actual = 0; - this->DVL_drivingDynamics1._fields.speed_target = 0; - this->DVL_drivingDynamics1._fields.steering_angle_actual = 0; - this->DVL_drivingDynamics1._fields.steering_angle_target = 0; - this->DVL_drivingDynamics1._fields.brake_hydr_actual = 0; - this->DVL_drivingDynamics1._fields.brake_hydr_target = 0; - this->DVL_drivingDynamics1._fields.motor_moment_actual = 0; - this->DVL_drivingDynamics1._fields.motor_moment_target = 0; - } - - public: - ASSupervisor() : Node("vehicle_supervisor_node") { - // Setup inital states - this->ros_state.state = DVL_STATES::DVL_STATE_START; - this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; - this->ros_state.lap_count = 0; - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; - - this->declare_parameter("manual_override", false); - - this->reset_dataLogger(); - - callback_group_subscriber1_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_subscriber2_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_subscriber3_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto sub1_opt = rclcpp::SubscriptionOptions(); - sub1_opt.callback_group = callback_group_subscriber1_; - auto sub2_opt = rclcpp::SubscriptionOptions(); - sub2_opt.callback_group = callback_group_subscriber2_; - auto sub3_opt = rclcpp::SubscriptionOptions(); - sub3_opt.callback_group = callback_group_subscriber3_; - - // CAN - this->can_sub_ = this->create_subscription( - "/can/canbus_rosbound", QOS_ALL, std::bind(&ASSupervisor::can_callback, this, _1), sub1_opt); - this->canopen_sub_ = this->create_subscription( - "/can/canopen_rosbound", QOS_ALL, std::bind(&ASSupervisor::canopen_callback, this, _1), sub1_opt); - - // Steering sub - this->steering_angle_sub_ = this->create_subscription( - "/vehicle/steering_angle", QOS_LATEST, std::bind(&ASSupervisor::steering_angle_callback, this, _1), - sub1_opt); - - // Velocity sub - this->velocity_sub_ = this->create_subscription( - "/vehicle/velocity", QOS_LATEST, std::bind(&ASSupervisor::velocity_callback, this, _1), sub1_opt); - - // Control -> sub to acceleration command - this->control_sub_ = this->create_subscription( - "/control/accel_command", QOS_LATEST, std::bind(&ASSupervisor::control_callback, this, _1), sub2_opt); - - // Lap counter sub - this->lap_sub_ = this->create_subscription( - "/system/laps_completed", QOS_LATEST, std::bind(&ASSupervisor::lap_counter_callback, this, _1), sub2_opt); - - // steering ready sub - this->steering_ready_sub_ = this->create_subscription( - "/system/steering_ready", QOS_LATEST, std::bind(&ASSupervisor::steering_state_callback, this, _1), - sub2_opt); - - // AS Heartbeat - this->heartbeat_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), - std::bind(&ASSupervisor::dvl_heartbeat_callback, this), - callback_group_subscriber3_); - - // RES Alive - this->res_alive_timer_ = - this->create_wall_timer(std::chrono::milliseconds(4000), std::bind(&ASSupervisor::res_alive_callback, this), - callback_group_subscriber3_); - - // Data Logger - this->dataLogger_timer_ = - this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&ASSupervisor::dataLogger_callback, this), - callback_group_subscriber3_); - this->logging_drivingDynamics1_pub_ = - this->create_publisher("/data_logger/drivingDynamics1", 10); - this->logging_systemStatus_pub_ = - this->create_publisher("/data_logger/systemStatus", 10); - - // Shutdown emergency - this->shutdown_sub_ = this->create_subscription( - "/system/shutdown", 10, std::bind(&ASSupervisor::shutdown_callback, this, _1)); - - // Outgoing CAN - this->can_pub_ = this->create_publisher("/can/canbus_carbound", 10); - - // State pub - this->state_pub_ = this->create_publisher("/system/as_status", 10); - - // RES status pub - this->res_status_pub_ = this->create_publisher("/system/res_status", 10); - - RCLCPP_INFO(this->get_logger(), "---Vehicle Supervisor Node Initialised---"); - } -}; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + + rclcpp::NodeOptions options; + auto node = std::make_shared(options); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); executor.spin(); - // rclcpp::spin(node); - // rclcpp::shutdown(); + rclcpp::shutdown(); + return 0; } diff --git a/src/hardware/vehicle_supervisor/src/node_supervisor_launch.cpp b/src/hardware/vehicle_supervisor/src/node_supervisor_launch.cpp new file mode 100644 index 000000000..894adc796 --- /dev/null +++ b/src/hardware/vehicle_supervisor/src/node_supervisor_launch.cpp @@ -0,0 +1,17 @@ +#include "component_supervisor_launch.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/src/hardware/vehicle_supervisor/src/node_supervisor_slim.cpp b/src/hardware/vehicle_supervisor/src/node_supervisor_slim.cpp deleted file mode 100644 index d6e6050bf..000000000 --- a/src/hardware/vehicle_supervisor/src/node_supervisor_slim.cpp +++ /dev/null @@ -1,559 +0,0 @@ -#include - -#include "CAN_DVL.h" -#include "CAN_EBS_CTRL.h" -#include "CAN_RES.h" -#include "CAN_SW.h" -#include "CAN_VCU.h" -#include "QUTMS_can.h" -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" -#include "can_interface.hpp" -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" -#include "driverless_msgs/msg/driving_dynamics1.hpp" -#include "driverless_msgs/msg/res.hpp" -#include "driverless_msgs/msg/shutdown.hpp" -#include "driverless_msgs/msg/state.hpp" -#include "driverless_msgs/msg/system_status.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/u_int8.hpp" - -using std::placeholders::_1; - -void copy_data(const std::vector &vec, uint8_t *dest, size_t n) { - for (size_t i = 0; i < n; i++) { - dest[i] = vec[i]; - } -} - -class ASSupervisor : public rclcpp::Node, public CanInterface { - private: - DVL_HeartbeatState_t DVL_heartbeat; - VCU_HeartbeatState_t CTRL_VCU_heartbeat; - EBS_CTRL_HeartbeatState_t EBS_heartbeat; - SW_HeartbeatState_t SW_heartbeat; - DVL_DrivingDynamics1_Data_u DVL_drivingDynamics1; - DVL_SystemStatus_Data_u DVL_systemStatus; - RES_Status_t RES_status; - - driverless_msgs::msg::State ros_state; - - // callback timers - rclcpp::TimerBase::SharedPtr heartbeat_timer_; - rclcpp::TimerBase::SharedPtr res_alive_timer_; - rclcpp::TimerBase::SharedPtr dataLogger_timer_; - - // subscribers - rclcpp::Subscription::SharedPtr can_sub_; - rclcpp::Subscription::SharedPtr canopen_sub_; - rclcpp::Subscription::SharedPtr control_sub_; - rclcpp::Subscription::SharedPtr shutdown_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr lap_sub_; - rclcpp::Subscription::SharedPtr steering_ready_sub_; - - // publishers - rclcpp::Publisher::SharedPtr can_pub_; - rclcpp::Publisher::SharedPtr state_pub_; - rclcpp::Publisher::SharedPtr res_status_pub_; - rclcpp::Publisher::SharedPtr logging_drivingDynamics1_pub_; - rclcpp::Publisher::SharedPtr logging_systemStatus_pub_; - - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_; - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_; - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber3_; - - bool res_alive = 0; - float last_torque = 0; - float last_steering_angle = 0; - float last_velocity = 0; - - void canopen_callback(const driverless_msgs::msg::Can msg) { - // RES boot up - if (msg.id == (RES_BOOT_UP_ID)) { - /* - RES has reported in, request state change to enable it - - Doing so will result in the RES reporting PDOs - 2000 - 20007 every 30ms with the ID 0x180 + RES_NODE_ID - - Byte 0 = state command (start up) - Byte 1 = Node ID (0x00 = All Nodes) - */ - RCLCPP_INFO(this->get_logger(), "RES Booted"); - uint8_t p[8] = {0x01, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - this->can_pub_->publish(this->_d_2_f(0x00, false, p, sizeof(p))); - this->res_alive = 1; - } - // RES heartbeat - else if (msg.id == (RES_HEARTBEAT_ID)) { - // RES Reciever Status Packet - Parse_RES_Heartbeat((uint8_t *)&msg.data[0], &this->RES_status); - // Log RES state - RCLCPP_DEBUG(this->get_logger(), "RES Status: [SW, BT]: %i, %i -- [EST]: %i, -- [RAD_QUAL]: %i", - this->RES_status.sw_k2, this->RES_status.bt_k3, this->RES_status.estop, - this->RES_status.radio_quality); - // publish RES status - driverless_msgs::msg::RES res_msg; - res_msg.sw_k2 = this->RES_status.sw_k2; - res_msg.bt_k3 = this->RES_status.bt_k3; - res_msg.estop = this->RES_status.estop; - res_msg.radio_quality = this->RES_status.radio_quality; - res_msg.loss_of_signal_shutdown_notice = this->RES_status.loss_of_signal_shutdown_notice; - this->res_status_pub_->publish(res_msg); - this->res_alive = 1; - } - } - - void can_callback(const driverless_msgs::msg::Can msg) { - uint32_t qutms_masked_id = msg.id & ~0xF; - - // VCU hearbeat - if (qutms_masked_id == VCU_Heartbeat_ID) { - uint8_t VCU_ID = msg.id & 0xF; - RCLCPP_DEBUG(this->get_logger(), "VCU ID: %u STATE: %02x", VCU_ID, msg.data[0]); - - // data vector to uint8_t array - uint8_t data[8]; - copy_data(msg.data, data, 8); - - // update heartbeat data for this specific VCU - if (VCU_ID == VCU_ID_CTRL) { - Parse_VCU_Heartbeat(data, &this->CTRL_VCU_heartbeat); - this->run_fsm(); - } - } else if (msg.id == EBS_CTRL_Heartbeat_ID) { - uint8_t data[8]; - copy_data(msg.data, data, 8); - - Parse_EBS_CTRL_Heartbeat(data, &this->EBS_heartbeat); - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE_DRIVE) { - this->DVL_systemStatus._fields.EBS_state = DVL_EBS_STATE_ARMED; - } else { - this->DVL_systemStatus._fields.EBS_state = DVL_EBS_STATE_ACTIVATED; - } - } - // Steering wheel heartbeat - else if (qutms_masked_id == SW_Heartbeat_ID) { - // data vector to uint8_t array - uint8_t data[4]; - copy_data(msg.data, data, 4); - - Parse_SW_Heartbeat(data, &this->SW_heartbeat); - RCLCPP_DEBUG(this->get_logger(), "SW State: %02x Mission Id: %d", this->SW_heartbeat.stateID, - this->SW_heartbeat.missionID); - } - // update state machine following any changes - this->run_fsm(); - } - - void velocity_callback(const std_msgs::msg::Float32 msg) { - this->last_velocity = msg.data; - this->DVL_drivingDynamics1._fields.speed_actual = (int8_t)msg.data; - } - - void steering_angle_callback(const std_msgs::msg::Float32 msg) { - this->last_steering_angle = msg.data; - this->DVL_drivingDynamics1._fields.steering_angle_actual = (int8_t)msg.data; - this->run_fsm(); - } - - void control_callback(const ackermann_msgs::msg::AckermannDriveStamped msg) { - // input range: -1 to 0, 0 to 1 - // torque to car range: 50 to 100 for accel, 40 to 0 for regen - - // compute torque from accel and braking - this->last_torque = 100 * msg.drive.acceleration; - // convert requested accel to estimated motor torque - float torqueValue = msg.drive.acceleration * 9 * 4.5 * 4; - - this->DVL_drivingDynamics1._fields.steering_angle_target = (int8_t)msg.drive.steering_angle; - this->DVL_drivingDynamics1._fields.speed_target = (int8_t)msg.drive.speed; - this->DVL_drivingDynamics1._fields.motor_moment_target = (int8_t)torqueValue; - this->DVL_drivingDynamics1._fields.motor_moment_actual = (int8_t)torqueValue; - } - - void lap_counter_callback(const std_msgs::msg::UInt8 msg) { - this->ros_state.lap_count = msg.data; - this->DVL_systemStatus._fields.lap_counter = msg.data; - } - - void steering_state_callback(const std_msgs::msg::Bool msg) { this->ros_state.navigation_ready = msg.data; } - - void dvl_heartbeat_callback() { - if (this->get_parameter("manual_override").as_bool()) { - // override straight to driving trackdrive - DVL_HeartbeatState_t or_DVL_heartbeat; - or_DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; - or_DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; - auto heartbeat = Compose_DVL_Heartbeat(&or_DVL_heartbeat); - this->can_pub_->publish(this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data))); - - driverless_msgs::msg::State or_ros_state; - or_ros_state.state = or_DVL_heartbeat.stateID; - or_ros_state.mission = DRIVERLESS_MISSIONS::MISSION_TRACK; - or_ros_state.lap_count = this->ros_state.lap_count; - or_ros_state.navigation_ready = this->ros_state.navigation_ready; - or_ros_state.header.stamp = this->now(); - this->state_pub_->publish(or_ros_state); - - return; - } - // CAN publisher - auto heartbeat = Compose_DVL_Heartbeat(&this->DVL_heartbeat); - this->can_pub_->publish(this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data))); - - // ROScube publisher - this->ros_state.header.stamp = this->now(); - this->ros_state.state = this->DVL_heartbeat.stateID; - this->state_pub_->publish(this->ros_state); - } - - void res_alive_callback() { - if (!this->res_alive) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - RCLCPP_WARN(this->get_logger(), "RES TIMEOUT: Attemping to start RES"); - uint8_t p[8] = {0x80, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - this->can_pub_->publish(this->_d_2_f(0x00, false, p, sizeof(p))); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - uint8_t p2[8] = {0x01, RES_NODE_ID, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - this->can_pub_->publish(this->_d_2_f(0x00, false, p2, sizeof(p2))); - } - this->res_alive = 0; - this->run_fsm(); - } - - void dataLogger_callback() { - // system status - auto systemStatusMsg = Compose_DVL_SystemStatus(&this->DVL_systemStatus); - this->can_pub_->publish( - this->_d_2_f(systemStatusMsg.id, false, systemStatusMsg.data, sizeof(systemStatusMsg.data))); - - driverless_msgs::msg::SystemStatus systemStatus_ROSmsg; - systemStatus_ROSmsg.as_state = this->DVL_systemStatus._fields.AS_state; - systemStatus_ROSmsg.ebs_state = this->DVL_systemStatus._fields.EBS_state; - systemStatus_ROSmsg.ami_state = this->DVL_systemStatus._fields.AMI_state; - systemStatus_ROSmsg.steering_state = this->DVL_systemStatus._fields.steering_state; - systemStatus_ROSmsg.service_brake_state = this->DVL_systemStatus._fields.service_brake_state; - systemStatus_ROSmsg.lap_counter = this->DVL_systemStatus._fields.lap_counter; - systemStatus_ROSmsg.cones_count_actual = this->DVL_systemStatus._fields.cones_count_actual; - systemStatus_ROSmsg.cones_count_all = this->DVL_systemStatus._fields.cones_count_all; - - this->logging_systemStatus_pub_->publish(systemStatus_ROSmsg); - - // very small delay between messages - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - // driving dynamics 1 - auto drivingDynamics1Msg = Compose_DVL_DrivingDynamics1(&this->DVL_drivingDynamics1); - this->can_pub_->publish( - this->_d_2_f(drivingDynamics1Msg.id, false, drivingDynamics1Msg.data, sizeof(drivingDynamics1Msg.data))); - - driverless_msgs::msg::DrivingDynamics1 drivingDynamics1_ROSmsg; - drivingDynamics1_ROSmsg.speed_actual = this->DVL_drivingDynamics1._fields.speed_actual; - drivingDynamics1_ROSmsg.speed_target = this->DVL_drivingDynamics1._fields.speed_target; - drivingDynamics1_ROSmsg.steering_angle_actual = this->DVL_drivingDynamics1._fields.steering_angle_actual; - drivingDynamics1_ROSmsg.steering_angle_target = this->DVL_drivingDynamics1._fields.steering_angle_target; - drivingDynamics1_ROSmsg.brake_hydr_actual = this->DVL_drivingDynamics1._fields.brake_hydr_actual; - drivingDynamics1_ROSmsg.brake_hydr_target = this->DVL_drivingDynamics1._fields.brake_hydr_target; - drivingDynamics1_ROSmsg.motor_moment_actual = this->DVL_drivingDynamics1._fields.motor_moment_actual; - drivingDynamics1_ROSmsg.motor_moment_target = this->DVL_drivingDynamics1._fields.motor_moment_target; - - this->logging_drivingDynamics1_pub_->publish(drivingDynamics1_ROSmsg); - } - - void shutdown_callback(const driverless_msgs::msg::Shutdown msg) { - if (msg.emergency_shutdown) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - run_fsm(); - } else if (msg.finished_engage_ebs) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_ACTIVATE_EBS; - run_fsm(); - } else if (msg.finished) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; - run_fsm(); - } - } - - void run_fsm() { - // by default, no torque - this->DVL_heartbeat.torqueRequest = 0.0; - - // Starting state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_START) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - - // reset mission selections - this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; - this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_NONE; - - // transition to select mission when res switch is backwards - if (!this->RES_status.sw_k2) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_SELECT_MISSION; - } - } - - // Select mission state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_SELECT_MISSION) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_SELECT_MISSION || - this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_WAIT_FOR_MISSION; - } - } - - // Wait for mission state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_WAIT_FOR_MISSION) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - if (this->SW_heartbeat.stateID == sw_state_t::SW_STATE_MISSION_ACK) { - this->ros_state.mission = this->SW_heartbeat.missionID; - } - - if (this->ros_state.mission != DVL_MISSION::DVL_MISSION_NONE) { - // set mission for logging - if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_INSPECTION) { - this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_INSPECTION; - } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_EBS) { - this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_BRAKETEST; - } else if (this->ros_state.mission == DRIVERLESS_MISSIONS::MISSION_TRACK) { - this->DVL_systemStatus._fields.AMI_state = DVL_AMI_State::DVL_AMI_STATE_TRACKDRIVE; - } else { - this->DVL_systemStatus._fields.AMI_state = 0; - } - - // transition to Check EBS state when mission is selected - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_CHECK_EBS; - - // set the DVL mission IDs according to selection - if (this->ros_state.mission == DVL_MISSION::DVL_MISSION_MANUAL) { - this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_MANUAL; - } else { - this->DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; - } - } - } - - // Check EBS state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_CHECK_EBS) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_READY && this->RES_status.sw_k2) { - // transition to Ready state when VCU reports EBS checks complete and res switch is forward - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_READY; - } - } - - // Ready state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_READY) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_READY; - - if (this->RES_status.bt_k3) { - // transition to Driving state when RES R2D button is pressed - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_RELEASE_EBS; - } - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { - // transition to E-Stop state when EBS VCU reports shutdown - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - // Release brake state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_RELEASE_EBS) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_DRIVE) { - // transition to Driving state when brakes r good - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; - } - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { - // transition to E-Stop state when EBS VCU reports shutdown - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - // Driving state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_DRIVING) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_DRIVING; - - // update torque with last saved value - this->DVL_heartbeat.torqueRequest = this->last_torque; - - // if (this->RES_status.bt_k3) { - // // transition to finished state when RES R2D button is pressed - // this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; - // } - - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_SHUTDOWN) { - // transition to E-Stop state when EBS VCU reports shutdown - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - // EBS Activated state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_ACTIVATE_EBS) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; - - // this state activates the EBS without tripping shutdown - // used at end of missions - - // if the vcu says its braking, we go to finished - if (this->EBS_heartbeat.stateID == EBS_CTRL_STATE::EBS_CTRL_STATE_BRAKE_ACTIVATE) { - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_FINISHED; - } - } - - // Finished state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_FINISHED) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_FINISH; - - this->DVL_heartbeat.torqueRequest = 0; - if (!this->RES_status.sw_k2) { - // transition to start when RES swtiched back - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; - } - } - - // Emergency state - if (this->DVL_heartbeat.stateID == DVL_STATES::DVL_STATE_EMERGENCY) { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_EBRAKE; - - this->DVL_heartbeat.torqueRequest = 0; - if (!this->RES_status.estop && !this->RES_status.sw_k2) { - // transition to start when RES swtiched back - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; - } - } - - // E-stop overrides - these are checked no matter what state we are in - if (this->RES_status.estop) { - // transition to E-Stop state when RES reports E-Stop or loss of signal - this->DVL_heartbeat.torqueRequest = 0; - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_EMERGENCY; - } - } - - void reset_dataLogger() { - this->DVL_systemStatus._fields.AS_state = DVL_AS_State::DVL_AS_STATE_OFF; - this->DVL_systemStatus._fields.EBS_state = DVL_EBS_State::DVL_EBS_STATE_UNAVAILABLE; - this->DVL_systemStatus._fields.AMI_state = 0; - this->DVL_systemStatus._fields.steering_state = 0; - this->DVL_systemStatus._fields.service_brake_state = DVL_SERVICE_BRAKE_State::DVL_SERVICE_BRAKE_STATE_AVAILABLE; - this->DVL_systemStatus._fields.lap_counter = 0; - this->DVL_systemStatus._fields.cones_count_actual = 0; - this->DVL_systemStatus._fields.cones_count_all = 0; - - this->DVL_drivingDynamics1._fields.speed_actual = 0; - this->DVL_drivingDynamics1._fields.speed_target = 0; - this->DVL_drivingDynamics1._fields.steering_angle_actual = 0; - this->DVL_drivingDynamics1._fields.steering_angle_target = 0; - this->DVL_drivingDynamics1._fields.brake_hydr_actual = 0; - this->DVL_drivingDynamics1._fields.brake_hydr_target = 0; - this->DVL_drivingDynamics1._fields.motor_moment_actual = 0; - this->DVL_drivingDynamics1._fields.motor_moment_target = 0; - } - - public: - ASSupervisor() : Node("vehicle_supervisor_node") { - // Setup inital states - this->ros_state.state = DVL_STATES::DVL_STATE_START; - this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; - this->ros_state.lap_count = 0; - this->DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_START; - - this->declare_parameter("manual_override", false); - - this->reset_dataLogger(); - - callback_group_subscriber1_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_subscriber2_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_subscriber3_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto sub1_opt = rclcpp::SubscriptionOptions(); - sub1_opt.callback_group = callback_group_subscriber1_; - auto sub2_opt = rclcpp::SubscriptionOptions(); - sub2_opt.callback_group = callback_group_subscriber2_; - auto sub3_opt = rclcpp::SubscriptionOptions(); - sub3_opt.callback_group = callback_group_subscriber3_; - - // CAN - this->can_sub_ = this->create_subscription( - "/can/canbus_rosbound", QOS_ALL, std::bind(&ASSupervisor::can_callback, this, _1), sub1_opt); - this->canopen_sub_ = this->create_subscription( - "/can/canopen_rosbound", QOS_ALL, std::bind(&ASSupervisor::canopen_callback, this, _1), sub1_opt); - - // Steering sub - this->steering_angle_sub_ = this->create_subscription( - "/vehicle/steering_angle", QOS_LATEST, std::bind(&ASSupervisor::steering_angle_callback, this, _1), - sub1_opt); - - // Velocity sub - this->velocity_sub_ = this->create_subscription( - "/vehicle/velocity", QOS_LATEST, std::bind(&ASSupervisor::velocity_callback, this, _1), sub1_opt); - - // Control -> sub to acceleration command - this->control_sub_ = this->create_subscription( - "/control/accel_command", QOS_LATEST, std::bind(&ASSupervisor::control_callback, this, _1), sub2_opt); - - // Lap counter sub - this->lap_sub_ = this->create_subscription( - "/system/laps_completed", QOS_LATEST, std::bind(&ASSupervisor::lap_counter_callback, this, _1), sub2_opt); - - // steering ready sub - this->steering_ready_sub_ = this->create_subscription( - "/system/steering_ready", QOS_LATEST, std::bind(&ASSupervisor::steering_state_callback, this, _1), - sub2_opt); - - // AS Heartbeat - this->heartbeat_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), - std::bind(&ASSupervisor::dvl_heartbeat_callback, this), - callback_group_subscriber3_); - - // RES Alive - this->res_alive_timer_ = - this->create_wall_timer(std::chrono::milliseconds(4000), std::bind(&ASSupervisor::res_alive_callback, this), - callback_group_subscriber3_); - - // Data Logger - this->dataLogger_timer_ = - this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&ASSupervisor::dataLogger_callback, this), - callback_group_subscriber3_); - this->logging_drivingDynamics1_pub_ = - this->create_publisher("/data_logger/drivingDynamics1", 10); - this->logging_systemStatus_pub_ = - this->create_publisher("/data_logger/systemStatus", 10); - - // Shutdown emergency - this->shutdown_sub_ = this->create_subscription( - "/system/shutdown", 10, std::bind(&ASSupervisor::shutdown_callback, this, _1)); - - // Outgoing CAN - this->can_pub_ = this->create_publisher("/can/canbus_carbound", 10); - - // State pub - this->state_pub_ = this->create_publisher("/system/as_status", 10); - - // RES status pub - this->res_status_pub_ = this->create_publisher("/system/res_status", 10); - - RCLCPP_INFO(this->get_logger(), "---Vehicle Supervisor Node Initialised---"); - } -}; - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - - // rclcpp::spin(node); - // rclcpp::shutdown(); - return 0; -} From 31c9b79f7e8e25f18d7a2a2209f62567b5f453bd Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:52:34 +1000 Subject: [PATCH 4/8] compose steering actuator. remove old node versions. --- src/hardware/steering_actuator/CMakeLists.txt | 46 +- ...on.hpp => component_steering_actuator.hpp} | 82 ++- .../launch/machine.launch.py | 2 +- .../src/component_steering_actuator.cpp | 288 ++++++++++ .../src/node_steering_actuator.cpp | 441 +-------------- .../src/node_steering_actuator_old.cpp | 524 ------------------ .../src/node_steering_actuator_test.cpp | 376 ------------- 7 files changed, 407 insertions(+), 1352 deletions(-) rename src/hardware/steering_actuator/include/{steering_actuator/steering_common.hpp => component_steering_actuator.hpp} (52%) create mode 100644 src/hardware/steering_actuator/src/component_steering_actuator.cpp delete mode 100644 src/hardware/steering_actuator/src/node_steering_actuator_old.cpp delete mode 100644 src/hardware/steering_actuator/src/node_steering_actuator_test.cpp diff --git a/src/hardware/steering_actuator/CMakeLists.txt b/src/hardware/steering_actuator/CMakeLists.txt index aa2bc0a4a..6fbaadf3f 100644 --- a/src/hardware/steering_actuator/CMakeLists.txt +++ b/src/hardware/steering_actuator/CMakeLists.txt @@ -5,38 +5,50 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. find_package(ackermann_msgs REQUIRED) find_package(driverless_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) -include_directories(include) -include_directories(${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/include/) -include_directories(${CMAKE_SOURCE_DIR}/../../common/driverless_common/include) +include_directories( + include + ${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/include/ + ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include +) set (SOURCES ${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/src/canopen.cpp ${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/src/can_interface.cpp ) -add_executable(steering_actuator_node src/node_steering_actuator.cpp ${SOURCES}) -add_executable(steering_actuator_test_node src/node_steering_actuator_test.cpp ${SOURCES}) +# ROS 2 components +add_library(steering_actuator_component SHARED src/component_steering_actuator.cpp ${SOURCES}) +ament_target_dependencies(steering_actuator_component + "rclcpp" + "rclcpp_components" + "std_msgs" + "driverless_msgs" + "ackermann_msgs" +) +rclcpp_components_register_nodes(steering_actuator_component "steering_actuator::SteeringActuator") -ament_target_dependencies(steering_actuator_node rclcpp ackermann_msgs driverless_msgs std_msgs) -ament_target_dependencies(steering_actuator_test_node rclcpp ackermann_msgs driverless_msgs std_msgs) +# ROS 2 nodes +add_executable(steering_actuator_node src/node_steering_actuator.cpp) +target_link_libraries(steering_actuator_node steering_actuator_component) +ament_target_dependencies(steering_actuator_node + "rclcpp" +) -target_include_directories(steering_actuator_node PUBLIC - $ - $) -target_include_directories(steering_actuator_test_node PUBLIC - $ - $) +install(TARGETS + steering_actuator_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) - install(TARGETS steering_actuator_node steering_actuator_test_node +install(TARGETS + steering_actuator_node DESTINATION lib/${PROJECT_NAME}) # config folder diff --git a/src/hardware/steering_actuator/include/steering_actuator/steering_common.hpp b/src/hardware/steering_actuator/include/component_steering_actuator.hpp similarity index 52% rename from src/hardware/steering_actuator/include/steering_actuator/steering_common.hpp rename to src/hardware/steering_actuator/include/component_steering_actuator.hpp index 2efceccd6..14d9a3e6e 100644 --- a/src/hardware/steering_actuator/include/steering_actuator/steering_common.hpp +++ b/src/hardware/steering_actuator/include/component_steering_actuator.hpp @@ -1,9 +1,29 @@ +#ifndef STEERING_ACTUATOR__COMPONENT_STEERING_ACTUATOR_HPP_ +#define STEERING_ACTUATOR__COMPONENT_STEERING_ACTUATOR_HPP_ + +#include +#include +#include #include #include - #include #include +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "can_interface.hpp" +#include "canopen.hpp" +#include "driverless_common/common.hpp" +#include "driverless_msgs/msg/can.hpp" +#include "driverless_msgs/msg/float32_stamped.hpp" +#include "driverless_msgs/msg/int32_stamped.hpp" +#include "driverless_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + +using std::placeholders::_1; + +namespace steering_actuator { + typedef enum c5e_object_id { HOME_OFFSET = 0x607C, MOTION_PROFILE_TYPE = 0x6086, @@ -107,3 +127,63 @@ c5e_state parse_state(uint16_t status_word) { } return states[F]; } + +class SteeringActuator : public rclcpp::Node, public CanInterface { + private: + // Creates + rclcpp::TimerBase::SharedPtr steering_update_timer_; + rclcpp::TimerBase::SharedPtr state_request_timer_; + + // Creates publishers and subscribers + rclcpp::Publisher::SharedPtr can_pub_; + rclcpp::Publisher::SharedPtr encoder_pub_; + rclcpp::Publisher::SharedPtr steering_ready_pub_; + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr ackermann_sub_; + rclcpp::Subscription::SharedPtr steer_ang_sub_; + rclcpp::Subscription::SharedPtr canopen_sub_; + + rclcpp::CallbackGroup::SharedPtr sensor_cb_group_; + rclcpp::CallbackGroup::SharedPtr control_cb_group_; + + // params + double Kp_, Ki_, Kd_, centre_range_, centre_angle_; + int settling_iter_, max_position_; + + c5e_state desired_state_ = states[RTSO]; + c5e_state current_state_ = states[NRTSO]; + uint16_t control_method_ = MODE_RELATIVE; + bool motor_enabled_ = false; + bool centred_ = false; + int32_t offset_ = 0; + int settled_count_ = 0; + bool initial_enc_saved_ = false; + int32_t initial_enc_ = 0; + bool steering_ang_received_ = false; + double current_steering_angle_ = 0; + int32_t current_enc_revolutions_ = 0; // Current Encoder Revolutions (Stepper encoder) + uint32_t current_velocity_; + uint32_t current_acceleration_; + + // time to reset node if no state received + std::chrono::time_point last_state_time = std::chrono::system_clock::now(); + + void configure_c5e(); + void c5e_state_request_callback(); + void as_state_callback(const driverless_msgs::msg::State::SharedPtr msg); + void steering_angle_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg); + void driving_command_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg); + void can_callback(const driverless_msgs::msg::Can::SharedPtr msg); + + void pre_op_centering(); + void target_position(int32_t target); + void send_steering_data(uint16_t obj_index, uint8_t *data, size_t data_size); + void read_steering_data(uint16_t obj_index); + + public: + SteeringActuator(const rclcpp::NodeOptions &options); +}; + +} // namespace steering_actuator + +#endif // STEERING_ACTUATOR__COMPONENT_STEERING_ACTUATOR_HPP_ diff --git a/src/hardware/steering_actuator/launch/machine.launch.py b/src/hardware/steering_actuator/launch/machine.launch.py index 0164c37d4..94df7e48f 100644 --- a/src/hardware/steering_actuator/launch/machine.launch.py +++ b/src/hardware/steering_actuator/launch/machine.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): [ Node( package="steering_actuator", - executable="steering_actuator_test_node", + executable="steering_actuator_node", parameters=[ get_package_share_path("steering_actuator") / "config" / "steering.yaml", ], diff --git a/src/hardware/steering_actuator/src/component_steering_actuator.cpp b/src/hardware/steering_actuator/src/component_steering_actuator.cpp new file mode 100644 index 000000000..dd5451eaf --- /dev/null +++ b/src/hardware/steering_actuator/src/component_steering_actuator.cpp @@ -0,0 +1,288 @@ +#include "component_steering_actuator.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace steering_actuator { + +void SteeringActuator::configure_c5e() { + this->send_steering_data(PROFILE_VELOCITY, (uint8_t *)¤t_velocity_, 4); + this->send_steering_data(PROFILE_ACCELERATION, (uint8_t *)¤t_acceleration_, 4); + this->send_steering_data(PROFILE_DECELERATION, (uint8_t *)¤t_acceleration_, 4); + this->send_steering_data(QUICK_STOP_DECELERATION, (uint8_t *)¤t_acceleration_, 4); + this->send_steering_data(MAX_ACCELERATION, (uint8_t *)¤t_acceleration_, 4); + this->send_steering_data(MAX_DECELERATION, (uint8_t *)¤t_acceleration_, 4); + // this->send_steering_data(HOME_OFFSET, (uint8_t *)&offset_, 4); + + RCLCPP_INFO(this->get_logger(), "Configured C5E"); +} + +void SteeringActuator::c5e_state_request_callback() { + this->read_steering_data(STATUS_WORD); + // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 250, "Requesting state"); + std_msgs::msg::Bool::UniquePtr msg(new std_msgs::msg::Bool()); + msg->data = centred_; + steering_ready_pub_->publish(std::move(msg)); +} + +// Check State to enable or disable motor +void SteeringActuator::as_state_callback(const driverless_msgs::msg::State::SharedPtr msg) { + if (msg->state == driverless_msgs::msg::State::DRIVING || + msg->state == driverless_msgs::msg::State::ACTIVATE_EBS) { + // Enable motor + motor_enabled_ = true; + } else { + // Disable motor + motor_enabled_ = false; + centred_ = false; + settled_count_ = 0; + initial_enc_saved_ = false; + steering_ang_received_ = false; + } +} + +// Get steering angle reading +void SteeringActuator::steering_angle_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg) { + current_steering_angle_ = msg->data; + if (!steering_ang_received_) steering_ang_received_ = true; + RCLCPP_INFO_ONCE(this->get_logger(), "Steering angle received"); + RCLCPP_DEBUG(this->get_logger(), "Current angle: %f", msg->data); +} + +// Get desired steering angle to update steering +void SteeringActuator::driving_command_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg) { + if (!motor_enabled_ || !centred_) return; + + double requested_steering_angle = msg->drive.steering_angle; + // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 500, "Requested angle: %f", + // msg->drive.steering_angle); turning left eqn: -83.95x - 398.92 turning right eqn: -96.19x - 83.79 + int32_t target; + if (requested_steering_angle > centre_angle_) { + target = int32_t(-86.45 * requested_steering_angle - 398.92) - offset_; + } else { + target = int32_t(-94.58 * requested_steering_angle - 83.79) - offset_; + } + target = std::max(std::min(target, max_position_ - offset_), -max_position_ - offset_); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 250, "Target: %f = %d", requested_steering_angle, + target); + this->target_position(target); +} + +// Receive message from CAN +void SteeringActuator::can_callback(const driverless_msgs::msg::Can::SharedPtr msg) { + // RCLCPP_INFO(this->get_logger(), "CAN ID: %x", msg->id); + // Message ID from the steering actuator + if (msg->id == C5E_EMCY_ID) { + // first two bytes are the error code + uint16_t error_code = (msg->data[1] << 8 | msg->data[0]); + RCLCPP_ERROR(this->get_logger(), "Emergency error code: %x", error_code); + } else if (msg->id == C5E_BOOT_UP_ID) { + RCLCPP_INFO(this->get_logger(), "C5E Booted Up"); + } else if (msg->id == C5E_POS_ID) { + // first 4 bytes are the position (int32) + uint32_t data = 0; + // iterate to get the 4 bytes + for (int i = 0; i < 4; i++) { + data |= msg->data[i] << (8 * i); + } + + // message received for position revolution + int32_t val = (int32_t)data; + current_enc_revolutions_ = val; + if (!initial_enc_saved_) { + RCLCPP_INFO_ONCE(this->get_logger(), "Position received"); + // initial_enc_ = val; + initial_enc_saved_ = true; + } + + driverless_msgs::msg::Int32Stamped::UniquePtr enc_msg(new driverless_msgs::msg::Int32Stamped()); + enc_msg->header.stamp = this->get_clock()->now(); + enc_msg->data = current_enc_revolutions_; + encoder_pub_->publish(std::move(enc_msg)); + } else if (msg->id == C5E_SRV_ID) { + // objects returned from sdo read + uint16_t object_id = (msg->data[2] & 0xFF) << 8 | (msg->data[1] & 0xFF); + + // if (msg->data[0] == 0x60 || msg->data[0] == 0x80) { + if (msg->data[0] == 0x80) { + // acknowledgements of sdo write: 0x60 -> success ack, 0x80 -> error ack + RCLCPP_INFO(this->get_logger(), "ACK object: %x, %x", object_id, msg->data[0]); + return; + } + + if (object_id == STATUS_WORD) { + uint16_t status_word = (msg->data[5] << 8 | msg->data[4]); + current_state_ = parse_state(status_word); + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 100, "Status word: %s", + std::bitset<16>(status_word).to_string().c_str()); + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 100, "Current state: %s", + current_state_.name.c_str()); + + if (motor_enabled_) { + if (current_state_ == states[RTSO]) { + desired_state_ = states[SO]; + } else if (current_state_ == states[SO]) { + desired_state_ = states[OE]; + // default params + this->configure_c5e(); + } else if (current_state_ == states[OE]) { + // stay here -> no transition + } else { + desired_state_ = states[RTSO]; + } + } else { + // disabled transitions + desired_state_ = states[RTSO]; + } + + // Print current and desired states + RCLCPP_DEBUG(this->get_logger(), "Desired state: %s", desired_state_.name.c_str()); + + // State transition stage via state map definitions + if (current_state_ != desired_state_) { + RCLCPP_INFO(this->get_logger(), "Current state: %s", current_state_.name.c_str()); + RCLCPP_INFO(this->get_logger(), "Sending state: %s", desired_state_.name.c_str()); + this->send_steering_data(CONTROL_WORD, (uint8_t *)&desired_state_.control_word, 2); + } + } + } +} + +// Update Steering +void SteeringActuator::pre_op_centering() { + if (!motor_enabled_ || !steering_ang_received_ || centred_ || current_state_ != states[OE]) + return; // if multithread doesn't work + + RCLCPP_INFO_ONCE(this->get_logger(), "Centering"); + // center steering + // splitting into threads means steering angle will be updated separately to this function + if (settled_count_ < settling_iter_) { // if multithread doesn't work + double error = current_steering_angle_ - centre_angle_; + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Error: %f", error); + if (abs(error) < centre_range_) { + // motor has settled enough + settled_count_ += 1; + } else { + // reset settled count to debounce initial overshoots + settled_count_ = 0; + } + // left hand down is +, rhd is - + double target = Kp_ * error; // P commands to send to plant + this->target_position(target); + return; // if multithread doesn't work + } + offset_ = initial_enc_ - current_enc_revolutions_; + centred_ = true; + // save offset and configure c5e for different motion profile + current_velocity_ = this->get_parameter("velocity").as_int(); + current_acceleration_ = this->get_parameter("acceleration").as_int(); + control_method_ = MODE_ABSOLUTE; + RCLCPP_INFO_ONCE(this->get_logger(), "Centered, offset: %d", offset_); + this->configure_c5e(); + return; +} + +void SteeringActuator::target_position(int32_t target) { + if (current_state_ != states[OE]) { + RCLCPP_INFO_ONCE(this->get_logger(), "Not enabled"); + return; + } + RCLCPP_INFO_ONCE(this->get_logger(), "Enabled, Targeting"); + + this->send_steering_data(CONTROL_WORD, (uint8_t *)&control_method_, 2); + // print bitset control word + // RCLCPP_INFO(this->get_logger(), "Control word: %s", + // std::bitset<16>(control_method_).to_string().c_str()); + this->send_steering_data(TARGET_POSITION, (uint8_t *)&target, 4); + uint16_t trigger_control_method = control_method_ | TRIGGER_MOTION; + this->send_steering_data(CONTROL_WORD, (uint8_t *)&trigger_control_method, 2); + // RCLCPP_INFO(this->get_logger(),"Trigger control word: %s", + // std::bitset<16>(trigger_control_method).to_string().c_str()); +} + +void SteeringActuator::send_steering_data(uint16_t obj_index, uint8_t *data, size_t data_size) { + uint32_t id; // Packet id out + uint8_t out[8]; // Data out + sdo_write(C5E_NODE_ID, obj_index, 0x00, (uint8_t *)data, data_size, &id, out); // Control Word + can_pub_->publish(std::move(_d_2_f(id, 0, out, sizeof(out)))); +} + +void SteeringActuator::read_steering_data(uint16_t obj_index) { + uint32_t id; // Packet id out + uint8_t out[8]; // Data out + sdo_read(C5E_NODE_ID, obj_index, 0x00, &id, out); // Control Word + can_pub_->publish(std::move(_d_2_f(id, 0, out, sizeof(out)))); +} + +SteeringActuator::SteeringActuator(const rclcpp::NodeOptions & options) : Node("steering_actuator_node", options) { + // Steering parameters + this->declare_parameter("velocity", 10000); + this->declare_parameter("velocity_centering", 100); + this->declare_parameter("acceleration", 2000); + this->declare_parameter("acceleration_centering", 100); + this->declare_parameter("centre_range", 5.0); + this->declare_parameter("centre_angle", -2.0); + this->declare_parameter("settling_iter", 20); + this->declare_parameter("max_position", 7500); + // PID controller parameters + this->declare_parameter("Kp", 1.0); + this->declare_parameter("Ki", 0.0); + this->declare_parameter("Kd", 0.0); + + centre_range_ = this->get_parameter("centre_range").as_double(); + centre_angle_ = this->get_parameter("centre_angle").as_double(); + settling_iter_ = this->get_parameter("settling_iter").as_int(); + max_position_ = this->get_parameter("max_position").as_int(); + Kp_ = this->get_parameter("Kp").as_double(); + Ki_ = this->get_parameter("Ki").as_double(); + Kd_ = this->get_parameter("Kd").as_double(); + current_velocity_ = this->get_parameter("velocity").as_int(); + current_acceleration_ = this->get_parameter("acceleration").as_int(); + + sensor_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + control_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto sensor_cb_opt = rclcpp::SubscriptionOptions(); + sensor_cb_opt.callback_group = sensor_cb_group_; + auto control_cb_opt = rclcpp::SubscriptionOptions(); + control_cb_opt.callback_group = control_cb_group_; + + // Create subscriber to topic "canbus_rosbound" + canopen_sub_ = this->create_subscription( + "/can/canopen_rosbound", QOS_ALL, std::bind(&SteeringActuator::can_callback, this, _1), sensor_cb_opt); + + // Create subscriber to topic "steering_angle" + steer_ang_sub_ = this->create_subscription( + "/vehicle/steering_angle", QOS_ALL, std::bind(&SteeringActuator::steering_angle_callback, this, _1), + sensor_cb_opt); + + // Create subscriber to topic "driving_command" + ackermann_sub_ = this->create_subscription( + "/control/driving_command", QOS_ALL, std::bind(&SteeringActuator::driving_command_callback, this, _1), + control_cb_opt); + + // Create subscriber to topic AS status + state_sub_ = this->create_subscription( + "/system/as_status", QOS_ALL, std::bind(&SteeringActuator::as_state_callback, this, _1), control_cb_opt); + + steering_update_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), + std::bind(&SteeringActuator::pre_op_centering, this), + sensor_cb_group_); + + // Create state request and config timers + state_request_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), + std::bind(&SteeringActuator::c5e_state_request_callback, this), + control_cb_group_); + + // Create publisher to topic "canbus_carbound" + can_pub_ = this->create_publisher("/can/canbus_carbound", QOS_ALL); + + // Create publisher to topic "encoder_reading" + encoder_pub_ = this->create_publisher("/vehicle/encoder_reading", QOS_ALL); + + // Create publisher to topic "steering_state" + steering_ready_pub_ = this->create_publisher("/system/steering_ready", QOS_ALL); + + RCLCPP_INFO(this->get_logger(), "---Steering Actuator Node Initialised---"); +} + +} // namespace steering_actuator + +RCLCPP_COMPONENTS_REGISTER_NODE(steering_actuator::SteeringActuator); diff --git a/src/hardware/steering_actuator/src/node_steering_actuator.cpp b/src/hardware/steering_actuator/src/node_steering_actuator.cpp index deb4fe520..93d6376ca 100644 --- a/src/hardware/steering_actuator/src/node_steering_actuator.cpp +++ b/src/hardware/steering_actuator/src/node_steering_actuator.cpp @@ -1,441 +1,16 @@ -#include // Timer library -#include // Container library +#include "component_steering_actuator.hpp" -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" // ROS Messages -#include "can_interface.hpp" // CAN interface library to convert data array into a canbus frame (data_2_frame) -#include "canopen.hpp" // CAN library to communicate systems via sdo_read and sdo_write -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" // ROS Messages -#include "driverless_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" // C++ Required Libraries -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" -#include "steering_actuator/steering_common.hpp" - -using std::placeholders::_1; - -// Steering Actuation Class -class SteeringActuator : public rclcpp::Node, public CanInterface { - private: - // Creates - rclcpp::TimerBase::SharedPtr c5e_state_request_timer; - rclcpp::TimerBase::SharedPtr c5e_config_request_timer; - rclcpp::TimerBase::SharedPtr steering_update_timer; - - // Creates publishers and subscribers - rclcpp::Publisher::SharedPtr can_pub; - rclcpp::Publisher::SharedPtr encoder_pub; - rclcpp::Subscription::SharedPtr ackermann_sub; - rclcpp::Subscription::SharedPtr steer_ang_sub; - rclcpp::Subscription::SharedPtr state_sub; - rclcpp::Subscription::SharedPtr canopen_sub_; - - // params - double Kp, Ki, Kd, centre_range; - - bool shutdown_requested = false; - driverless_msgs::msg::State state; - c5e_state desired_state = states[RTSO]; - c5e_state current_state = states[RTSO]; - uint16_t control_method = MODE_ABSOLUTE; - bool motor_enabled = true; - bool centred = false; - bool steering_ang_received = false; - int32_t offset = 0; - int settled_count = 0; - bool initial_enc_saved = false; - int32_t initial_enc; - double current_steering_angle = 0; - double center_steering = -2.0; - double requested_steering_angle = center_steering; // Desired Steering Angle (Guidance Logic) - int32_t current_enc_revolutions = 0; // Current Encoder Revolutions (Stepper encoder) - float integral_error = 0; // Integral Error - float prev_error = 0; // Derivative Error - uint32_t current_velocity; - uint32_t current_acceleration; - std::chrono::high_resolution_clock::time_point last_update = std::chrono::high_resolution_clock::now(); // Timer - std::chrono::high_resolution_clock::time_point last_reading = std::chrono::high_resolution_clock::now(); // Timer - - // These publishings are required to send a service 'trigger' to the c5e controller - // with the specific object ID (seen in struct above). - // Then the c5e controller will send a CAN message back with that ID - void c5e_state_request_callback() { - this->read_steering_data(STATUS_WORD); - this->read_steering_data(POSITION_ACTUAL_VAL); - } - - void c5e_config_request_callback() { - this->read_steering_data(HOME_OFFSET); - this->read_steering_data(MOTION_PROFILE_TYPE); - this->read_steering_data(PROFILE_VELOCITY); - this->read_steering_data(END_VELOCITY); - this->read_steering_data(PROFILE_ACCELERATION); - this->read_steering_data(PROFILE_DECELERATION); - this->read_steering_data(QUICK_STOP_DECELERATION); - this->read_steering_data(MAX_ACCELERATION); - this->read_steering_data(MAX_DECELERATION); - this->read_steering_data(MODE_OF_OPERATION); - } - - // Receive message from CAN - void can_callback(const driverless_msgs::msg::Can msg) { - if (msg.id == C5E_BOOT_UP_ID) { - RCLCPP_INFO(this->get_logger(), "C5E Booted Up"); - } else if (msg.id == C5E_SRV_ID) { - uint16_t object_id = (msg.data[2] & 0xFF) << 8 | (msg.data[1] & 0xFF); - if (msg.data[0] == 0x60 || msg.data[0] == 0x80) { - // 0x60 -> success ack - // 0x80 -> error ack - RCLCPP_DEBUG(this->get_logger(), "ACK object: %x, %x", object_id, msg.data[0]); - return; - } - - uint32_t data = 0; - size_t size = can_open_size_map[msg.data[0]]; - for (size_t i = 0; i < size; i++) { - data |= (msg.data[4 + i] & 0xFF) << i * 8; - } - - RCLCPP_INFO(this->get_logger(), "Object ID: %x", object_id); - - if (object_id == STATUS_WORD) { - uint16_t status_word = (msg.data[3] << 8 | msg.data[4]); - this->current_state = parse_state(status_word); - - if (this->motor_enabled) { - if (this->current_state == this->desired_state) { - // enabled transitions - if (this->current_state == states[NRTSO]) { - this->desired_state = states[SOD]; - } else if (this->current_state == states[SOD]) { - this->desired_state = states[RTSO]; - } else if (this->current_state == states[RTSO]) { - this->desired_state = states[SO]; - } else if (this->current_state == states[SO]) { - this->desired_state = states[OE]; - } else if (this->current_state == states[OE]) { - // stay here -> no transition - } else { - this->desired_state == states[RTSO]; - } - } - } else { - // disabled transitions - this->desired_state = states[RTSO]; - } - - if (shutdown_requested) { - this->desired_state = states[RTSO]; - if (this->current_state == this->desired_state) { - rclcpp::shutdown(); - } - } - - // Print current and desired states - RCLCPP_DEBUG(this->get_logger(), "Parsed state: %s", this->current_state.name.c_str()); - RCLCPP_DEBUG(this->get_logger(), "Desired state: %s", this->desired_state.name.c_str()); - - // State transition stage via state map definitions (seriously figure out what the hell sdo write - // is) - if (this->current_state != this->desired_state) { - RCLCPP_INFO(this->get_logger(), "Sending state: %s", this->desired_state.name.c_str()); - this->send_steering_data(CONTROL_WORD, (uint8_t *)&this->desired_state.control_word, 2); - } - } - // message received for position revolution - else if (object_id == POSITION_ACTUAL_VAL) { - int32_t val = (int32_t)data; - this->current_enc_revolutions = val; - if (!this->initial_enc_saved) { - this->initial_enc = val; - this->initial_enc_saved = true; - } - std_msgs::msg::Int32 enc_msg; - enc_msg.data = this->current_enc_revolutions; - this->encoder_pub->publish(enc_msg); - } - // message received for offset position - else if (object_id == HOME_OFFSET) { - int32_t val = (int32_t)data; - RCLCPP_DEBUG(this->get_logger(), "HOME_OFFSET: %i", val); - - int32_t desired_val = 0; - if (val != desired_val) { - this->send_steering_data(HOME_OFFSET, (uint8_t *)&this->offset, 4); - } - } else if (object_id == MOTION_PROFILE_TYPE) { - int16_t val = (int16_t)data; - RCLCPP_DEBUG(this->get_logger(), "MOTION_PROFILE_TYPE: %i", val); - - int32_t desired_val = 0; - if (val != desired_val) { - this->send_steering_data(MOTION_PROFILE_TYPE, (uint8_t *)&desired_val, 4); - } - } else if (object_id == PROFILE_VELOCITY) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "PROFILE_VELOCITY: %u", val); - - if (val != this->current_velocity) { - this->send_steering_data(PROFILE_VELOCITY, (uint8_t *)&this->current_velocity, 4); - } - } else if (object_id == END_VELOCITY) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "END_VELOCITY: %u", val); - - uint32_t desired_val = 0; - if (val != desired_val) { - this->send_steering_data(END_VELOCITY, (uint8_t *)&desired_val, 4); - } - } else if (object_id == PROFILE_ACCELERATION) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "PROFILE_ACCELERATION: %u", val); - - if (val != this->current_acceleration) { - this->send_steering_data(PROFILE_ACCELERATION, (uint8_t *)&this->current_acceleration, 4); - } - } else if (object_id == PROFILE_DECELERATION) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "PROFILE_DECELERATION: %u", val); - - if (val != this->current_acceleration) { - this->send_steering_data(PROFILE_DECELERATION, (uint8_t *)&this->current_acceleration, 4); - } - } else if (object_id == QUICK_STOP_DECELERATION) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "QUICK_STOP_DECELERATION: %u", val); - - if (val != this->current_acceleration) { - this->send_steering_data(QUICK_STOP_DECELERATION, (uint8_t *)&this->current_acceleration, 4); - } - } else if (object_id == MAX_ACCELERATION) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "MAX_ACCELERATION: %u", val); - - if (val != this->current_acceleration) { - this->send_steering_data(MAX_ACCELERATION, (uint8_t *)&this->current_acceleration, 4); - } - } else if (object_id == MAX_DECELERATION) { - uint32_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "MAX_DECELERATION: %u", val); - - if (val != this->current_acceleration) { - this->send_steering_data(MAX_DECELERATION, (uint8_t *)&this->current_acceleration, 4); - } - } else if (object_id == MODE_OF_OPERATION) { - int8_t val = (uint32_t)data; - RCLCPP_DEBUG(this->get_logger(), "MODE_OF_OPERATION: %i", val); - - int32_t desired_val = 1; - if (val != desired_val) { - this->send_steering_data(MODE_OF_OPERATION, (uint8_t *)&desired_val, 4); - } - } - } - } - - // Check State to enable or disable motor - void as_state_callback(const driverless_msgs::msg::State msg) { - this->state = msg; - if (msg.state == driverless_msgs::msg::State::DRIVING || - msg.state == driverless_msgs::msg::State::ACTIVATE_EBS || - msg.state == driverless_msgs::msg::State::EMERGENCY) { - // Enable motor - this->motor_enabled = true; - } else { - this->motor_enabled = false; - } - } - - // Get steering angle reading - void steering_angle_callback(const std_msgs::msg::Float32 msg) { - this->current_steering_angle = msg.data; - auto current_reading = std::chrono::high_resolution_clock::now(); // Update clock - double elapsed_time_seconds = - std::chrono::duration(current_reading - last_reading).count() / - 1000; // Calculate time elapsed - this->last_reading = current_reading; // Set previous time to current time - if (!this->steering_ang_received) this->steering_ang_received = true; - RCLCPP_DEBUG(this->get_logger(), "Current angle: %f", msg.data); - RCLCPP_DEBUG(this->get_logger(), "Time: %f", elapsed_time_seconds); - } - - // Get desired steering angle to update steering - void driving_command_callback(const ackermann_msgs::msg::AckermannDriveStamped msg) { - this->requested_steering_angle = msg.drive.steering_angle; - RCLCPP_INFO(this->get_logger(), "Requested angle: %f", msg.drive.steering_angle); - } - - // Update Steering - void update_steering() { - // all state transitions passed - if (this->motor_enabled && this->steering_ang_received) { - // motor has not centred itself with steering ang sensor - if (!this->centred) { - auto current_update = std::chrono::high_resolution_clock::now(); // Update clock - double elapsed_time_seconds = - std::chrono::duration(current_update - last_update).count() / - 1000; // Calculate time elapsed - this->last_update = current_update; // Set previous time to current time - - // Grab error between steering angle and "zero" - double error = -(this->current_steering_angle - this->center_steering); - auto &clk = *this->get_clock(); - RCLCPP_INFO_THROTTLE(this->get_logger(), clk, 1000, "Error: %f", error); - - if (abs(error) < this->centre_range) { - // motor has settled enough - this->settled_count += 1; - - if (this->settled_count > 200) { - this->offset = this->initial_enc - this->current_enc_revolutions; - this->centred = true; - this->current_velocity = this->get_parameter("velocity").as_int(); - this->current_acceleration = this->get_parameter("acceleration").as_int(); - this->control_method = MODE_ABSOLUTE; - RCLCPP_INFO(this->get_logger(), "CENTRED STEERING, %i", this->offset); - return; - } - } else { - // reset settled count to debounce initial overshoots - this->settled_count = 0; - } - - this->integral_error += error * elapsed_time_seconds; // Grab integral error - double derivative_error = (error - this->prev_error) / elapsed_time_seconds; // Grab derivative error - - // left hand down is +, rhd is - - double target = -(this->Kp * error + this->Ki * this->integral_error + - this->Kd * derivative_error); // PID commands to send to plant - // RCLCPP_INFO(this->get_logger(), "Kp: %f err: %f Ki: %f i: %f Kd: %f d: %f target: %f", Kp, error, Ki, - // integral_error, Kd, derivative_error, target); // Prirnt PID commands - - this->prev_error = error; - - this->target_position(target); - } - // we have set a home offset - else if (this->centred) { - // turning left eqn: -83.95x - 398.92 - // turning right eqn: -96.19x - 83.79 - int32_t target; - if (this->requested_steering_angle > this->center_steering) { - target = int32_t(-86.45 * this->requested_steering_angle - 398.92) - this->offset; - } else { - target = int32_t(-94.58 * this->requested_steering_angle - 83.79) - this->offset; - } - target = std::max(std::min(target, 7500 - this->offset), -7500 - this->offset); - this->target_position(target); - } - } - } - - void target_position(int32_t target) { - if (this->current_state != states[OE]) { - RCLCPP_INFO_ONCE(this->get_logger(), "Not enabled, can't target"); - return; - } - RCLCPP_INFO_ONCE(this->get_logger(), "Enabled, Targeting"); - - this->send_steering_data(CONTROL_WORD, (uint8_t *)&this->control_method, 2); - this->send_steering_data(TARGET_POSITION, (uint8_t *)&target, 4); - uint16_t trigger_control_method = this->control_method | TRIGGER_MOTION; - this->send_steering_data(CONTROL_WORD, (uint8_t *)&trigger_control_method, 2); - } - - void send_steering_data(uint16_t obj_index, uint8_t *data, size_t data_size) { - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - sdo_write(C5E_NODE_ID, obj_index, 0x00, (uint8_t *)data, data_size, &id, out); // Control Word - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - void read_steering_data(uint16_t obj_index) { - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - sdo_read(C5E_NODE_ID, obj_index, 0x00, &id, out); // Control Word - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - public: - SteeringActuator() : Node("steering_actuator_node") { - // Steering parameters - this->declare_parameter("velocity", 100); - this->declare_parameter("velocity_centering", 200); - this->declare_parameter("acceleration", 100); - this->declare_parameter("acceleration_centering", 100); - this->declare_parameter("centre_range", 5); - // PID controller parameters - this->declare_parameter("Kp", 1.0); - this->declare_parameter("Ki", 0.0); - this->declare_parameter("Kd", 0.0); - - this->centre_range = this->get_parameter("centre_range").as_double(); - this->Kp = this->get_parameter("Kp").as_double(); - this->Ki = this->get_parameter("Ki").as_double(); - this->Kd = this->get_parameter("Kd").as_double(); - this->current_velocity = this->get_parameter("velocity_centering").as_int(); - this->current_acceleration = this->get_parameter("acceleration_centering").as_int(); - this->control_method = MODE_RELATIVE; - - // Create publisher to topic "canbus_carbound" - this->can_pub = this->create_publisher("/can/canbus_carbound", QOS_ALL); - - // Create publisher to topic "encoder_reading" - this->encoder_pub = this->create_publisher("/vehicle/encoder_reading", QOS_ALL); - - // Create subscriber to topic AS status - // this->state_sub = this->create_subscription( - // "/system/as_status", QOS_ALL, std::bind(&SteeringActuator::as_state_callback, this, _1)); - - // Create subscriber to topic "steering_angle" - this->steer_ang_sub = this->create_subscription( - "/vehicle/steering_angle", QOS_ALL, std::bind(&SteeringActuator::steering_angle_callback, this, _1)); - - // Create subscriber to topic "driving_command" - this->ackermann_sub = this->create_subscription( - "/control/driving_command", QOS_ALL, std::bind(&SteeringActuator::driving_command_callback, this, _1)); - - // Create subscriber to topic "canbus_rosbound" - this->canopen_sub_ = this->create_subscription( - "/can/canopen_rosbound", QOS_ALL, std::bind(&SteeringActuator::can_callback, this, _1)); - - // Create state request and config timers - this->steering_update_timer = - this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&SteeringActuator::update_steering, this)); - - // Create state request and config timers - this->c5e_state_request_timer = this->create_wall_timer( - std::chrono::milliseconds(50), std::bind(&SteeringActuator::c5e_state_request_callback, this)); - - // this->c5e_config_request_timer = this->create_wall_timer( - // std::chrono::seconds(1), std::bind(&SteeringActuator::c5e_config_request_callback, this)); - - RCLCPP_INFO(this->get_logger(), "---Steering Actuator Node Initialised---"); - } - - // Shutdown system - void shutdown() { this->shutdown_requested = true; } -}; - -// Prototype functions initialisations? -std::function handler; -void signal_handler(int signal) { handler(signal); } - -// Main loop int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); // Initialise ROS2 + rclcpp::init(argc, argv); - auto node = std::make_shared(); // Constructs an empty SteeringActuator class + rclcpp::NodeOptions options; + auto node = std::make_shared(options); - // Hack - // signal(SIGINT, signal_handler); - // handler = [node](int signal) { - // RCLCPP_INFO(node->get_logger(), "Shutting down motor."); - // node->shutdown(); - // return signal; - // }; + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); - rclcpp::spin(node); rclcpp::shutdown(); + return 0; } diff --git a/src/hardware/steering_actuator/src/node_steering_actuator_old.cpp b/src/hardware/steering_actuator/src/node_steering_actuator_old.cpp deleted file mode 100644 index dc88121c9..000000000 --- a/src/hardware/steering_actuator/src/node_steering_actuator_old.cpp +++ /dev/null @@ -1,524 +0,0 @@ -#include // Timer library -#include // Container library - -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" // ROS Messages -#include "can_interface.hpp" // CAN interface library to convert data array into a canbus frame (data_2_frame) -#include "canopen.hpp" // CAN library to communicate systems via sdo_read and sdo_write -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" // ROS Messages -#include "driverless_msgs/msg/state.hpp" -#include "driverless_msgs/msg/steering_reading.hpp" -#include "rclcpp/rclcpp.hpp" // C++ Required Libraries - -using std::placeholders::_1; - -const int C5_E_ID = 0x70; - -/* State ID Definitions - * NRTSO = 0 Not ready to switch on - * SOD = 64 Switch on disabled - * RTSO = 33 Ready to switch on - * SO = 35 Switched on - * OE = 39 Operation enabled - * QSA = 7 Quick stop active - * FRA = 15 Fault reaction active - * F = 72 Fault - */ - -typedef enum c5e_state_id { - NRTSO = 0b0000000000000000, - SOD = 0b0000000001000000, - RTSO = 0b0000000000100001, - SO = 0b0000000000100011, - OE = 0b0000000000100111, - QSA = 0b0000000000000111, - FRA = 0b0000000000001111, - F = 0b0000000001001000 -} c5e_state_id_t; - -/* Object ID Definitions - * HOME_OFFSET = 24700 - * MOTION_PROFILE_TYPE = 24710 - * PROFILE_VELOCITY = 24705 - * END_VELOCTITY = 24706 - * PROFILE_ACCELERATION = 24707 - * PROFILE_DECELERATION = 24708 - * QUICK_STOP_DECELERATION = 24709 - * MAX_ACCELERATION = 24773 - * MAX_DECELERATION = 24774 - * MODE_OF_OPERATION = 24672 - * TARGET_POSITION = 24698 - * CONTROL_WORD = 24640 - * STATUS_WORD = 24641 - */ - -typedef enum c5e_object_id { - HOME_OFFSET = 0x607C, - MOTION_PROFILE_TYPE = 0x6086, - PROFILE_VELOCITY = 0x6081, - END_VELOCTITY = 0x6082, - PROFILE_ACCELERATION = 0x6083, - PROFILE_DECELERATION = 0x6084, - QUICK_STOP_DECELERATION = 0x6085, - MAX_ACCELERATION = 0x60C5, - MAX_DECELERATION = 0x60C6, - MODE_OF_OPERATION = 0x6060, - TARGET_POSITION = 0x607A, - CONTROL_WORD = 0x6040, - STATUS_WORD = 0x6041, -} c5e_object_id_t; - -/* State Structure -name - The state which the steering wheel is switched on, enabled, or at fault -mask - ? -state_id - State ID Number -control_word - ? -*/ - -struct c5e_state { - std::string name; - uint16_t mask; - uint16_t state_id; - uint16_t control_word; - - bool operator==(const c5e_state &rhs) { - return (this->name == rhs.name && this->mask == rhs.mask && this->state_id == rhs.state_id); - } - - bool operator!=(const c5e_state &rhs) { return !(*this == rhs); } -}; - -// State Map Definitions -std::map states = { - {NRTSO, {"Not ready to switch on", 0b0000000001001111, NRTSO, 0}}, - {SOD, {"Switch on disabled", 0b0000000001001111, SOD, 0}}, - {RTSO, {"Ready to switch on", 0b0000000001101111, RTSO, 6}}, - {SO, {"Switched on", 0b0000000001101111, SO, 7}}, - {OE, {"Operation enabled", 0b0000000001101111, OE, 15}}, - {QSA, {"Quick stop active", 0b0000000001101111, QSA, 0}}, - {FRA, {"Fault reaction active", 0b0000000001001111, FRA, 0}}, - {F, {"Fault", 0b0000000001001111, F, 0}}, -}; - -// Parameter string definitions -const std::string PARAM_ACCELERATION = "acceleration"; -const std::string PARAM_VELOCITY = "velocity"; -const std::string PARAM_KP = "Kp"; -const std::string PARAM_KI = "Ki"; -const std::string PARAM_KD = "Kd"; - -// Steering Actuation Class -class SteeringActuator : public rclcpp::Node, public CanInterface { - private: - // Creates - rclcpp::TimerBase::SharedPtr c5e_state_request_timer; - rclcpp::TimerBase::SharedPtr c5e_config_request_timer; - - // Creates publisher for Can - // Creates subscribers for AckermannDriveStamped, State, SteeringReading, and Can - rclcpp::Publisher::SharedPtr can_pub; - rclcpp::Subscription::SharedPtr ackermann_sub; - rclcpp::Subscription::SharedPtr state_sub; - rclcpp::Subscription::SharedPtr steering_reading_sub; - rclcpp::Subscription::SharedPtr can_sub; - - driverless_msgs::msg::State state; // State message - c5e_state desired_state = states[RTSO]; // Desired State - c5e_state current_state = states[RTSO]; // Current State - bool motor_enabled = false; // Enable motors logic - double current_steering_angle = 0; // Current Steering Angle (Angle Sensor) - double requested_steering_angle = 0; // Desired Steering ANgle (Guidance Logic) - bool shutdown_requested = false; // Shutdown logic - - float Kp, Ki, Kd; // PID Gain - float integral_error = 0; // Integral Error - float prev_error = 0; // Derivative Error - std::chrono::high_resolution_clock::time_point last_update = std::chrono::high_resolution_clock::now(); // Timer - - // Request callback for state (via ROS2) - void c5e_state_request_callback() { - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - - sdo_read(C5_E_ID, STATUS_WORD, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - // Request callback for configuration (via ROS2) - void c5e_config_request_callback() { - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - - // Read callback information from canbus, grab can packet id for each object, then publish id - sdo_read(C5_E_ID, HOME_OFFSET, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, MOTION_PROFILE_TYPE, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, PROFILE_VELOCITY, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, END_VELOCTITY, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, PROFILE_ACCELERATION, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, PROFILE_DECELERATION, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, QUICK_STOP_DECELERATION, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, MAX_ACCELERATION, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, MAX_DECELERATION, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_read(C5_E_ID, MODE_OF_OPERATION, 0x00, &id, (uint8_t *)&out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - // Receive message from CAN - void can_callback(const driverless_msgs::msg::Can msg) { - if (msg.id == 0x5F0) { - // CAN message from the steering actuator - - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - - if (msg.data[0] == 0x60 || msg.data[0] == 0x80) { - // 0x60 -> success ack - // 0x80 -> error ack - return; - } - - uint16_t object_id = (msg.data[2] & 0xFF) << 8 | (msg.data[1] & 0xFF); - uint32_t data = 0; - size_t size = can_open_size_map[msg.data[0]]; - for (size_t i = 0; i < size; i++) { - data |= (msg.data[4 + i] & 0xFF) << i * 8; - } - - uint32_t param_velocity = this->get_parameter(PARAM_VELOCITY).as_int(); - uint32_t param_acceleration = this->get_parameter(PARAM_ACCELERATION).as_int(); - - switch (object_id) { - case STATUS_WORD: { - uint16_t status_word = (msg.data[3] << 8 | msg.data[4]); - this->current_state = this->parse_state(status_word); - - if (this->motor_enabled) { - if (this->current_state == this->desired_state) { - // enabled transitions - if (this->current_state == states[RTSO]) { - this->desired_state = states[SO]; - } else if (this->current_state == states[SO]) { - this->desired_state = states[OE]; - } else if (this->current_state == states[OE]) { - // stay here -> no transition - } else { - this->desired_state == states[RTSO]; - } - } - } else { - // disabled transitions - this->desired_state = states[RTSO]; - } - - if (shutdown_requested) { - this->desired_state = states[RTSO]; - if (this->current_state == this->desired_state) { - rclcpp::shutdown(); - } - } - - // Print current and desired states - RCLCPP_INFO(this->get_logger(), "Parsed state: %s", this->current_state.name.c_str()); - RCLCPP_INFO(this->get_logger(), "Desired state: %s", this->desired_state.name.c_str()); - - // State transition stage via state map definitions (seriously figure out what the hell sdo write - // is) - if (this->current_state != this->desired_state) { - RCLCPP_INFO(this->get_logger(), "Sending state: %s", this->desired_state.name.c_str()); - sdo_write(C5_E_ID, CONTROL_WORD, 0x00, (uint8_t *)&this->desired_state.control_word, 2, &id, - out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - break; - } - - // To set the controller to a usable state, we must set the: - // Home Offset = 0 - // Motion Profile Type = trapezoidal ramp (0) - // Profile Velocity = PARAM_VELOCITY - // End Velocity = 0 - // Profile Acceleration = PARAM_ACCELERATION - // Profile Deceleration = PARAM_ACCELERATION - // Quick Stop Deceleration = PARAM_ACCELERATION - // Max Acceleration = PARAM_ACCELERATION - // Max Deceleration = PARAM_ACCELERATION - // Mode of Operation = 1 (Profile Position) - - case HOME_OFFSET: { - int32_t val = (int32_t)data; - RCLCPP_INFO(this->get_logger(), "HOME_OFFSET: %i", val); - - int32_t desired_val = 0; - if (val != desired_val) { - sdo_write(C5_E_ID, HOME_OFFSET, 0x00, (uint8_t *)&desired_val, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case MOTION_PROFILE_TYPE: { - int16_t val = (int16_t)data; - RCLCPP_INFO(this->get_logger(), "MOTION_PROFILE_TYPE: %i", val); - - int32_t desired_val = 0; - if (val != desired_val) { - sdo_write(C5_E_ID, MOTION_PROFILE_TYPE, 0x00, (uint8_t *)&desired_val, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case PROFILE_VELOCITY: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "PROFILE_VELOCITY: %u", val); - - if (val != param_velocity) { - sdo_write(C5_E_ID, PROFILE_VELOCITY, 0x00, (uint8_t *)¶m_velocity, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case END_VELOCTITY: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "END_VELOCTITY: %u", val); - - uint32_t desired_val = 0; - if (val != desired_val) { - sdo_write(C5_E_ID, END_VELOCTITY, 0x00, (uint8_t *)&desired_val, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case PROFILE_ACCELERATION: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "PROFILE_ACCELERATION: %u", val); - - if (val != param_acceleration) { - sdo_write(C5_E_ID, PROFILE_ACCELERATION, 0x00, (uint8_t *)¶m_acceleration, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case PROFILE_DECELERATION: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "PROFILE_DECELERATION: %u", val); - - if (val != param_acceleration) { - sdo_write(C5_E_ID, PROFILE_DECELERATION, 0x00, (uint8_t *)¶m_acceleration, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case QUICK_STOP_DECELERATION: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "QUICK_STOP_DECELERATION: %u", val); - - if (val != param_acceleration) { - sdo_write(C5_E_ID, QUICK_STOP_DECELERATION, 0x00, (uint8_t *)¶m_acceleration, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case MAX_ACCELERATION: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "MAX_ACCELERATION: %u", val); - - if (val != param_acceleration) { - sdo_write(C5_E_ID, MAX_ACCELERATION, 0x00, (uint8_t *)¶m_acceleration, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case MAX_DECELERATION: { - uint32_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "MAX_DECELERATION: %u", val); - - if (val != param_acceleration) { - sdo_write(C5_E_ID, MAX_DECELERATION, 0x00, (uint8_t *)¶m_acceleration, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - case MODE_OF_OPERATION: { - int8_t val = (uint32_t)data; - RCLCPP_INFO(this->get_logger(), "MODE_OF_OPERATION: %i", val); - - int32_t desired_val = 1; - if (val != desired_val) { - sdo_write(C5_E_ID, MODE_OF_OPERATION, 0x00, (uint8_t *)&desired_val, 4, &id, out); - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - break; - } - } - } - } - - // Check State to enable or disable motor - void as_state_callback(const driverless_msgs::msg::State msg) { - this->state = msg; - if (msg.state == driverless_msgs::msg::State::DRIVING || - msg.state == driverless_msgs::msg::State::ACTIVATE_EBS || - msg.state == driverless_msgs::msg::State::EMERGENCY) { - // Enable motor - this->motor_enabled = true; - } else { - this->motor_enabled = false; - } - } - - // Check steering angle reading and upate steering - void steering_reading_callback(const driverless_msgs::msg::SteeringReading msg) { - this->current_steering_angle = msg.steering_angle; - this->update_steering(); - } - - // Check steering angle and desired steering angle to update steering - void driving_command_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg) { - float cappedAngle = std::fmax(std::fmin(msg->drive.steering_angle, 90), -90); - this->requested_steering_angle = cappedAngle; - this->update_steering(); - } - - // Update Steering - void update_steering() { - if (this->motor_enabled) { - auto current_update = std::chrono::high_resolution_clock::now(); // Update clock - double elapsed_time_seconds = - std::chrono::duration(current_update - last_update).count() / - 1000; // Calculate time elapsed - this->last_update = current_update; // Set previous time to current time - - double error = - this->requested_steering_angle - this->current_steering_angle; // Grab error between steering angle - this->integral_error += error * elapsed_time_seconds; // Grab integral error - double derivative_error = (error - this->prev_error) / elapsed_time_seconds; // Grab derivative error - - double target = - Kp * error + Ki * this->integral_error + Kd * derivative_error; // PID commands to send to plant - RCLCPP_INFO(this->get_logger(), "Kp: %f err: %f Ki: %f i: %f Kd: %f d: %f target: %f", Kp, error, Ki, - integral_error, Kd, derivative_error, target); // Prirnt PID commands - - // left hand down is +, rhd is - - this->target_position(target); - } - } - - // Figure out what control_worrd is and what it does - void target_position(int32_t target) { - if (this->current_state != states[OE]) { - return; - } - - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - uint16_t control_word; - - control_word = 111; - sdo_write(C5_E_ID, CONTROL_WORD, 0x00, (uint8_t *)&control_word, 2, &id, out); // Control Word - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - sdo_write(C5_E_ID, TARGET_POSITION, 0x00, (uint8_t *)&target, 4, &id, out); // Target - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - - // Set Control Word - control_word = 127; - sdo_write(C5_E_ID, CONTROL_WORD, 0x00, (uint8_t *)&control_word, 2, &id, out); // Control Word - this->can_pub->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - // Print state function - c5e_state parse_state(uint16_t status_word) { - for (const auto &[key, state] : states) { - if ((status_word & state.mask) == state.state_id) { - return state; - } - } - return states[F]; - } - - public: - SteeringActuator() : Node("steering_actuator_node") { - // Steering parameters - this->declare_parameter(PARAM_ACCELERATION, 0); - this->declare_parameter(PARAM_VELOCITY, 0); - - // PID controller parameters - this->declare_parameter(PARAM_KP, 0); - this->declare_parameter(PARAM_KI, 0); - this->declare_parameter(PARAM_KD, 0); - - this->get_parameter(PARAM_KP, this->Kp); - this->get_parameter(PARAM_KI, this->Ki); - this->get_parameter(PARAM_KD, this->Kd); - - // Create publisher to topic "canbus_carbound" - this->can_pub = this->create_publisher("/can/canbus_carbound", 10); - - // Create subscriber to topic "as_status" - this->state_sub = this->create_subscription( - "/system/as_status", QOS_LATEST, std::bind(&SteeringActuator::as_state_callback, this, _1)); - - // Create subscriber to topic "steering_reading" - this->steering_reading_sub = this->create_subscription( - "/vehicle/steering_reading", QOS_ALL, std::bind(&SteeringActuator::steering_reading_callback, this, _1)); - - // Create subscriber to topic "driving_command" - this->ackermann_sub = this->create_subscription( - "/control/driving_command", QOS_ALL, std::bind(&SteeringActuator::driving_command_callback, this, _1)); - - // Create subscriber to topic "canbus_rosbound" - this->can_sub = this->create_subscription( - "/can/canbus_rosbound", QOS_ALL, std::bind(&SteeringActuator::can_callback, this, _1)); - - // Create state request and config timers - this->c5e_state_request_timer = this->create_wall_timer( - std::chrono::milliseconds(50), std::bind(&SteeringActuator::c5e_state_request_callback, this)); - - this->c5e_config_request_timer = this->create_wall_timer( - std::chrono::seconds(1), std::bind(&SteeringActuator::c5e_config_request_callback, this)); - } - - // Shutdown system - void shutdown() { this->shutdown_requested = true; } -}; - -// Prototype functions initialisations? -std::function handler; -void signal_handler(int signal) { handler(signal); } - -// Main loop -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); // Initialise ROS2 - - auto node = std::make_shared(); // Constructs an empty SteeringActuator class - - // Hack - signal(SIGINT, signal_handler); // - handler = [node](int signal) { // - RCLCPP_INFO(node->get_logger(), "Shutting down motor."); // - node->shutdown(); // - return signal; // - }; - - rclcpp::spin(node); // - rclcpp::shutdown(); // - return 0; -} diff --git a/src/hardware/steering_actuator/src/node_steering_actuator_test.cpp b/src/hardware/steering_actuator/src/node_steering_actuator_test.cpp deleted file mode 100644 index 2d0d7da8a..000000000 --- a/src/hardware/steering_actuator/src/node_steering_actuator_test.cpp +++ /dev/null @@ -1,376 +0,0 @@ -#include -#include // Timer library -#include // Container library - -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" // ROS Messages -#include "can_interface.hpp" // CAN interface library to convert data array into a canbus frame (data_2_frame) -#include "canopen.hpp" // CAN library to communicate systems via sdo_read and sdo_write -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" // ROS Messages -#include "driverless_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" // C++ Required Libraries -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" -#include "steering_actuator/steering_common.hpp" - -using std::placeholders::_1; - -// Steering Actuation Class -class SteeringActuator : public rclcpp::Node, public CanInterface { - private: - // Creates - rclcpp::TimerBase::SharedPtr steering_update_timer_; - rclcpp::TimerBase::SharedPtr state_request_timer_; - - // Creates publishers and subscribers - rclcpp::Publisher::SharedPtr can_pub_; - rclcpp::Publisher::SharedPtr encoder_pub_; - rclcpp::Publisher::SharedPtr steering_ready_pub_; - rclcpp::Subscription::SharedPtr state_sub_; - rclcpp::Subscription::SharedPtr ackermann_sub_; - rclcpp::Subscription::SharedPtr steer_ang_sub_; - rclcpp::Subscription::SharedPtr canopen_sub_; - - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_; - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_; - - // params - double Kp_, Ki_, Kd_, centre_range_, centre_angle_; - int settling_iter_, max_position_; - - bool shutdown_requested_ = false; - driverless_msgs::msg::State state_; - c5e_state desired_state_ = states[RTSO]; - c5e_state current_state_ = states[NRTSO]; - uint16_t control_method_ = MODE_RELATIVE; - bool motor_enabled_ = false; - bool centred_ = false; - int32_t offset_ = 0; - int settled_count_ = 0; - bool initial_enc_saved_ = false; - int32_t initial_enc_ = 0; - bool steering_ang_received_ = false; - double current_steering_angle_ = 0; - int32_t current_enc_revolutions_ = 0; // Current Encoder Revolutions (Stepper encoder) - uint32_t current_velocity_; - uint32_t current_acceleration_; - - // time to reset node if no state received - std::chrono::time_point last_state_time = std::chrono::system_clock::now(); - - void configure_c5e() { - this->send_steering_data(PROFILE_VELOCITY, (uint8_t *)¤t_velocity_, 4); - this->send_steering_data(PROFILE_ACCELERATION, (uint8_t *)¤t_acceleration_, 4); - this->send_steering_data(PROFILE_DECELERATION, (uint8_t *)¤t_acceleration_, 4); - this->send_steering_data(QUICK_STOP_DECELERATION, (uint8_t *)¤t_acceleration_, 4); - this->send_steering_data(MAX_ACCELERATION, (uint8_t *)¤t_acceleration_, 4); - this->send_steering_data(MAX_DECELERATION, (uint8_t *)¤t_acceleration_, 4); - // this->send_steering_data(HOME_OFFSET, (uint8_t *)&offset_, 4); - - RCLCPP_INFO(this->get_logger(), "Configured C5E"); - } - - void c5e_state_request_callback() { - this->read_steering_data(STATUS_WORD); - // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 250, "Requesting state"); - std_msgs::msg::Bool msg; - msg.data = centred_; - steering_ready_pub_->publish(msg); - } - - // Check State to enable or disable motor - void as_state_callback(const driverless_msgs::msg::State msg) { - state_ = msg; - if (msg.state == driverless_msgs::msg::State::DRIVING || - msg.state == driverless_msgs::msg::State::ACTIVATE_EBS) { - // Enable motor - motor_enabled_ = true; - } else { - // Disable motor - motor_enabled_ = false; - centred_ = false; - settled_count_ = 0; - initial_enc_saved_ = false; - steering_ang_received_ = false; - } - } - - // Get steering angle reading - void steering_angle_callback(const std_msgs::msg::Float32 msg) { - current_steering_angle_ = msg.data; - if (!steering_ang_received_) steering_ang_received_ = true; - RCLCPP_INFO_ONCE(this->get_logger(), "Steering angle received"); - RCLCPP_DEBUG(this->get_logger(), "Current angle: %f", msg.data); - } - - // Get desired steering angle to update steering - void driving_command_callback(const ackermann_msgs::msg::AckermannDriveStamped msg) { - if (!motor_enabled_ || !centred_) return; - - double requested_steering_angle = msg.drive.steering_angle; - // RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 500, "Requested angle: %f", - // msg.drive.steering_angle); turning left eqn: -83.95x - 398.92 turning right eqn: -96.19x - 83.79 - int32_t target; - if (requested_steering_angle > centre_angle_) { - target = int32_t(-86.45 * requested_steering_angle - 398.92) - offset_; - } else { - target = int32_t(-94.58 * requested_steering_angle - 83.79) - offset_; - } - target = std::max(std::min(target, max_position_ - offset_), -max_position_ - offset_); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 250, "Target: %f = %d", requested_steering_angle, - target); - this->target_position(target); - } - - // Receive message from CAN - void can_callback(const driverless_msgs::msg::Can msg) { - // RCLCPP_INFO(this->get_logger(), "CAN ID: %x", msg.id); - // Message ID from the steering actuator - if (msg.id == C5E_EMCY_ID) { - // first two bytes are the error code - uint16_t error_code = (msg.data[1] << 8 | msg.data[0]); - RCLCPP_ERROR(this->get_logger(), "Emergency error code: %x", error_code); - } else if (msg.id == C5E_BOOT_UP_ID) { - RCLCPP_INFO(this->get_logger(), "C5E Booted Up"); - } else if (msg.id == C5E_POS_ID) { - // first 4 bytes are the position (int32) - uint32_t data = 0; - // iterate to get the 4 bytes - for (int i = 0; i < 4; i++) { - data |= msg.data[i] << (8 * i); - } - - // message received for position revolution - int32_t val = (int32_t)data; - current_enc_revolutions_ = val; - if (!initial_enc_saved_) { - RCLCPP_INFO_ONCE(this->get_logger(), "Position received"); - // initial_enc_ = val; - initial_enc_saved_ = true; - } - - std_msgs::msg::Int32 enc_msg; - enc_msg.data = current_enc_revolutions_; - encoder_pub_->publish(enc_msg); - } else if (msg.id == C5E_SRV_ID) { - // objects returned from sdo read - uint16_t object_id = (msg.data[2] & 0xFF) << 8 | (msg.data[1] & 0xFF); - - // if (msg.data[0] == 0x60 || msg.data[0] == 0x80) { - if (msg.data[0] == 0x80) { - // acknowledgements of sdo write: 0x60 -> success ack, 0x80 -> error ack - RCLCPP_INFO(this->get_logger(), "ACK object: %x, %x", object_id, msg.data[0]); - return; - } - - if (object_id == STATUS_WORD) { - uint16_t status_word = (msg.data[5] << 8 | msg.data[4]); - current_state_ = parse_state(status_word); - RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 100, "Status word: %s", - std::bitset<16>(status_word).to_string().c_str()); - RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 100, "Current state: %s", - current_state_.name.c_str()); - - if (motor_enabled_) { - if (current_state_ == states[RTSO]) { - desired_state_ = states[SO]; - } else if (current_state_ == states[SO]) { - desired_state_ = states[OE]; - // default params - this->configure_c5e(); - } else if (current_state_ == states[OE]) { - // stay here -> no transition - } else { - desired_state_ = states[RTSO]; - } - } else { - // disabled transitions - desired_state_ = states[RTSO]; - } - - if (shutdown_requested_) { - desired_state_ = states[RTSO]; - if (current_state_ == desired_state_) { - rclcpp::shutdown(); - } - } - - // Print current and desired states - RCLCPP_DEBUG(this->get_logger(), "Desired state: %s", desired_state_.name.c_str()); - - // State transition stage via state map definitions - if (current_state_ != desired_state_) { - RCLCPP_INFO(this->get_logger(), "Current state: %s", current_state_.name.c_str()); - RCLCPP_INFO(this->get_logger(), "Sending state: %s", desired_state_.name.c_str()); - this->send_steering_data(CONTROL_WORD, (uint8_t *)&desired_state_.control_word, 2); - } - } - } - } - - // Update Steering - void pre_op_centering() { - if (!motor_enabled_ || !steering_ang_received_ || centred_ || current_state_ != states[OE]) - return; // if multithread doesn't work - - RCLCPP_INFO_ONCE(this->get_logger(), "Centering"); - // center steering - // splitting into threads means steering angle will be updated separately to this function - if (settled_count_ < settling_iter_) { // if multithread doesn't work - double error = current_steering_angle_ - centre_angle_; - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Error: %f", error); - if (abs(error) < centre_range_) { - // motor has settled enough - settled_count_ += 1; - } else { - // reset settled count to debounce initial overshoots - settled_count_ = 0; - } - // left hand down is +, rhd is - - double target = Kp_ * error; // P commands to send to plant - this->target_position(target); - return; // if multithread doesn't work - } - offset_ = initial_enc_ - current_enc_revolutions_; - centred_ = true; - // save offset and configure c5e for different motion profile - current_velocity_ = this->get_parameter("velocity").as_int(); - current_acceleration_ = this->get_parameter("acceleration").as_int(); - control_method_ = MODE_ABSOLUTE; - RCLCPP_INFO_ONCE(this->get_logger(), "Centered, offset: %d", offset_); - this->configure_c5e(); - return; - } - - void target_position(int32_t target) { - if (current_state_ != states[OE]) { - RCLCPP_INFO_ONCE(this->get_logger(), "Not enabled"); - return; - } - RCLCPP_INFO_ONCE(this->get_logger(), "Enabled, Targeting"); - - this->send_steering_data(CONTROL_WORD, (uint8_t *)&control_method_, 2); - // print bitset control word - // RCLCPP_INFO(this->get_logger(), "Control word: %s", - // std::bitset<16>(control_method_).to_string().c_str()); - this->send_steering_data(TARGET_POSITION, (uint8_t *)&target, 4); - uint16_t trigger_control_method = control_method_ | TRIGGER_MOTION; - this->send_steering_data(CONTROL_WORD, (uint8_t *)&trigger_control_method, 2); - // RCLCPP_INFO(this->get_logger(),"Trigger control word: %s", - // std::bitset<16>(trigger_control_method).to_string().c_str()); - } - - void send_steering_data(uint16_t obj_index, uint8_t *data, size_t data_size) { - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - sdo_write(C5E_NODE_ID, obj_index, 0x00, (uint8_t *)data, data_size, &id, out); // Control Word - can_pub_->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - void read_steering_data(uint16_t obj_index) { - uint32_t id; // Packet id out - uint8_t out[8]; // Data out - sdo_read(C5E_NODE_ID, obj_index, 0x00, &id, out); // Control Word - can_pub_->publish(_d_2_f(id, 0, out, sizeof(out))); - } - - public: - SteeringActuator() : Node("steering_actuator_node") { - // Steering parameters - this->declare_parameter("velocity", 10000); - this->declare_parameter("velocity_centering", 100); - this->declare_parameter("acceleration", 2000); - this->declare_parameter("acceleration_centering", 100); - this->declare_parameter("centre_range", 5.0); - this->declare_parameter("centre_angle", -2.0); - this->declare_parameter("settling_iter", 20); - this->declare_parameter("max_position", 7500); - // PID controller parameters - this->declare_parameter("Kp", 1.0); - this->declare_parameter("Ki", 0.0); - this->declare_parameter("Kd", 0.0); - - centre_range_ = this->get_parameter("centre_range").as_double(); - centre_angle_ = this->get_parameter("centre_angle").as_double(); - settling_iter_ = this->get_parameter("settling_iter").as_int(); - max_position_ = this->get_parameter("max_position").as_int(); - Kp_ = this->get_parameter("Kp").as_double(); - Ki_ = this->get_parameter("Ki").as_double(); - Kd_ = this->get_parameter("Kd").as_double(); - current_velocity_ = this->get_parameter("velocity").as_int(); - current_acceleration_ = this->get_parameter("acceleration").as_int(); - - callback_group_subscriber1_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_subscriber2_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto sub1_opt = rclcpp::SubscriptionOptions(); - sub1_opt.callback_group = callback_group_subscriber1_; - auto sub2_opt = rclcpp::SubscriptionOptions(); - sub2_opt.callback_group = callback_group_subscriber2_; - - // Create subscriber to topic "canbus_rosbound" - canopen_sub_ = this->create_subscription( - "/can/canopen_rosbound", QOS_ALL, std::bind(&SteeringActuator::can_callback, this, _1), sub1_opt); - - // Create subscriber to topic "steering_angle" - steer_ang_sub_ = this->create_subscription( - "/vehicle/steering_angle", QOS_ALL, std::bind(&SteeringActuator::steering_angle_callback, this, _1), - sub2_opt); - - // Create subscriber to topic "driving_command" - ackermann_sub_ = this->create_subscription( - "/control/driving_command", QOS_ALL, std::bind(&SteeringActuator::driving_command_callback, this, _1), - sub2_opt); - - // Create subscriber to topic AS status - state_sub_ = this->create_subscription( - "/system/as_status", QOS_ALL, std::bind(&SteeringActuator::as_state_callback, this, _1)); - - steering_update_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), - std::bind(&SteeringActuator::pre_op_centering, this)); - - // Create state request and config timers - state_request_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), - std::bind(&SteeringActuator::c5e_state_request_callback, this), - callback_group_subscriber1_); - - // Create publisher to topic "canbus_carbound" - can_pub_ = this->create_publisher("/can/canbus_carbound", QOS_ALL); - - // Create publisher to topic "encoder_reading" - encoder_pub_ = this->create_publisher("/vehicle/encoder_reading", QOS_ALL); - - // Create publisher to topic "steering_state" - steering_ready_pub_ = this->create_publisher("/system/steering_ready", QOS_ALL); - - RCLCPP_INFO(this->get_logger(), "---Steering Actuator Node Initialised---"); - } - - // Shutdown system - void shutdown() { shutdown_requested_ = true; } -}; - -// Prototype functions initialisations? -std::function handler; -void signal_handler(int signal) { handler(signal); } - -// Main loop -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); // Initialise ROS2 - auto node = std::make_shared(); // Constructs an empty SteeringActuator class - // Hack - signal(SIGINT, signal_handler); - handler = [node](int signal) { - RCLCPP_INFO(node->get_logger(), "Shutting down motor."); - node->shutdown(); - return signal; - }; - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - - // rclcpp::spin(node); - // rclcpp::shutdown(); - return 0; -} From 376fcfbe5cb183678bc2f2a7da066f2248d7ebfb Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:53:42 +1000 Subject: [PATCH 5/8] compose canbus. inherit CAN driver interface for socket or tritium. --- .../CAN_Common/include/can_interface.hpp | 2 +- src/hardware/CAN_Common/src/can_interface.cpp | 12 +- src/hardware/canbus/CMakeLists.txt | 64 +++-- src/hardware/canbus/config/canbus.yaml | 5 +- src/hardware/canbus/include/SocketCAN.hpp | 5 +- src/hardware/canbus/include/TritiumCAN.hpp | 12 +- .../canbus/include/can2etherenet_adapter.hpp | 38 --- .../include/component_canbus_translator.hpp | 94 +++++++ src/hardware/canbus/include/interface.hpp | 19 ++ src/hardware/canbus/src/SocketCAN.cpp | 4 +- src/hardware/canbus/src/TritiumCAN.cpp | 21 +- .../canbus/src/can2ethernet_adapter.cpp | 27 -- .../src/component_canbus_translator.cpp | 206 ++++++++++++++ src/hardware/canbus/src/node_canbus.cpp | 90 ------ .../canbus/src/node_canbus_translator.cpp | 239 ---------------- .../src/node_canbus_translator_socket.cpp | 264 ------------------ .../canbus/src/node_socket_translator.cpp | 17 ++ .../canbus/src/node_tritium_translator.cpp | 17 ++ 18 files changed, 425 insertions(+), 711 deletions(-) delete mode 100644 src/hardware/canbus/include/can2etherenet_adapter.hpp create mode 100644 src/hardware/canbus/include/component_canbus_translator.hpp create mode 100644 src/hardware/canbus/include/interface.hpp delete mode 100644 src/hardware/canbus/src/can2ethernet_adapter.cpp create mode 100644 src/hardware/canbus/src/component_canbus_translator.cpp delete mode 100644 src/hardware/canbus/src/node_canbus.cpp delete mode 100644 src/hardware/canbus/src/node_canbus_translator.cpp delete mode 100644 src/hardware/canbus/src/node_canbus_translator_socket.cpp create mode 100644 src/hardware/canbus/src/node_socket_translator.cpp create mode 100644 src/hardware/canbus/src/node_tritium_translator.cpp diff --git a/src/hardware/CAN_Common/include/can_interface.hpp b/src/hardware/CAN_Common/include/can_interface.hpp index 5b0e658fc..354465d57 100644 --- a/src/hardware/CAN_Common/include/can_interface.hpp +++ b/src/hardware/CAN_Common/include/can_interface.hpp @@ -28,5 +28,5 @@ class CanInterface { protected: void copy_data(const std::vector &vec, uint8_t *dest, size_t n); // Convert a data array, bool and id information to a canbus frame (data_2_frame) - driverless_msgs::msg::Can _d_2_f(uint32_t id, bool is_extended, uint8_t *data, uint8_t dlc); + driverless_msgs::msg::Can::UniquePtr _d_2_f(uint32_t id, bool is_extended, uint8_t *data, uint8_t dlc); }; diff --git a/src/hardware/CAN_Common/src/can_interface.cpp b/src/hardware/CAN_Common/src/can_interface.cpp index 7aee54e9d..ac01ebf82 100644 --- a/src/hardware/CAN_Common/src/can_interface.cpp +++ b/src/hardware/CAN_Common/src/can_interface.cpp @@ -6,13 +6,13 @@ void CanInterface::copy_data(const std::vector &vec, uint8_t *dest, siz } } -driverless_msgs::msg::Can CanInterface::_d_2_f(uint32_t id, bool is_extended, uint8_t *data, uint8_t dlc) { - driverless_msgs::msg::Can frame; - frame.id = id; - frame.id_type = is_extended; +driverless_msgs::msg::Can::UniquePtr CanInterface::_d_2_f(uint32_t id, bool is_extended, uint8_t *data, uint8_t dlc) { + driverless_msgs::msg::Can::UniquePtr frame(new driverless_msgs::msg::Can()); + frame->id = id; + frame->id_type = is_extended; std::vector v; v.assign(data, data + dlc); - frame.data = v; - frame.dlc = dlc; + frame->data = v; + frame->dlc = dlc; return frame; } diff --git a/src/hardware/canbus/CMakeLists.txt b/src/hardware/canbus/CMakeLists.txt index 7b9faa50d..60f96591d 100644 --- a/src/hardware/canbus/CMakeLists.txt +++ b/src/hardware/canbus/CMakeLists.txt @@ -5,63 +5,75 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. find_package(driverless_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(std_msgs REQUIRED) add_compile_definitions(QUTMS_CAN_VESC) add_compile_definitions(QUTMS_CAN_VCU) -# add_compile_definitions(QUTMS_CAN_BMU) add_compile_definitions(QUTMS_CAN_RES) add_compile_definitions(QUTMS_CAN_SW) add_compile_definitions(QUTMS_CAN_EBS) -include_directories(include) -include_directories(${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Inc) +include_directories( + include + ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Inc + ${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/include/ + ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include +) set (SOURCES - src/can2ethernet_adapter.cpp src/tcp_client.cpp src/udp_client.cpp src/TritiumCAN.cpp src/SocketCAN.cpp ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_VCU.c ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_VESC.c - # ${CMAKE_SOURCE_DIR}/../../hardware/QUTMS_Embedded_Common/Src/CAN_BMU.c + ${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/src/can_interface.cpp ) SET_SOURCE_FILES_PROPERTIES(${SOURCES} PROPERTIES LANGUAGE CXX ) -include_directories(${CMAKE_SOURCE_DIR}/../../hardware/CAN_Common/include/) +# ROS 2 components +add_library(canbus_translator_component SHARED src/component_canbus_translator.cpp ${SOURCES}) +ament_target_dependencies(canbus_translator_component + "rclcpp" + "rclcpp_components" + "std_msgs" + "nav_msgs" + "driverless_msgs" +) +rclcpp_components_register_nodes(canbus_translator_component "canbus::CANTranslator") + +# ROS 2 nodes +add_executable(socket_translator_node src/node_socket_translator.cpp) +target_link_libraries(socket_translator_node canbus_translator_component) +ament_target_dependencies(socket_translator_node + "rclcpp" +) -# add_executable(canbus_translator_node src/node_canbus_translator.cpp ${SOURCES}) -# ament_target_dependencies(canbus_translator_node rclcpp driverless_msgs nav_msgs std_msgs) -# target_include_directories(canbus_translator_node PUBLIC include PUBLIC ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include) -# target_include_directories(canbus_translator_node PUBLIC -# $ -# $) +add_executable(tritium_translator_node src/node_tritium_translator.cpp) +target_link_libraries(tritium_translator_node canbus_translator_component) +ament_target_dependencies(tritium_translator_node + "rclcpp" +) -add_executable(canbus_translator_socket_node src/node_canbus_translator_socket.cpp ${SOURCES}) -ament_target_dependencies(canbus_translator_socket_node rclcpp driverless_msgs nav_msgs std_msgs) -target_include_directories(canbus_translator_socket_node PUBLIC include PUBLIC ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include) -target_include_directories(canbus_translator_socket_node PUBLIC - $ - $) +install(TARGETS + canbus_translator_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) install(TARGETS - canbus_translator_socket_node # canbus_translator_node - DESTINATION lib/${PROJECT_NAME} -) + socket_translator_node tritium_translator_node + DESTINATION lib/${PROJECT_NAME}) # config folder install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) + DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/src/hardware/canbus/config/canbus.yaml b/src/hardware/canbus/config/canbus.yaml index ccb43569d..694f36841 100644 --- a/src/hardware/canbus/config/canbus.yaml +++ b/src/hardware/canbus/config/canbus.yaml @@ -1,6 +1,5 @@ canbus_translator_node: ros__parameters: - ip: "192.168.2.125" - port: 20005 - interface: "faro_can0" + interface_name: "socketcan" # "tritium" + interface: "can0" # "192.168.2.125" # socketcan or tritium can base_frame: "base_footprint" diff --git a/src/hardware/canbus/include/SocketCAN.hpp b/src/hardware/canbus/include/SocketCAN.hpp index 8f2adf87f..41283ba95 100644 --- a/src/hardware/canbus/include/SocketCAN.hpp +++ b/src/hardware/canbus/include/SocketCAN.hpp @@ -8,11 +8,12 @@ #include #include "driverless_msgs/msg/can.hpp" +#include "interface.hpp" #include "rclcpp/rclcpp.hpp" const int SCAN_RECV_SIZE = 4096; -class SocketCAN { +class SocketCAN : public CANInterface { private: bool isConnected; int sock; @@ -29,5 +30,5 @@ class SocketCAN { void compose_socketcan_frame(driverless_msgs::msg::Can *msg, struct can_frame *frame); bool parse_socketcan_frame(struct can_frame *frame, driverless_msgs::msg::Can *msg); - ~SocketCAN(); + void deconstruct(); }; diff --git a/src/hardware/canbus/include/TritiumCAN.hpp b/src/hardware/canbus/include/TritiumCAN.hpp index cdb1ea013..092005932 100644 --- a/src/hardware/canbus/include/TritiumCAN.hpp +++ b/src/hardware/canbus/include/TritiumCAN.hpp @@ -8,12 +8,13 @@ #include #include "driverless_msgs/msg/can.hpp" +#include "interface.hpp" #include "tcp_client.hpp" #include "udp_client.hpp" const int CAN_MSG_LEN = (4 + 2 + 8); -class TritiumCAN { +class TritiumCAN : public CANInterface { std::shared_ptr rxClient; std::vector> txClients; std::shared_ptr tcpClient; @@ -25,14 +26,15 @@ class TritiumCAN { public: TritiumCAN(); - bool setup(std::string ip); + bool setup(std::string ip, rclcpp::Logger logger); + + void tx(driverless_msgs::msg::Can *msg, rclcpp::Logger logger); + std::shared_ptr> rx(rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - void tx(driverless_msgs::msg::Can *msg); - std::shared_ptr> rx(); bool process_can_msg(uint8_t *data, driverless_msgs::msg::Can *msg); std::shared_ptr> compose_tritium_tcp_header(); std::shared_ptr> compose_tritium_packet(std::vector msgs); std::shared_ptr> compose_tritum_can_bytes(driverless_msgs::msg::Can msg); - ~TritiumCAN(); + void deconstruct(); }; diff --git a/src/hardware/canbus/include/can2etherenet_adapter.hpp b/src/hardware/canbus/include/can2etherenet_adapter.hpp deleted file mode 100644 index e9238c020..000000000 --- a/src/hardware/canbus/include/can2etherenet_adapter.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once -#include -#include - -#include "tcp_client.hpp" - -class Can2Ethernet : TCPClient { - private: - std::string ip; - int port; - std::string address; - - public: - Can2Ethernet(std::string ip, int port); - void tx(uint32_t id, bool std_id, uint8_t* data); - std::shared_ptr> rx(); - ~Can2Ethernet() { this->_disconnect(); }; -}; - -/* --> Target Steering Angle --> Torque --> Regen --> TV --> EBS Activate --> GPS RTK --> State (Flow diagram in rules xoxo) - -<- Steering Angle -<- Current Torque -<- Current Regen -<- Current TV State -<- EBS State -<- Car State (RTD) -<- IMU + GPS -<- AS Mission -<- Go Button -*/ diff --git a/src/hardware/canbus/include/component_canbus_translator.hpp b/src/hardware/canbus/include/component_canbus_translator.hpp new file mode 100644 index 000000000..f87ffb535 --- /dev/null +++ b/src/hardware/canbus/include/component_canbus_translator.hpp @@ -0,0 +1,94 @@ +#ifndef CANBUS__COMPONENT_CANBUS_TRANSLATOR_HPP_ +#define CANBUS__COMPONENT_CANBUS_TRANSLATOR_HPP_ + +#include +#include +#include + +#include "CAN_DVL.h" +#include "CAN_RES.h" +#include "CAN_SW.h" +#include "CAN_VCU.h" +#include "CAN_VESC.h" +#include "QUTMS_can.h" +#include "interface.hpp" +#include "SocketCAN.hpp" +#include "TritiumCAN.hpp" +#include "can_interface.hpp" +#include "driverless_common/common.hpp" +#include "driverless_msgs/msg/can.hpp" +#include "driverless_msgs/msg/float32_stamped.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +using std::placeholders::_1; + +namespace canbus { + +const int NUM_CMUS = 8; +const int NUM_VOLTAGES = 14; +const int NUM_TEMPERATURES = 16; +const float WHEEL_DIAMETER = 0.4064; +const float AXLE_WIDTH = 1.4; + +// create array of CAN IDs we care about +std::vector canopen_ids = {RES_BOOT_UP_ID, RES_HEARTBEAT_ID, C5E_BOOT_UP_ID, C5E_POS_ID, + C5E_EMCY_ID, C5E_STATUS_ID, C5E_SRV_ID}; +std::vector can_ids = {SW_Heartbeat_ID, EBS_CTRL_Heartbeat_ID}; + +// names for the CAN IDs +std::vector canopen_names = {"RES_BOOT_UP_ID", "RES_HEARTBEAT_ID", "C5E_BOOT_UP_ID", "C5E_POS_ID", + "C5E_EMCY_ID", "C5E_STATUS_ID", "C5E_SRV_ID"}; +std::vector can_names = {"SW_Heartbeat_ID", "EBS_CTRL_Heartbeat_ID", "VCU_TransmitSteering_ID"}; + +class CANTranslator : public rclcpp::Node, public CanInterface { + private: + // can connection queue retrieval timer + rclcpp::TimerBase::SharedPtr timer_; + + // subscriber + rclcpp::Subscription::SharedPtr can_sub_; + + // publishers + rclcpp::Publisher::SharedPtr can_pub_; + rclcpp::Publisher::SharedPtr canopen_pub_; + // ADD PUBS FOR CAN TOPICS HERE + rclcpp::Publisher::SharedPtr steering_angle_pub_; + rclcpp::Publisher::SharedPtr velocity_pub_; + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr wss_velocity_pub1_; + rclcpp::Publisher::SharedPtr wss_velocity_pub2_; + rclcpp::Publisher::SharedPtr wss_velocity_pub3_; + rclcpp::Publisher::SharedPtr wss_velocity_pub4_; + std::vector::SharedPtr> wheel_speed_pubs_; + + rclcpp::CallbackGroup::SharedPtr timer_cb_group_; + rclcpp::CallbackGroup::SharedPtr sub_cb_group_; + + std::string ros_base_frame_; + + // can connection + std::shared_ptr can_interface_; + + // class variables for sensor data + float wheel_speeds_[4]; + float last_velocity_; + float last_steering_angle_; + + std::vector last_canopen_times_{canopen_ids.size(), rclcpp::Time(0)}; + std::vector last_can_times_{can_names.size(), rclcpp::Time(0)}; + + void update_twist(); + void canmsg_timer(); + void canmsg_callback(const driverless_msgs::msg::Can::SharedPtr msg) const; + + public: + CANTranslator(const rclcpp::NodeOptions & options); + ~CANTranslator() override; + + bool set_interface(); +}; + +} // namespace canbus + +#endif // CANBUS__COMPONENT_CANBUS_TRANSLATOR_HPP_ \ No newline at end of file diff --git a/src/hardware/canbus/include/interface.hpp b/src/hardware/canbus/include/interface.hpp new file mode 100644 index 000000000..7372a288a --- /dev/null +++ b/src/hardware/canbus/include/interface.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include +#include +#include + +#include "driverless_msgs/msg/can.hpp" +#include "rclcpp/rclcpp.hpp" + +class CANInterface { + public: + CANInterface() {}; + virtual bool setup(std::string interface, rclcpp::Logger logger) = 0; + + virtual void tx(driverless_msgs::msg::Can *msg, rclcpp::Logger logger) = 0; + virtual std::shared_ptr> rx(rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) = 0; + + virtual void deconstruct() = 0; +}; diff --git a/src/hardware/canbus/src/SocketCAN.cpp b/src/hardware/canbus/src/SocketCAN.cpp index 19b28e188..b57a19f9d 100644 --- a/src/hardware/canbus/src/SocketCAN.cpp +++ b/src/hardware/canbus/src/SocketCAN.cpp @@ -94,7 +94,6 @@ void SocketCAN::tx(driverless_msgs::msg::Can *msg, rclcpp::Logger logger) { std::shared_ptr> SocketCAN::rx(rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) { auto msgs = std::make_shared>(); - driverless_msgs::msg::Can rxMsg; if (this->isConnected) { // only need to try Rx if we're connected @@ -113,6 +112,7 @@ std::shared_ptr> SocketCAN::rx(rclcpp::Lo // convert appropriate bytes from socket into a can_frame struct can_frame *frame = (struct can_frame *)&(this->rxBuf[offset]); + driverless_msgs::msg::Can rxMsg; if (parse_socketcan_frame(frame, &rxMsg)) { msgs->push_back(rxMsg); } @@ -124,7 +124,7 @@ std::shared_ptr> SocketCAN::rx(rclcpp::Lo return msgs; } -SocketCAN::~SocketCAN() { +void SocketCAN::deconstruct() { if (this->sock != -1) { close(this->sock); this->isConnected = false; diff --git a/src/hardware/canbus/src/TritiumCAN.cpp b/src/hardware/canbus/src/TritiumCAN.cpp index a348749ba..510c7f39e 100644 --- a/src/hardware/canbus/src/TritiumCAN.cpp +++ b/src/hardware/canbus/src/TritiumCAN.cpp @@ -10,7 +10,7 @@ std::string groupAddr("239.255.60.60"); TritiumCAN::TritiumCAN() { this->isConnected = false; } -bool TritiumCAN::setup(std::string ip) { +bool TritiumCAN::setup(std::string ip, rclcpp::Logger logger) { int port = 4876; std::string localLoopbackAddr("127.0.0.1"); @@ -42,7 +42,7 @@ bool TritiumCAN::setup(std::string ip) { if (getnameinfo(ifAddrRaw, sizeof(struct sockaddr_in), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST) == 0) { std::string hostname(host); - std::cout << "Found interface: " << hostname << std::endl; + RCLCPP_INFO(logger, "Found interface: %s", hostname.c_str()); rxClient->join_multicast_group(inet_addr(groupAddr.c_str()), ifAddr->sin_addr.s_addr); @@ -66,7 +66,7 @@ bool TritiumCAN::setup(std::string ip) { return true; } -void TritiumCAN::tx(driverless_msgs::msg::Can *msg) { +void TritiumCAN::tx(driverless_msgs::msg::Can *msg, rclcpp::Logger logger) { // std::vector msgs; // msgs.push_back(*msg); @@ -81,7 +81,11 @@ void TritiumCAN::tx(driverless_msgs::msg::Can *msg) { } auto msgData = this->compose_tritum_can_bytes(*msg); - this->tcpClient->send_data(msgData); + bool success = this->tcpClient->send_data(msgData); + + if (!success) { + RCLCPP_WARN(logger, "CAN - Failed TX."); + } /* auto data = this->compose_tritium_packet(msgs); @@ -185,7 +189,8 @@ std::shared_ptr> TritiumCAN::compose_tritum_can_bytes(drive return result; } -std::shared_ptr> TritiumCAN::rx() { +std::shared_ptr> TritiumCAN::rx(rclcpp::Logger logger, + rclcpp::Clock::SharedPtr clock) { auto msgs = std::make_shared>(); // auto rxData = this->tcpClient->recieve_data(); @@ -200,7 +205,7 @@ std::shared_ptr> TritiumCAN::rx() { int numCANBytes = rxData->size() - 16; int numCAN = numCANBytes / CAN_MSG_LEN; - // std::cout << ", Num CAN: " << numCAN << std::endl; + RCLCPP_DEBUG_THROTTLE(logger, *clock, 500, "CAN - RX %d bytes", numCAN); for (int i = 0; i < numCAN; i++) { uint8_t data[CAN_MSG_LEN]; @@ -211,7 +216,7 @@ std::shared_ptr> TritiumCAN::rx() { if (result) { msgs->push_back(msg); } else { - std::cout << "RX - INVALID CAN PACKET " << std::endl; + RCLCPP_WARN(logger, "RX - INVALID CAN PACKET"); } } @@ -311,4 +316,4 @@ bool TritiumCAN::process_can_msg(uint8_t *data, driverless_msgs::msg::Can *msg) return true; } -TritiumCAN::~TritiumCAN() {} +void TritiumCAN::deconstruct() {} diff --git a/src/hardware/canbus/src/can2ethernet_adapter.cpp b/src/hardware/canbus/src/can2ethernet_adapter.cpp deleted file mode 100644 index a4f6a1f36..000000000 --- a/src/hardware/canbus/src/can2ethernet_adapter.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "can2etherenet_adapter.hpp" - -Can2Ethernet::Can2Ethernet(std::string ip, int port) { - this->ip = ip; - this->port = port; - this->address = ip + std::to_string(port); - this->setup(this->ip, this->port); -} - -void Can2Ethernet::tx(uint32_t id, bool std_id, uint8_t* data) { - auto d = std::make_shared>(); - uint8_t enetByte = 8; - enetByte = enetByte | ((std_id << 7) & 1); - d->emplace_back(enetByte); - for (int i = 0; i < 4; i++) { - uint8_t ourByte = (id >> (24 - i * 8)) & 0xFF; - d->emplace_back(ourByte); - } - for (int i = 0; i < 8; i++) { - d->emplace_back(data[i]); - } - auto didSend = this->send_data(d); - if (!didSend) { - std::cout << "Failed to send_data!" << std::endl; - } -} -std::shared_ptr> Can2Ethernet::rx() { return this->recieve_data(); } diff --git a/src/hardware/canbus/src/component_canbus_translator.cpp b/src/hardware/canbus/src/component_canbus_translator.cpp new file mode 100644 index 000000000..a59644a6c --- /dev/null +++ b/src/hardware/canbus/src/component_canbus_translator.cpp @@ -0,0 +1,206 @@ +#include "component_canbus_translator.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace canbus { + +void CANTranslator::update_twist() { + geometry_msgs::msg::TwistWithCovarianceStamped::UniquePtr twist_msg(new geometry_msgs::msg::TwistWithCovarianceStamped()); + // use last velocity and steering angle to update twist + twist_msg->header.stamp = this->now(); + twist_msg->header.frame_id = ros_base_frame_; // PARAMETERISE + twist_msg->twist.twist.linear.x = last_velocity_; + twist_msg->twist.twist.linear.y = 0.0; + twist_msg->twist.twist.angular.z = last_velocity_ * tan(last_steering_angle_) / AXLE_WIDTH; + twist_pub_->publish(std::move(twist_msg)); +} + +void CANTranslator::canmsg_timer() { + auto res = can_interface_->rx(this->get_logger(), this->get_clock()); + + for (auto &can_msg : *res) { + // convert to unique pointer + driverless_msgs::msg::Can::UniquePtr msg(new driverless_msgs::msg::Can()); + msg->id = can_msg.id; + msg->dlc = can_msg.dlc; + msg->data = can_msg.data; + + // only publish can messages with IDs we care about to not flood memory + uint32_t canopen_index = std::find(canopen_ids.begin(), canopen_ids.end(), msg->id) - canopen_ids.begin(); + if (canopen_index < canopen_ids.size()) { + // int age_ms = (this->now().nanoseconds() - last_canopen_times[canopen_index].nanoseconds()) / 1e6; + // RCLCPP_INFO(this->get_logger(), "Received %s, age: %d", canopen_names[canopen_index].c_str(), age_ms); + // last_canopen_times[canopen_index] = this->now(); + canopen_pub_->publish(std::move(msg)); + return; + } + uint32_t can_index = std::find(can_ids.begin(), can_ids.end(), msg->id) - can_ids.begin(); + if (can_index < can_ids.size()) { + // int age_ms = (this->now().nanoseconds() - last_can_times[can_index].nanoseconds()) / 1e6; + // RCLCPP_INFO(this->get_logger(), "Received %s, age: %d", can_names[can_index].c_str(), age_ms); + // last_can_times[can_index] = this->now(); + can_pub_->publish(std::move(msg)); + return; + } + + // CAN TRANSLATION OPTIONS + // Wheel speed velocity + uint32_t vesc_masked_id = (msg->id & ~0xFF) >> 8; + uint8_t vesc_id = msg->id & 0xFF; + if (vesc_id < 4) { + if (vesc_masked_id == VESC_CAN_PACKET_STATUS) { + // 3 wheel drive :/ + if (vesc_id == 1) { continue; } + + uint8_t data[8]; + this->copy_data(msg->data, data, 8); + // extract and publish RPM + int32_t rpm; + float current; + float duty; + Parse_VESC_CANPacketStatus(data, &rpm, ¤t, &duty); + + wheel_speeds_[vesc_id] = (rpm / (21.0 * 4.50)) * M_PI * WHEEL_DIAMETER / 60; + driverless_msgs::msg::Float32Stamped::UniquePtr speed_msg(new driverless_msgs::msg::Float32Stamped()); + speed_msg->header.stamp = this->now(); + speed_msg->header.frame_id = ros_base_frame_; // could change to wheel frame + speed_msg->data = wheel_speeds_[vesc_id]; + wheel_speed_pubs_[vesc_id]->publish(std::move(speed_msg)); + + float av_velocity = 0; + for (int i = 0; i < 4; i++) { + av_velocity += wheel_speeds_[i]; + } + av_velocity = av_velocity / 3; + + // publish velocity + driverless_msgs::msg::Float32Stamped::UniquePtr vel_msg(new driverless_msgs::msg::Float32Stamped()); + vel_msg->header.stamp = this->now(); + vel_msg->header.frame_id = ros_base_frame_; + vel_msg->data = av_velocity; + last_velocity_ = av_velocity; + velocity_pub_->publish(std::move(vel_msg)); + + // update twist msg with new velocity + update_twist(); + } + } + // Steering Angle + else if (msg->id == VCU_TransmitSteering_ID) { + // int age_ms = (this->now().nanoseconds() - last_can_times[2].nanoseconds()) / 1e6; + // RCLCPP_INFO(this->get_logger(), "Received %s, age: %d", can_names[2].c_str(), age_ms); + // last_can_times[2] = this->now(); + + // data vector to uint8_t array + uint8_t data[8]; + copy_data(msg->data, data, 8); + + int16_t steering_0_raw; + int16_t steering_1_raw; + uint16_t adc_0; + uint16_t adc_1; + + Parse_VCU_TransmitSteering(data, &steering_0_raw, &steering_1_raw, &adc_0, &adc_1); + // Log steering angle + RCLCPP_DEBUG(this->get_logger(), "Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", + steering_0_raw, steering_1_raw, adc_0, adc_1); + double steering_0 = steering_0_raw / 10.0; + double steering_1 = steering_1_raw / 10.0; + + driverless_msgs::msg::Float32Stamped::UniquePtr angle_msg(new driverless_msgs::msg::Float32Stamped()); + if (abs(steering_0 - steering_1) < 10) { + angle_msg->header.stamp = this->now(); + angle_msg->header.frame_id = ros_base_frame_; + angle_msg->data = steering_0; + last_steering_angle_ = steering_0; + steering_angle_pub_->publish(std::move(angle_msg)); + update_twist(); + } else { + RCLCPP_FATAL(this->get_logger(), + "MISMATCH: Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", + steering_0_raw, steering_1_raw, adc_0, adc_1); + } + } + } +} + +// ROS can msgs +void CANTranslator::canmsg_callback(const driverless_msgs::msg::Can::SharedPtr msg) const { + can_interface_->tx(msg.get(), this->get_logger()); +} + +bool CANTranslator::set_interface() { + // can_interface_ parameters + std::string interface = this->declare_parameter("interface", "can0"); + std::string interface_name = this->declare_parameter("interface_name", "socketcan"); + this->get_parameter("interface", interface); + this->get_parameter("base_frame", ros_base_frame_); + + RCLCPP_INFO(this->get_logger(), "Creating Connection on %s...", interface.c_str()); + + if (interface_name == "socketcan") { + can_interface_ = std::make_shared(); + } else if (interface_name == "tritium") { + can_interface_ = std::make_shared(); + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid interface_name: %s", interface_name.c_str()); + return false; + } + + while (!can_interface_->setup(interface, this->get_logger())) { + RCLCPP_ERROR(this->get_logger(), "Failed to create connection on %s. Retrying...", interface.c_str()); + // check for keyboard interrupt + if (!rclcpp::ok()) { + return false; + } + rclcpp::sleep_for(std::chrono::seconds(1)); + } + return true; +} + +CANTranslator::CANTranslator(const rclcpp::NodeOptions & options) : Node("canbus_translator_node", options) { + ros_base_frame_ = this->declare_parameter("base_frame", "base_link"); + + // set can interface + if (!set_interface()) { return; } + + timer_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + sub_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = sub_cb_group_; + + // retrieve can messages from queue + timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&CANTranslator::canmsg_timer, this), timer_cb_group_); + // subscribe to can messages from ROS system + can_sub_ = this->create_subscription( + "/can/canbus_carbound", QOS_ALL, std::bind(&CANTranslator::canmsg_callback, this, _1), sub_opt); + // publish can messages to ROS system + can_pub_ = this->create_publisher("/can/canbus_rosbound", QOS_ALL); + canopen_pub_ = this->create_publisher("/can/canopen_rosbound", QOS_ALL); + + // ADD PUBS FOR CAN TOPICS HERE + // Steering ang + steering_angle_pub_ = this->create_publisher("/vehicle/steering_angle", 10); + // Vehicle velocity + velocity_pub_ = this->create_publisher("/vehicle/velocity", 10); + + wss_velocity_pub1_ = this->create_publisher("/vehicle/wheel_speed1", 10); + wss_velocity_pub2_ = this->create_publisher("/vehicle/wheel_speed2", 10); + wss_velocity_pub3_ = this->create_publisher("/vehicle/wheel_speed3", 10); + wss_velocity_pub4_ = this->create_publisher("/vehicle/wheel_speed4", 10); + wheel_speed_pubs_.push_back(wss_velocity_pub1_); + wheel_speed_pubs_.push_back(wss_velocity_pub2_); + wheel_speed_pubs_.push_back(wss_velocity_pub3_); + wheel_speed_pubs_.push_back(wss_velocity_pub4_); + + // Twist + twist_pub_ = this->create_publisher("/vehicle/wheel_twist", 10); + + RCLCPP_INFO(this->get_logger(), "---CANBus Translator Node Initialised---"); +} + +CANTranslator::~CANTranslator() { can_interface_->deconstruct(); } + +} // namespace canbus + +RCLCPP_COMPONENTS_REGISTER_NODE(canbus::CANTranslator) diff --git a/src/hardware/canbus/src/node_canbus.cpp b/src/hardware/canbus/src/node_canbus.cpp deleted file mode 100644 index 39aed817b..000000000 --- a/src/hardware/canbus/src/node_canbus.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include - -#include "CAN_VESC.h" -#include "QUTMS_can.h" -#include "TritiumCAN.hpp" -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" -#include "rclcpp/rclcpp.hpp" - -using std::placeholders::_1; - -// create array of CAN IDs we care about -std::vector can_ids = { - 0x5F0, // C5_E stepper ID - BMU_TransmitVoltage_ID, - BMU_TransmitTemperature_ID, - (0x700 + RES_NODE_ID), // boot up message - RES_Heartbeat_ID, -}; - -class CanBus : public rclcpp::Node { - private: - // std::shared_ptr c; - std::shared_ptr tritiumCAN; - - rclcpp::Subscription::SharedPtr can_sub_; - rclcpp::Publisher::SharedPtr can_pub_; - - rclcpp::TimerBase::SharedPtr timer_; - - void canmsg_callback(const driverless_msgs::msg::Can::SharedPtr msg) const { this->tritiumCAN->tx(msg.get()); } - - void canmsg_timer() { - auto res = this->tritiumCAN->rx(); - - for (auto& msg : *res) { - uint32_t vesc_masked_id = (msg.id & ~0xFF) >> 8; - uint8_t vesc_id = msg.id & 0xFF; - uint32_t qutms_masked_id = msg.id & ~0xF; - // only publish messages with IDs we care about to not flood memory - if (std::find(can_ids.begin(), can_ids.end(), msg.id) != can_ids.end()) { - this->can_pub_->publish(msg); - } - // motor RPM CAN - else if (vesc_id < 4 && vesc_masked_id == VESC_CAN_PACKET_STATUS) { - this->can_pub_->publish(msg); - } - // VCU CAN - else if (qutms_masked_id == VCU_Heartbeat_ID || qutms_masked_id == VCU_TransmitSteering_ID || - qutms_masked_id == SW_Heartbeat_ID) { - this->can_pub_->publish(msg); - } - } - } - - public: - CanBus() : Node("canbus_translator_node") { - // Can2Ethernet parameters - std::string _ip = this->declare_parameter("ip", "192.168.2.125"); - int _port = this->declare_parameter("port", 20005); - this->get_parameter("ip", _ip); - this->get_parameter("port", _port); - - RCLCPP_INFO(this->get_logger(), "Creating Connection on %s:%i...", _ip.c_str(), _port); - // this->c = std::make_shared(_ip, _port); - this->tritiumCAN = std::make_shared(); - this->tritiumCAN->setup(_ip); - RCLCPP_INFO(this->get_logger(), "done!"); - - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&CanBus::canmsg_timer, this)); - - this->can_sub_ = this->create_subscription( - "/can/canbus_carbound", QOS_ALL, std::bind(&CanBus::canmsg_callback, this, _1)); - - this->can_pub_ = this->create_publisher("/can/canbus_rosbound", 10); - - RCLCPP_INFO(this->get_logger(), "---CANBus Translator Node Initialised---"); - } - - ~CanBus() { this->tritiumCAN->~TritiumCAN(); } -}; - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/hardware/canbus/src/node_canbus_translator.cpp b/src/hardware/canbus/src/node_canbus_translator.cpp deleted file mode 100644 index 2daa69c0e..000000000 --- a/src/hardware/canbus/src/node_canbus_translator.cpp +++ /dev/null @@ -1,239 +0,0 @@ -#include -#include -#include - -#include "CAN_BMU.h" -#include "CAN_DVL.h" -#include "CAN_RES.h" -#include "CAN_SW.h" -#include "CAN_VCU.h" -#include "CAN_VESC.h" -#include "QUTMS_can.h" -#include "TritiumCAN.hpp" -#include "can_interface.hpp" -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" -#include "driverless_msgs/msg/car_status.hpp" -#include "driverless_msgs/msg/res.hpp" -#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/u_int8.hpp" - -using std::placeholders::_1; - -const int NUM_CMUS = 8; -const int NUM_VOLTAGES = 14; -const int NUM_TEMPERATURES = 16; -const float WHEEL_DIAMETER = 0.4064; -const float AXLE_WIDTH = 1.4; - -void copy_data(const std::vector &vec, uint8_t *dest, size_t n) { - for (size_t i = 0; i < n; i++) { - dest[i] = vec[i]; - } -} - -// create array of CAN IDs we care about -std::vector canopen_ids = {RES_BOOT_UP_ID, RES_HEARTBEAT_ID, C5E_BOOT_UP_ID, C5E_POS_ID, - C5E_EMCY_ID, C5E_STATUS_ID, C5E_SRV_ID}; - -class CanBus : public rclcpp::Node { - private: - // can connection queue retrieval timer - rclcpp::TimerBase::SharedPtr timer_; - - // subscriber - rclcpp::Subscription::SharedPtr can_sub_; - - // publishers - rclcpp::Publisher::SharedPtr can_pub_; - rclcpp::Publisher::SharedPtr canopen_pub_; - // ADD PUBS FOR CAN TOPICS HERE - rclcpp::Publisher::SharedPtr steering_angle_pub_; - rclcpp::Publisher::SharedPtr velocity_pub_; - rclcpp::Publisher::SharedPtr bmu_status_pub_; - rclcpp::Publisher::SharedPtr twist_pub_; - - std::string ros_base_frame_; - - // can connection - std::shared_ptr tritiumCAN; - - // class variables for sensor data - float wheel_speeds[4]; - driverless_msgs::msg::CarStatus bmu_status; - geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; - float last_velocity; - float last_steering_angle; - - void update_twist() { - // use last velocity and steering angle to update twist - twist_msg.header.stamp = this->now(); - twist_msg.header.frame_id = ros_base_frame_; // PARAMETERISE - twist_msg.twist.twist.linear.x = last_velocity; - twist_msg.twist.twist.linear.y = 0.0; - twist_msg.twist.twist.angular.z = last_velocity * tan(last_steering_angle) / AXLE_WIDTH; - twist_pub_->publish(twist_msg); - } - - void canmsg_timer() { - auto res = this->tritiumCAN->rx(); - // std::cout << "RX - Num CAN: " << res->size() << std::endl; - - for (auto &msg : *res) { - // cout all msgs and timestamp and data - uint32_t qutms_masked_id = msg.id & ~0xF; - // only publish can messages with IDs we care about to not flood memory - if (std::find(canopen_ids.begin(), canopen_ids.end(), msg.id) != canopen_ids.end()) { - this->canopen_pub_->publish(msg); - } else if (qutms_masked_id == VCU_Heartbeat_ID || qutms_masked_id == SW_Heartbeat_ID) { - this->can_pub_->publish(msg); - } - - // CAN TRANSLATION OPTIONS - // Wheel speed velocity - uint32_t vesc_masked_id = (msg.id & ~0xFF) >> 8; - uint8_t vesc_id = msg.id & 0xFF; - if (vesc_id < 4) { - if (vesc_masked_id == VESC_CAN_PACKET_STATUS) { - uint8_t data[8]; - copy_data(msg.data, data, 8); - // extract and publish RPM - int32_t rpm; - float current; - float duty; - Parse_VESC_CANPacketStatus(data, &rpm, ¤t, &duty); - - wheel_speeds[vesc_id] = (rpm / (21.0 * 4.50)) * M_PI * WHEEL_DIAMETER / 60; - float av_velocity = 0; - for (int i = 0; i < 4; i++) { - av_velocity += this->wheel_speeds[i]; - } - av_velocity = av_velocity / 4; - - // publish velocity - std_msgs::msg::Float32 vel_msg; - vel_msg.data = av_velocity; - last_velocity = av_velocity; - this->velocity_pub_->publish(vel_msg); - - // update twist msg with new velocity - update_twist(); - } - } - // Steering Angle - else if (qutms_masked_id == VCU_TransmitSteering_ID) { - // data vector to uint8_t array - uint8_t data[8]; - copy_data(msg.data, data, 8); - - int16_t steering_0_raw; - int16_t steering_1_raw; - uint16_t adc_0; - uint16_t adc_1; - - Parse_VCU_TransmitSteering(data, &steering_0_raw, &steering_1_raw, &adc_0, &adc_1); - // Log steering angle - RCLCPP_DEBUG(this->get_logger(), "Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", - steering_0_raw, steering_1_raw, adc_0, adc_1); - double steering_0 = steering_0_raw / 10.0; - double steering_1 = steering_1_raw / 10.0; - - std_msgs::msg::Float32 angle_msg; - if (abs(steering_0 - steering_1) < 10) { - angle_msg.data = steering_0; - last_steering_angle = steering_0; - this->steering_angle_pub_->publish(angle_msg); - update_twist(); - } else { - RCLCPP_FATAL(this->get_logger(), - "MISMATCH: Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", - steering_0_raw, steering_1_raw, adc_0, adc_1); - } - } - // BMU - else if (msg.id == BMU_TransmitVoltage_ID) { - uint8_t cmu_id; - uint8_t packet_id; - uint16_t voltages[3]; - Parse_BMU_TransmitVoltage((uint8_t *)&msg.data[0], &cmu_id, &packet_id, voltages); - for (int i = 0; i < 3 && (packet_id * 3 + i) < NUM_VOLTAGES; i++) { - this->bmu_status.brick_data[cmu_id].voltages[packet_id * 3 + i] = voltages[i]; - } - this->bmu_status_pub_->publish(this->bmu_status); - - } else if (msg.id == BMU_TransmitTemperature_ID) { - uint8_t cmu_id; - uint8_t packet_id; - uint8_t temps[6]; - Parse_BMU_TransmitTemperatures((uint8_t *)&msg.data[0], &cmu_id, &packet_id, temps); - for (int i = 0; i < 6 && (packet_id * 6 + i) < NUM_TEMPERATURES; i++) { - bmu_status.brick_data[cmu_id].temperatures[packet_id * 6 + i] = temps[i]; - } - this->bmu_status_pub_->publish(this->bmu_status); - } - } - } - - // ROS can msgs - void canmsg_callback(const driverless_msgs::msg::Can::SharedPtr msg) const { this->tritiumCAN->tx(msg.get()); } - - public: - CanBus() : Node("canbus_translator_node") { - // Can2Ethernet parameters - // IP configured in TritiumCAN driver (x.x.2. subnet separate from car x.x.3. subnet for data passthru) - // https://tritiumcharging.com/wp-content/uploads/2020/11/TritiumCAN%E2%80%93Ethernet-Bridge_Manual.pdf - std::string ip = this->declare_parameter("ip", "192.168.2.126"); - int port = this->declare_parameter("port", 20005); - ros_base_frame_ = this->declare_parameter("base_frame", "base_link"); - - this->get_parameter("ip", ip); - this->get_parameter("port", port); - this->get_parameter("base_frame", ros_base_frame_); - - RCLCPP_INFO(this->get_logger(), "Creating Connection on %s:%i...", ip.c_str(), port); - this->tritiumCAN = std::make_shared(); - this->tritiumCAN->setup(ip); - RCLCPP_INFO(this->get_logger(), "done!"); - - // retrieve can messages from queue - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&CanBus::canmsg_timer, this)); - // subscribe to can messages from ROS system - this->can_sub_ = this->create_subscription( - "/can/canbus_carbound", QOS_ALL, std::bind(&CanBus::canmsg_callback, this, _1)); - // publish can messages to ROS system - this->can_pub_ = this->create_publisher("/can/canbus_rosbound", QOS_ALL); - this->canopen_pub_ = this->create_publisher("/can/canopen_rosbound", QOS_ALL); - - // ADD PUBS FOR CAN TOPICS HERE - // Steering ang - this->steering_angle_pub_ = this->create_publisher("/vehicle/steering_angle", 10); - // Vehicle velocity - this->velocity_pub_ = this->create_publisher("/vehicle/velocity", 10); - // Twist - this->twist_pub_ = - this->create_publisher("/vehicle/wheel_twist", 10); - // BMU - this->bmu_status_pub_ = this->create_publisher("/vehicle/bmu_status", 10); - this->bmu_status.brick_data = std::vector(NUM_CMUS); - for (int i = 0; i < NUM_CMUS; i++) { - this->bmu_status.brick_data[i].id = i + 1; - this->bmu_status.brick_data[i].voltages = std::vector(NUM_VOLTAGES); - this->bmu_status.brick_data[i].temperatures = std::vector(NUM_TEMPERATURES); - } - - RCLCPP_INFO(this->get_logger(), "---CANBus Translator Node Initialised---"); - } - - ~CanBus() { this->tritiumCAN->~TritiumCAN(); } -}; - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/src/hardware/canbus/src/node_canbus_translator_socket.cpp b/src/hardware/canbus/src/node_canbus_translator_socket.cpp deleted file mode 100644 index 4c948a979..000000000 --- a/src/hardware/canbus/src/node_canbus_translator_socket.cpp +++ /dev/null @@ -1,264 +0,0 @@ -#include -#include -#include - -// #include "CAN_BMU.h" -#include "CAN_DVL.h" -#include "CAN_RES.h" -#include "CAN_SW.h" -#include "CAN_VCU.h" -#include "CAN_VESC.h" -#include "QUTMS_can.h" -#include "SocketCAN.hpp" -#include "can_interface.hpp" -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/can.hpp" -#include "driverless_msgs/msg/car_status.hpp" -#include "driverless_msgs/msg/res.hpp" -#include "driverless_msgs/msg/wss_velocity.hpp" -#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/u_int8.hpp" - -using std::placeholders::_1; - -const int NUM_CMUS = 8; -const int NUM_VOLTAGES = 14; -const int NUM_TEMPERATURES = 16; -const float WHEEL_DIAMETER = 0.4064; -const float AXLE_WIDTH = 1.4; - -void copy_data(const std::vector &vec, uint8_t *dest, size_t n) { - for (size_t i = 0; i < n; i++) { - dest[i] = vec[i]; - } -} - -// create array of CAN IDs we care about -std::vector canopen_ids = {RES_BOOT_UP_ID, RES_HEARTBEAT_ID, C5E_BOOT_UP_ID, C5E_POS_ID, - C5E_EMCY_ID, C5E_STATUS_ID, C5E_SRV_ID}; - -class CanBus : public rclcpp::Node { - private: - // can connection queue retrieval timer - rclcpp::TimerBase::SharedPtr timer_; - - // subscriber - rclcpp::Subscription::SharedPtr can_sub_; - - // publishers - rclcpp::Publisher::SharedPtr can_pub_; - rclcpp::Publisher::SharedPtr canopen_pub_; - // ADD PUBS FOR CAN TOPICS HERE - rclcpp::Publisher::SharedPtr steering_angle_pub_; - rclcpp::Publisher::SharedPtr velocity_pub_; - rclcpp::Publisher::SharedPtr bmu_status_pub_; - rclcpp::Publisher::SharedPtr twist_pub_; - - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_; - rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_; - - std::string ros_base_frame_; - - // can connection - std::shared_ptr socketCAN; - // msg counter - uint32_t msg_counter = 0; - driverless_msgs::msg::Can last_tx_msg; - driverless_msgs::msg::Can last_rx_msg; - - // class variables for sensor data - float wheel_speeds[4]; - driverless_msgs::msg::CarStatus bmu_status; - geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; - float last_velocity; - float last_steering_angle; - - void update_twist() { - // use last velocity and steering angle to update twist - twist_msg.header.stamp = this->now(); - twist_msg.header.frame_id = ros_base_frame_; // PARAMETERISE - twist_msg.twist.twist.linear.x = last_velocity; - twist_msg.twist.twist.linear.y = 0.0; - twist_msg.twist.twist.angular.z = last_velocity * tan(last_steering_angle) / AXLE_WIDTH; - twist_pub_->publish(twist_msg); - } - - void canmsg_timer() { - auto res = this->socketCAN->rx(this->get_logger(), this->get_clock()); - - for (auto &msg : *res) { - // cout all msgs and timestamp and data - uint32_t qutms_masked_id = msg.id & ~0xF; - // only publish can messages with IDs we care about to not flood memory - if (std::find(canopen_ids.begin(), canopen_ids.end(), msg.id) != canopen_ids.end()) { - this->canopen_pub_->publish(msg); - } else if (qutms_masked_id == VCU_Heartbeat_ID || qutms_masked_id == SW_Heartbeat_ID || - qutms_masked_id == EBS_CTRL_Heartbeat_ID) { - this->can_pub_->publish(msg); - } - - // CAN TRANSLATION OPTIONS - // Wheel speed velocity - uint32_t vesc_masked_id = (msg.id & ~0xFF) >> 8; - uint8_t vesc_id = msg.id & 0xFF; - if (vesc_id < 4) { - if (vesc_masked_id == VESC_CAN_PACKET_STATUS) { - uint8_t data[8]; - copy_data(msg.data, data, 8); - // extract and publish RPM - int32_t rpm; - float current; - float duty; - Parse_VESC_CANPacketStatus(data, &rpm, ¤t, &duty); - - wheel_speeds[vesc_id] = (rpm / (21.0 * 4.50)) * M_PI * WHEEL_DIAMETER / 60; - float av_velocity = 0; - for (int i = 0; i < 4; i++) { - av_velocity += this->wheel_speeds[i]; - } - av_velocity = av_velocity / 4; - - // publish velocity - std_msgs::msg::Float32 vel_msg; - vel_msg.data = av_velocity; - last_velocity = av_velocity; - this->velocity_pub_->publish(vel_msg); - - // update twist msg with new velocity - update_twist(); - } - } - // Steering Angle - else if (qutms_masked_id == VCU_TransmitSteering_ID) { - // data vector to uint8_t array - uint8_t data[8]; - copy_data(msg.data, data, 8); - - int16_t steering_0_raw; - int16_t steering_1_raw; - uint16_t adc_0; - uint16_t adc_1; - - Parse_VCU_TransmitSteering(data, &steering_0_raw, &steering_1_raw, &adc_0, &adc_1); - // Log steering angle - RCLCPP_DEBUG(this->get_logger(), "Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", - steering_0_raw, steering_1_raw, adc_0, adc_1); - double steering_0 = steering_0_raw / 10.0; - double steering_1 = steering_1_raw / 10.0; - - std_msgs::msg::Float32 angle_msg; - if (abs(steering_0 - steering_1) < 10) { - angle_msg.data = steering_0; - last_steering_angle = steering_0; - this->steering_angle_pub_->publish(angle_msg); - update_twist(); - } else { - RCLCPP_FATAL(this->get_logger(), - "MISMATCH: Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", - steering_0_raw, steering_1_raw, adc_0, adc_1); - } - } - // BMU - // else if (msg.id == BMU_TransmitVoltage_ID) { - // uint8_t cmu_id; - // uint8_t packet_id; - // uint16_t voltages[3]; - // Parse_BMU_TransmitVoltage((uint8_t *)&msg.data[0], &cmu_id, &packet_id, voltages); - // for (int i = 0; i < 3 && (packet_id * 3 + i) < NUM_VOLTAGES; i++) { - // this->bmu_status.brick_data[cmu_id].voltages[packet_id * 3 + i] = voltages[i]; - // } - // this->bmu_status_pub_->publish(this->bmu_status); - - // } else if (msg.id == BMU_TransmitTemperature_ID) { - // uint8_t cmu_id; - // uint8_t packet_id; - // uint8_t temps[6]; - // Parse_BMU_TransmitTemperatures((uint8_t *)&msg.data[0], &cmu_id, &packet_id, temps); - // for (int i = 0; i < 6 && (packet_id * 6 + i) < NUM_TEMPERATURES; i++) { - // bmu_status.brick_data[cmu_id].temperatures[packet_id * 6 + i] = temps[i]; - // } - // this->bmu_status_pub_->publish(this->bmu_status); - // } - last_rx_msg = msg; - } - } - - // ROS can msgs - void canmsg_callback(const driverless_msgs::msg::Can::SharedPtr msg) const { - this->socketCAN->tx(msg.get(), this->get_logger()); - } - - public: - CanBus() : Node("canbus_translator_node") { - // socketCAN parameters - std::string interface = this->declare_parameter("interface", "faro_can0"); - ros_base_frame_ = this->declare_parameter("base_frame", "base_link"); - this->get_parameter("interface", interface); - this->get_parameter("base_frame", ros_base_frame_); - - RCLCPP_INFO(this->get_logger(), "Creating Connection on %s...", interface.c_str()); - this->socketCAN = std::make_shared(); - while (!this->socketCAN->setup(interface, this->get_logger())) { - RCLCPP_ERROR(this->get_logger(), "Failed to create connection on %s. Retrying...", interface.c_str()); - // check for keyboard interrupt - if (!rclcpp::ok()) { - return; - } - rclcpp::sleep_for(std::chrono::seconds(1)); - } - - callback_group_subscriber1_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_subscriber2_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - auto sub1_opt = rclcpp::SubscriptionOptions(); - sub1_opt.callback_group = callback_group_subscriber1_; - auto sub2_opt = rclcpp::SubscriptionOptions(); - sub2_opt.callback_group = callback_group_subscriber2_; - - // retrieve can messages from queue - this->timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&CanBus::canmsg_timer, this)); - // subscribe to can messages from ROS system - this->can_sub_ = this->create_subscription( - "/can/canbus_carbound", QOS_ALL, std::bind(&CanBus::canmsg_callback, this, _1), sub2_opt); - // publish can messages to ROS system - this->can_pub_ = this->create_publisher("/can/canbus_rosbound", QOS_ALL); - this->canopen_pub_ = this->create_publisher("/can/canopen_rosbound", QOS_ALL); - - // ADD PUBS FOR CAN TOPICS HERE - // Steering ang - this->steering_angle_pub_ = this->create_publisher("/vehicle/steering_angle", 10); - // Vehicle velocity - this->velocity_pub_ = this->create_publisher("/vehicle/velocity", 10); - // Twist - this->twist_pub_ = - this->create_publisher("/vehicle/wheel_twist", 10); - // BMU - this->bmu_status_pub_ = this->create_publisher("/vehicle/bmu_status", 10); - this->bmu_status.brick_data = std::vector(NUM_CMUS); - for (int i = 0; i < NUM_CMUS; i++) { - this->bmu_status.brick_data[i].id = i + 1; - this->bmu_status.brick_data[i].voltages = std::vector(NUM_VOLTAGES); - this->bmu_status.brick_data[i].temperatures = std::vector(NUM_TEMPERATURES); - } - - RCLCPP_INFO(this->get_logger(), "---CANBus Translator Node Initialised---"); - } - - ~CanBus() { this->socketCAN->~SocketCAN(); } -}; - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - - // rclcpp::spin(node); - // rclcpp::shutdown(); - return 0; -} diff --git a/src/hardware/canbus/src/node_socket_translator.cpp b/src/hardware/canbus/src/node_socket_translator.cpp new file mode 100644 index 000000000..71b2ca435 --- /dev/null +++ b/src/hardware/canbus/src/node_socket_translator.cpp @@ -0,0 +1,17 @@ +#include "component_canbus_translator.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/src/hardware/canbus/src/node_tritium_translator.cpp b/src/hardware/canbus/src/node_tritium_translator.cpp new file mode 100644 index 000000000..71b2ca435 --- /dev/null +++ b/src/hardware/canbus/src/node_tritium_translator.cpp @@ -0,0 +1,17 @@ +#include "component_canbus_translator.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} From 35b9eb12500b7f851857249e4470ccc1e18e5bbe Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:54:16 +1000 Subject: [PATCH 6/8] compose velocity controller. adjust velocity gain from comp --- .../velocity_controller/CMakeLists.txt | 39 +++-- .../config/velocity_controller.yaml | 2 +- .../include/component_velocity_controller.hpp | 65 ++++++++ .../include/node_velocity_controller.hpp | 57 ------- .../src/component_velocity_controller.cpp | 144 +++++++++++++++++ .../src/node_velocity_controller.cpp | 146 ++---------------- 6 files changed, 248 insertions(+), 205 deletions(-) create mode 100644 src/control/velocity_controller/include/component_velocity_controller.hpp delete mode 100644 src/control/velocity_controller/include/node_velocity_controller.hpp create mode 100644 src/control/velocity_controller/src/component_velocity_controller.cpp diff --git a/src/control/velocity_controller/CMakeLists.txt b/src/control/velocity_controller/CMakeLists.txt index b25b108ac..e30dd47a5 100644 --- a/src/control/velocity_controller/CMakeLists.txt +++ b/src/control/velocity_controller/CMakeLists.txt @@ -12,23 +12,40 @@ find_package(ackermann_msgs REQUIRED) find_package(driverless_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) -include_directories(include) +include_directories( + include + ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include +) SET_SOURCE_FILES_PROPERTIES(${SOURCES} PROPERTIES LANGUAGE CXX ) -add_executable(velocity_controller_node src/node_velocity_controller.cpp ${SOURCES}) -target_include_directories(velocity_controller_node PUBLIC ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include) -target_include_directories(velocity_controller_node PUBLIC - $ - $) +# ROS 2 components +add_library(velocity_controller_component SHARED src/component_velocity_controller.cpp) +ament_target_dependencies(velocity_controller_component + "rclcpp" + "rclcpp_components" + "rcl_interfaces" + "std_msgs" + "nav_msgs" + "ackermann_msgs" + "driverless_msgs" +) +rclcpp_components_register_nodes(velocity_controller_component "velocity_controller::VelocityController") +# ROS 2 nodes +add_executable(velocity_controller_node src/node_velocity_controller.cpp) +target_link_libraries(velocity_controller_node velocity_controller_component) ament_target_dependencies(velocity_controller_node - rclcpp - rcl_interfaces - nav_msgs - ackermann_msgs - driverless_msgs) + "rclcpp" +) + +install(TARGETS + velocity_controller_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) install(TARGETS velocity_controller_node DESTINATION lib/${PROJECT_NAME}) diff --git a/src/control/velocity_controller/config/velocity_controller.yaml b/src/control/velocity_controller/config/velocity_controller.yaml index dbc4cd9ae..bdc5608eb 100644 --- a/src/control/velocity_controller/config/velocity_controller.yaml +++ b/src/control/velocity_controller/config/velocity_controller.yaml @@ -1,6 +1,6 @@ velocity_controller_node: ros__parameters: - Kp: 0.05 + Kp: 0.15 Ki: 0.0 max_integral_torque: 0.0 histerisis_kickin_ms: 0.0 diff --git a/src/control/velocity_controller/include/component_velocity_controller.hpp b/src/control/velocity_controller/include/component_velocity_controller.hpp new file mode 100644 index 000000000..4200f4300 --- /dev/null +++ b/src/control/velocity_controller/include/component_velocity_controller.hpp @@ -0,0 +1,65 @@ +#ifndef VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_ +#define VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_ + +#include + +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "driverless_common/common.hpp" +#include "driverless_msgs/msg/state.hpp" +#include "driverless_msgs/msg/float32_stamped.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rclcpp/rclcpp.hpp" + +using std::placeholders::_1; + +namespace velocity_controller { + +class VelocityController : public rclcpp::Node { + private: + float Kp_ = 0; + float Ki_ = 0; + float max_integral_torque_ = 0; + float histerisis_kickin_ms_ = 0; + float histerisis_reset_ms_ = 0; + float min_time_to_max_accel_sec_ = 0; + float max_accel_per_tick_ = 0; + + float integral_error_ = 0; + + rclcpp::TimerBase::SharedPtr controller_timer_; + + rclcpp::Publisher::SharedPtr accel_pub_; + rclcpp::Subscription::SharedPtr ackermann_sub_; + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + + std::shared_ptr param_event_handler_; + std::shared_ptr param_cb_handle_; + + const int loop_ms_ = 10; + + // Enable motors logic + driverless_msgs::msg::State::SharedPtr state_ = std::make_shared(); + bool motors_enabled_ = false; + bool received_velocity_ = false; + bool received_ackermann_ = false; + + float avg_velocity_; + ackermann_msgs::msg::AckermannDriveStamped::SharedPtr target_ackermann_ = std::make_shared(); + float prev_accel_ = 0; + + public: + VelocityController(const rclcpp::NodeOptions & options); + + void update_parameters(const rcl_interfaces::msg::ParameterEvent& event); + + void ackermann_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg); + void velocity_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg); + void state_callback(const driverless_msgs::msg::State::SharedPtr msg); + + void controller_callback(); +}; + +} // namespace vehicle_supervisor + +#endif // VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_ \ No newline at end of file diff --git a/src/control/velocity_controller/include/node_velocity_controller.hpp b/src/control/velocity_controller/include/node_velocity_controller.hpp deleted file mode 100644 index 10d933017..000000000 --- a/src/control/velocity_controller/include/node_velocity_controller.hpp +++ /dev/null @@ -1,57 +0,0 @@ -#include - -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" -#include "driverless_common/common.hpp" -#include "driverless_msgs/msg/state.hpp" -#include "rcl_interfaces/msg/set_parameters_result.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float32.hpp" - -using std::placeholders::_1; - -class Velocity_Controller : public rclcpp::Node { - private: - float Kp = 0; - float Ki = 0; - float max_integral_torque = 0; - float histerisis_kickin_ms = 0; - float histerisis_reset_ms = 0; - float min_time_to_max_accel_sec = 0; - float max_accel_per_tick = 0; - - float integral_error = 0; - - rclcpp::Publisher::SharedPtr accel_pub; - rclcpp::Subscription::SharedPtr ackermann_sub; - rclcpp::Subscription::SharedPtr state_sub; - rclcpp::Subscription::SharedPtr velocity_sub; - rclcpp::TimerBase::SharedPtr controller_timer; - - std::shared_ptr param_event_handler; - std::shared_ptr param_cb_handle; - - const int loop_ms = 10; - - // Enable motors logic - driverless_msgs::msg::State state; - bool motors_enabled = false; - bool received_velocity = false; - bool received_ackermann = false; - - float avg_velocity; - ackermann_msgs::msg::AckermannDriveStamped target_ackermann; - ackermann_msgs::msg::AckermannDriveStamped prev_accel_cmd; - - public: - Velocity_Controller(); - - void update_parameters(const rcl_interfaces::msg::ParameterEvent& event); - - void ackermann_callback(const ackermann_msgs::msg::AckermannDriveStamped msg); - - void velocity_callback(const std_msgs::msg::Float32 msg); - - void state_callback(const driverless_msgs::msg::State msg); - - void controller_callback(); -}; diff --git a/src/control/velocity_controller/src/component_velocity_controller.cpp b/src/control/velocity_controller/src/component_velocity_controller.cpp new file mode 100644 index 000000000..36240f41f --- /dev/null +++ b/src/control/velocity_controller/src/component_velocity_controller.cpp @@ -0,0 +1,144 @@ +#include "component_velocity_controller.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace velocity_controller { + +VelocityController::VelocityController(const rclcpp::NodeOptions & options) : Node("velocity_controller_node", options) { + // PID controller parameters + this->declare_parameter("Kp", 0.05); + this->declare_parameter("Ki", 0); + this->declare_parameter("max_integral_torque", 0); + this->declare_parameter("histerisis_kick_ms", 0); + this->declare_parameter("histerisis_reset_ms", 0); + this->declare_parameter("min_time_to_max_accel_sec", 2.0); + + this->update_parameters(rcl_interfaces::msg::ParameterEvent()); + + if (Kp_ == 0) { + RCLCPP_ERROR(this->get_logger(), "Please provide parameters!"); + } + + // State updates + state_sub_ = this->create_subscription( + "/system/as_status", QOS_LATEST, std::bind(&VelocityController::state_callback, this, _1)); + + // Ackermann + ackermann_sub_ = this->create_subscription( + "/control/driving_command", QOS_ALL, std::bind(&VelocityController::ackermann_callback, this, _1)); + + // Velocity updates + velocity_sub_ = this->create_subscription( + "/vehicle/velocity", QOS_LATEST, std::bind(&VelocityController::velocity_callback, this, _1)); + + // Control loop -> 10ms so runs at double speed heartbeats are sent at + controller_timer_ = this->create_wall_timer(std::chrono::milliseconds(loop_ms_), + std::bind(&VelocityController::controller_callback, this)); + + // Acceleration command publisher (to Supervisor so it can be sent in the DVL heartbeat) + accel_pub_ = this->create_publisher("/control/accel_command", 10); + + // Param callback + param_event_handler_ = std::make_shared(this); + param_cb_handle_ = param_event_handler_->add_parameter_event_callback( + std::bind(&VelocityController::update_parameters, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "---Velocity Controller Node Initialised---"); +} + +void VelocityController::update_parameters(const rcl_interfaces::msg::ParameterEvent& event) { + (void)event; + + this->get_parameter("Kp", Kp_); + this->get_parameter("Ki", Ki_); + this->get_parameter("max_integral_torque", max_integral_torque_); + this->get_parameter("histerisis_kickin_ms", histerisis_kickin_ms_); + this->get_parameter("histerisis_reset_ms", histerisis_reset_ms_); + this->get_parameter("min_time_to_max_accel_sec", min_time_to_max_accel_sec_); + max_accel_per_tick_ = loop_ms_ / (1000 * min_time_to_max_accel_sec_); + + RCLCPP_DEBUG(this->get_logger(), + "Kp: %f Ki: %f max_integral_torque: %f histerisis_kickin_ms: %f histerisis_reset_ms: %f", Kp_, + Ki_, max_integral_torque_, histerisis_kickin_ms_, histerisis_reset_ms_); +} + +void VelocityController::ackermann_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg) { + target_ackermann_ = msg; + received_ackermann_ = true; +} + +void VelocityController::velocity_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg) { + avg_velocity_ = msg->data; + received_velocity_ = true; +} + +void VelocityController::controller_callback() { + if (!motors_enabled_) { + RCLCPP_INFO_ONCE(this->get_logger(), "Motors not enabled, awaiting State::DRIVING"); + return; + } + if (!received_velocity_ || !received_ackermann_) { + RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for target and current velocities"); + return; + } + RCLCPP_INFO_ONCE(this->get_logger(), + "Motors enabled, Received target and current velocities\n - Starting control loop"); + + // calculate error + float error = target_ackermann_->drive.speed - avg_velocity_; + integral_error_ += error; + + // clip the integral error based on max_integral_torque_ + if (integral_error_ < 0) { + integral_error_ = 0; + } else if (integral_error_ > (max_integral_torque_ / Ki_)) { + integral_error_ = max_integral_torque_ / Ki_; + } + + if (avg_velocity_ < histerisis_reset_ms_) { + integral_error_ = 0; + } + + // calculate control variable + float p_term = Kp_ * error; + float i_term = Ki_ * integral_error_; + + float accel = p_term; + if (avg_velocity_ > histerisis_kickin_ms_) { + accel += i_term; + } + + if ((accel - prev_accel_) > max_accel_per_tick_) { + accel = prev_accel_ + max_accel_per_tick_; + } + + // limit output accel to be between -1 (braking) and 1 (accel) + if (accel > 1) { + accel = 1; + } else if (accel < -1) { + accel = -1; + } + + // create control ackermann based off desired and calculated acceleration + ackermann_msgs::msg::AckermannDriveStamped::UniquePtr accel_cmd(new ackermann_msgs::msg::AckermannDriveStamped()); + accel_cmd->header.stamp = this->now(); + accel_cmd->drive = target_ackermann_->drive; + accel_cmd->drive.acceleration = accel; + + // publish accel + accel_pub_->publish(std::move(accel_cmd)); + prev_accel_ = accel; +} + +void VelocityController::state_callback(const driverless_msgs::msg::State::SharedPtr msg) { + state_ = msg; + if (msg->state == driverless_msgs::msg::State::DRIVING && msg->navigation_ready + && msg->mission != driverless_msgs::msg::State::INSPECTION) { + motors_enabled_ = true; + } else { + motors_enabled_ = false; + } +} + +} // namespace velocity_controller + +RCLCPP_COMPONENTS_REGISTER_NODE(velocity_controller::VelocityController) \ No newline at end of file diff --git a/src/control/velocity_controller/src/node_velocity_controller.cpp b/src/control/velocity_controller/src/node_velocity_controller.cpp index e7887d4ad..9ad2a9e9e 100644 --- a/src/control/velocity_controller/src/node_velocity_controller.cpp +++ b/src/control/velocity_controller/src/node_velocity_controller.cpp @@ -1,143 +1,17 @@ -#include "node_velocity_controller.hpp" +#include "component_velocity_controller.hpp" +#include "rclcpp/rclcpp.hpp" -Velocity_Controller::Velocity_Controller() : Node("velocity_controller_node") { - // PID controller parameters - this->declare_parameter("Kp", 1.0); - this->declare_parameter("Ki", 0); - this->declare_parameter("max_integral_torque", 0); - this->declare_parameter("histerisis_kick_ms", 0); - this->declare_parameter("histerisis_reset_ms", 0); - this->declare_parameter("min_time_to_max_accel_sec", 0); - - this->update_parameters(rcl_interfaces::msg::ParameterEvent()); - - if (this->Kp == 0) { - RCLCPP_ERROR(this->get_logger(), "Please provide parameters!"); - } - - // State updates - this->state_sub = this->create_subscription( - "/system/as_status", QOS_LATEST, std::bind(&Velocity_Controller::state_callback, this, _1)); - - // Ackermann - this->ackermann_sub = this->create_subscription( - "/control/driving_command", QOS_ALL, std::bind(&Velocity_Controller::ackermann_callback, this, _1)); - - // Velocity updates - this->velocity_sub = this->create_subscription( - "/vehicle/velocity", QOS_LATEST, std::bind(&Velocity_Controller::velocity_callback, this, _1)); - - // Control loop -> 10ms so runs at double speed heartbeats are sent at - this->controller_timer = this->create_wall_timer(std::chrono::milliseconds(this->loop_ms), - std::bind(&Velocity_Controller::controller_callback, this)); - - // Acceleration command publisher (to Supervisor so it can be sent in the DVL heartbeat) - this->accel_pub = this->create_publisher("/control/accel_command", 10); - - // Param callback - this->param_event_handler = std::make_shared(this); - this->param_cb_handle = this->param_event_handler->add_parameter_event_callback( - std::bind(&Velocity_Controller::update_parameters, this, std::placeholders::_1)); - - RCLCPP_INFO(this->get_logger(), "---Velocity Controller Node Initialised---"); -} - -void Velocity_Controller::update_parameters(const rcl_interfaces::msg::ParameterEvent& event) { - (void)event; - - this->get_parameter("Kp", this->Kp); - this->get_parameter("Ki", this->Ki); - this->get_parameter("max_integral_torque", this->max_integral_torque); - this->get_parameter("histerisis_kickin_ms", this->histerisis_kickin_ms); - this->get_parameter("histerisis_reset_ms", this->histerisis_reset_ms); - this->get_parameter("min_time_to_max_accel_sec", this->min_time_to_max_accel_sec); - this->max_accel_per_tick = this->loop_ms / (1000 * this->min_time_to_max_accel_sec); - - RCLCPP_DEBUG(this->get_logger(), - "Kp: %f Ki: %f max_integral_torque: %f histerisis_kickin_ms: %f histerisis_reset_ms: %f", this->Kp, - this->Ki, this->max_integral_torque, this->histerisis_kickin_ms, this->histerisis_reset_ms); -} - -void Velocity_Controller::ackermann_callback(const ackermann_msgs::msg::AckermannDriveStamped msg) { - this->target_ackermann = msg; - this->received_ackermann = true; -} - -void Velocity_Controller::velocity_callback(const std_msgs::msg::Float32 msg) { - this->avg_velocity = msg.data; - this->received_velocity = true; -} - -void Velocity_Controller::controller_callback() { - if (!this->motors_enabled) { - RCLCPP_INFO_ONCE(this->get_logger(), "Motors not enabled, awaiting State::DRIVING"); - return; - } - if (!this->received_velocity || !this->received_ackermann) { - RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for target and current velocities"); - return; - } - RCLCPP_INFO_ONCE(this->get_logger(), - "Motors enabled, Received target and current velocities\n - Starting control loop"); - - // calculate error - float error = this->target_ackermann.drive.speed - this->avg_velocity; - this->integral_error += error; - - // clip the integral error based on max_integral_torque - if (this->integral_error < 0) { - this->integral_error = 0; - } else if (this->integral_error > (this->max_integral_torque / this->Ki)) { - this->integral_error = this->max_integral_torque / this->Ki; - } - - if (this->avg_velocity < this->histerisis_reset_ms) { - this->integral_error = 0; - } - - // calculate control variable - float p_term = this->Kp * error; - float i_term = this->Ki * this->integral_error; - - float accel = p_term; - if (this->avg_velocity > this->histerisis_kickin_ms) { - accel += i_term; - } - - if ((accel - this->prev_accel_cmd.drive.acceleration) > this->max_accel_per_tick) { - accel = this->prev_accel_cmd.drive.acceleration + this->max_accel_per_tick; - } - - // limit output accel to be between -1 (braking) and 1 (accel) - if (accel > 1) { - accel = 1; - } else if (accel < -1) { - accel = -1; - } - - // create control ackermann based off desired and calculated acceleration - ackermann_msgs::msg::AckermannDriveStamped accel_cmd; - accel_cmd.header.stamp = this->now(); - accel_cmd.drive = this->target_ackermann.drive; - accel_cmd.drive.acceleration = accel; +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); - // publish accel - this->accel_pub->publish(accel_cmd); - this->prev_accel_cmd = accel_cmd; -} + rclcpp::NodeOptions options; + auto node = std::make_shared(options); -void Velocity_Controller::state_callback(const driverless_msgs::msg::State msg) { - this->state = msg; - if (msg.state == driverless_msgs::msg::State::DRIVING && msg.navigation_ready) { - this->motors_enabled = true; - } else { - this->motors_enabled = false; - } -} + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); rclcpp::shutdown(); + return 0; } From 7873efbd048bd4b89088b8e086a1437eb8c5e113 Mon Sep 17 00:00:00 2001 From: Alastair Date: Tue, 5 Mar 2024 17:54:41 +1000 Subject: [PATCH 7/8] machine launch with composed nodes --- .../roscube_machine/launch/machine.launch.py | 69 ++++++++++++------- 1 file changed, 45 insertions(+), 24 deletions(-) diff --git a/src/machines/roscube_machine/launch/machine.launch.py b/src/machines/roscube_machine/launch/machine.launch.py index 2312489d1..516923397 100644 --- a/src/machines/roscube_machine/launch/machine.launch.py +++ b/src/machines/roscube_machine/launch/machine.launch.py @@ -2,47 +2,68 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode def generate_launch_description(): - return LaunchDescription( - [ - Node( - package="canbus", - executable="canbus_translator_socket_node", - parameters=[ - get_package_share_path("canbus") / "config" / "canbus.yaml", - ], - ), - Node( + # safety critical signals should use intra process comms for low latency + scs_container = ComposableNodeContainer( + name="critcial_signal_container", + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( package="vehicle_supervisor", - executable="vehicle_supervisor_node", + plugin="vehicle_supervisor::ASSupervisorLaunch", + name="vehicle_supervisor_launch_node", parameters=[ {"manual_override": False}, ], + extra_arguments=[{"use_intra_process_comms": True}], ), - Node( - package="rosboard", - executable="rosboard_node", - ), - Node( - package="driverless_common", - executable="display", - ), - Node( + ComposableNode( package="steering_actuator", - executable="steering_actuator_test_node", + plugin="steering_actuator::SteeringActuator", + name="steering_actuator_node", parameters=[ get_package_share_path("steering_actuator") / "config" / "steering.yaml", ], + extra_arguments=[{"use_intra_process_comms": True}], ), - Node( + ComposableNode( package="velocity_controller", - executable="velocity_controller_node", + plugin="velocity_controller::VelocityController", + name="velocity_controller_node", parameters=[ get_package_share_path("velocity_controller") / "config" / "velocity_controller.yaml", ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="canbus", + plugin="canbus::CANTranslator", + name="canbus_translator_node", + parameters=[ + get_package_share_path("canbus") / "config" / "canbus.yaml", + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="both", + ) + + return LaunchDescription( + [ + scs_container, + Node( + package="rosboard", + executable="rosboard_node", + ), + Node( + package="driverless_common", + executable="display", ), Node( package="lidar_pipeline", From 907b6407b0748bbdefe24d25802f0e6bd882d468 Mon Sep 17 00:00:00 2001 From: Alastair Bradford Date: Wed, 20 Mar 2024 14:53:00 +1100 Subject: [PATCH 8/8] pre-commit --- .../include/component_velocity_controller.hpp | 11 ++--- .../src/component_velocity_controller.cpp | 15 ++++--- .../include/component_canbus_translator.hpp | 6 +-- src/hardware/canbus/include/interface.hpp | 5 ++- .../src/component_canbus_translator.cpp | 27 +++++++----- .../include/component_steering_actuator.hpp | 6 +-- .../src/component_steering_actuator.cpp | 23 +++++----- .../include/component_supervisor.hpp | 9 ++-- .../include/component_supervisor_launch.hpp | 6 +-- .../src/component_supervisor.cpp | 44 ++++++++----------- .../src/component_supervisor_launch.cpp | 7 ++- 11 files changed, 82 insertions(+), 77 deletions(-) diff --git a/src/control/velocity_controller/include/component_velocity_controller.hpp b/src/control/velocity_controller/include/component_velocity_controller.hpp index 4200f4300..11d04b7bf 100644 --- a/src/control/velocity_controller/include/component_velocity_controller.hpp +++ b/src/control/velocity_controller/include/component_velocity_controller.hpp @@ -5,8 +5,8 @@ #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "driverless_common/common.hpp" -#include "driverless_msgs/msg/state.hpp" #include "driverless_msgs/msg/float32_stamped.hpp" +#include "driverless_msgs/msg/state.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/rclcpp.hpp" @@ -45,11 +45,12 @@ class VelocityController : public rclcpp::Node { bool received_ackermann_ = false; float avg_velocity_; - ackermann_msgs::msg::AckermannDriveStamped::SharedPtr target_ackermann_ = std::make_shared(); + ackermann_msgs::msg::AckermannDriveStamped::SharedPtr target_ackermann_ = + std::make_shared(); float prev_accel_ = 0; public: - VelocityController(const rclcpp::NodeOptions & options); + VelocityController(const rclcpp::NodeOptions& options); void update_parameters(const rcl_interfaces::msg::ParameterEvent& event); @@ -60,6 +61,6 @@ class VelocityController : public rclcpp::Node { void controller_callback(); }; -} // namespace vehicle_supervisor +} // namespace velocity_controller -#endif // VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_ \ No newline at end of file +#endif // VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_ diff --git a/src/control/velocity_controller/src/component_velocity_controller.cpp b/src/control/velocity_controller/src/component_velocity_controller.cpp index 36240f41f..d2f299ebc 100644 --- a/src/control/velocity_controller/src/component_velocity_controller.cpp +++ b/src/control/velocity_controller/src/component_velocity_controller.cpp @@ -1,9 +1,10 @@ #include "component_velocity_controller.hpp" + #include "rclcpp_components/register_node_macro.hpp" namespace velocity_controller { -VelocityController::VelocityController(const rclcpp::NodeOptions & options) : Node("velocity_controller_node", options) { +VelocityController::VelocityController(const rclcpp::NodeOptions& options) : Node("velocity_controller_node", options) { // PID controller parameters this->declare_parameter("Kp", 0.05); this->declare_parameter("Ki", 0); @@ -32,7 +33,7 @@ VelocityController::VelocityController(const rclcpp::NodeOptions & options) : No // Control loop -> 10ms so runs at double speed heartbeats are sent at controller_timer_ = this->create_wall_timer(std::chrono::milliseconds(loop_ms_), - std::bind(&VelocityController::controller_callback, this)); + std::bind(&VelocityController::controller_callback, this)); // Acceleration command publisher (to Supervisor so it can be sent in the DVL heartbeat) accel_pub_ = this->create_publisher("/control/accel_command", 10); @@ -57,8 +58,8 @@ void VelocityController::update_parameters(const rcl_interfaces::msg::ParameterE max_accel_per_tick_ = loop_ms_ / (1000 * min_time_to_max_accel_sec_); RCLCPP_DEBUG(this->get_logger(), - "Kp: %f Ki: %f max_integral_torque: %f histerisis_kickin_ms: %f histerisis_reset_ms: %f", Kp_, - Ki_, max_integral_torque_, histerisis_kickin_ms_, histerisis_reset_ms_); + "Kp: %f Ki: %f max_integral_torque: %f histerisis_kickin_ms: %f histerisis_reset_ms: %f", Kp_, Ki_, + max_integral_torque_, histerisis_kickin_ms_, histerisis_reset_ms_); } void VelocityController::ackermann_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg) { @@ -131,8 +132,8 @@ void VelocityController::controller_callback() { void VelocityController::state_callback(const driverless_msgs::msg::State::SharedPtr msg) { state_ = msg; - if (msg->state == driverless_msgs::msg::State::DRIVING && msg->navigation_ready - && msg->mission != driverless_msgs::msg::State::INSPECTION) { + if (msg->state == driverless_msgs::msg::State::DRIVING && msg->navigation_ready && + msg->mission != driverless_msgs::msg::State::INSPECTION) { motors_enabled_ = true; } else { motors_enabled_ = false; @@ -141,4 +142,4 @@ void VelocityController::state_callback(const driverless_msgs::msg::State::Share } // namespace velocity_controller -RCLCPP_COMPONENTS_REGISTER_NODE(velocity_controller::VelocityController) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(velocity_controller::VelocityController) diff --git a/src/hardware/canbus/include/component_canbus_translator.hpp b/src/hardware/canbus/include/component_canbus_translator.hpp index f87ffb535..bad4ce8e6 100644 --- a/src/hardware/canbus/include/component_canbus_translator.hpp +++ b/src/hardware/canbus/include/component_canbus_translator.hpp @@ -11,7 +11,6 @@ #include "CAN_VCU.h" #include "CAN_VESC.h" #include "QUTMS_can.h" -#include "interface.hpp" #include "SocketCAN.hpp" #include "TritiumCAN.hpp" #include "can_interface.hpp" @@ -19,6 +18,7 @@ #include "driverless_msgs/msg/can.hpp" #include "driverless_msgs/msg/float32_stamped.hpp" #include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "interface.hpp" #include "rclcpp/rclcpp.hpp" using std::placeholders::_1; @@ -83,7 +83,7 @@ class CANTranslator : public rclcpp::Node, public CanInterface { void canmsg_callback(const driverless_msgs::msg::Can::SharedPtr msg) const; public: - CANTranslator(const rclcpp::NodeOptions & options); + CANTranslator(const rclcpp::NodeOptions& options); ~CANTranslator() override; bool set_interface(); @@ -91,4 +91,4 @@ class CANTranslator : public rclcpp::Node, public CanInterface { } // namespace canbus -#endif // CANBUS__COMPONENT_CANBUS_TRANSLATOR_HPP_ \ No newline at end of file +#endif // CANBUS__COMPONENT_CANBUS_TRANSLATOR_HPP_ diff --git a/src/hardware/canbus/include/interface.hpp b/src/hardware/canbus/include/interface.hpp index 7372a288a..212a8f200 100644 --- a/src/hardware/canbus/include/interface.hpp +++ b/src/hardware/canbus/include/interface.hpp @@ -9,11 +9,12 @@ class CANInterface { public: - CANInterface() {}; + CANInterface(){}; virtual bool setup(std::string interface, rclcpp::Logger logger) = 0; virtual void tx(driverless_msgs::msg::Can *msg, rclcpp::Logger logger) = 0; - virtual std::shared_ptr> rx(rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) = 0; + virtual std::shared_ptr> rx(rclcpp::Logger logger, + rclcpp::Clock::SharedPtr clock) = 0; virtual void deconstruct() = 0; }; diff --git a/src/hardware/canbus/src/component_canbus_translator.cpp b/src/hardware/canbus/src/component_canbus_translator.cpp index a59644a6c..2b03e76b0 100644 --- a/src/hardware/canbus/src/component_canbus_translator.cpp +++ b/src/hardware/canbus/src/component_canbus_translator.cpp @@ -1,10 +1,12 @@ #include "component_canbus_translator.hpp" + #include "rclcpp_components/register_node_macro.hpp" namespace canbus { void CANTranslator::update_twist() { - geometry_msgs::msg::TwistWithCovarianceStamped::UniquePtr twist_msg(new geometry_msgs::msg::TwistWithCovarianceStamped()); + geometry_msgs::msg::TwistWithCovarianceStamped::UniquePtr twist_msg( + new geometry_msgs::msg::TwistWithCovarianceStamped()); // use last velocity and steering angle to update twist twist_msg->header.stamp = this->now(); twist_msg->header.frame_id = ros_base_frame_; // PARAMETERISE @@ -23,7 +25,7 @@ void CANTranslator::canmsg_timer() { msg->id = can_msg.id; msg->dlc = can_msg.dlc; msg->data = can_msg.data; - + // only publish can messages with IDs we care about to not flood memory uint32_t canopen_index = std::find(canopen_ids.begin(), canopen_ids.end(), msg->id) - canopen_ids.begin(); if (canopen_index < canopen_ids.size()) { @@ -49,7 +51,9 @@ void CANTranslator::canmsg_timer() { if (vesc_id < 4) { if (vesc_masked_id == VESC_CAN_PACKET_STATUS) { // 3 wheel drive :/ - if (vesc_id == 1) { continue; } + if (vesc_id == 1) { + continue; + } uint8_t data[8]; this->copy_data(msg->data, data, 8); @@ -62,7 +66,7 @@ void CANTranslator::canmsg_timer() { wheel_speeds_[vesc_id] = (rpm / (21.0 * 4.50)) * M_PI * WHEEL_DIAMETER / 60; driverless_msgs::msg::Float32Stamped::UniquePtr speed_msg(new driverless_msgs::msg::Float32Stamped()); speed_msg->header.stamp = this->now(); - speed_msg->header.frame_id = ros_base_frame_; // could change to wheel frame + speed_msg->header.frame_id = ros_base_frame_; // could change to wheel frame speed_msg->data = wheel_speeds_[vesc_id]; wheel_speed_pubs_[vesc_id]->publish(std::move(speed_msg)); @@ -102,7 +106,7 @@ void CANTranslator::canmsg_timer() { Parse_VCU_TransmitSteering(data, &steering_0_raw, &steering_1_raw, &adc_0, &adc_1); // Log steering angle RCLCPP_DEBUG(this->get_logger(), "Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", - steering_0_raw, steering_1_raw, adc_0, adc_1); + steering_0_raw, steering_1_raw, adc_0, adc_1); double steering_0 = steering_0_raw / 10.0; double steering_1 = steering_1_raw / 10.0; @@ -116,8 +120,8 @@ void CANTranslator::canmsg_timer() { update_twist(); } else { RCLCPP_FATAL(this->get_logger(), - "MISMATCH: Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", - steering_0_raw, steering_1_raw, adc_0, adc_1); + "MISMATCH: Steering Angle 0: %i Steering Angle 1: %i ADC 0: %i ADC 1: %i", steering_0_raw, + steering_1_raw, adc_0, adc_1); } } } @@ -157,11 +161,13 @@ bool CANTranslator::set_interface() { return true; } -CANTranslator::CANTranslator(const rclcpp::NodeOptions & options) : Node("canbus_translator_node", options) { +CANTranslator::CANTranslator(const rclcpp::NodeOptions &options) : Node("canbus_translator_node", options) { ros_base_frame_ = this->declare_parameter("base_frame", "base_link"); // set can interface - if (!set_interface()) { return; } + if (!set_interface()) { + return; + } timer_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); sub_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -170,7 +176,8 @@ CANTranslator::CANTranslator(const rclcpp::NodeOptions & options) : Node("canbus sub_opt.callback_group = sub_cb_group_; // retrieve can messages from queue - timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&CANTranslator::canmsg_timer, this), timer_cb_group_); + timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&CANTranslator::canmsg_timer, this), + timer_cb_group_); // subscribe to can messages from ROS system can_sub_ = this->create_subscription( "/can/canbus_carbound", QOS_ALL, std::bind(&CANTranslator::canmsg_callback, this, _1), sub_opt); diff --git a/src/hardware/steering_actuator/include/component_steering_actuator.hpp b/src/hardware/steering_actuator/include/component_steering_actuator.hpp index 14d9a3e6e..760388936 100644 --- a/src/hardware/steering_actuator/include/component_steering_actuator.hpp +++ b/src/hardware/steering_actuator/include/component_steering_actuator.hpp @@ -1,11 +1,11 @@ #ifndef STEERING_ACTUATOR__COMPONENT_STEERING_ACTUATOR_HPP_ #define STEERING_ACTUATOR__COMPONENT_STEERING_ACTUATOR_HPP_ -#include -#include -#include #include #include + +#include +#include #include #include diff --git a/src/hardware/steering_actuator/src/component_steering_actuator.cpp b/src/hardware/steering_actuator/src/component_steering_actuator.cpp index dd5451eaf..e417fad17 100644 --- a/src/hardware/steering_actuator/src/component_steering_actuator.cpp +++ b/src/hardware/steering_actuator/src/component_steering_actuator.cpp @@ -1,4 +1,5 @@ #include "component_steering_actuator.hpp" + #include "rclcpp_components/register_node_macro.hpp" namespace steering_actuator { @@ -25,8 +26,7 @@ void SteeringActuator::c5e_state_request_callback() { // Check State to enable or disable motor void SteeringActuator::as_state_callback(const driverless_msgs::msg::State::SharedPtr msg) { - if (msg->state == driverless_msgs::msg::State::DRIVING || - msg->state == driverless_msgs::msg::State::ACTIVATE_EBS) { + if (msg->state == driverless_msgs::msg::State::DRIVING || msg->state == driverless_msgs::msg::State::ACTIVATE_EBS) { // Enable motor motor_enabled_ = true; } else { @@ -62,7 +62,7 @@ void SteeringActuator::driving_command_callback(const ackermann_msgs::msg::Acker } target = std::max(std::min(target, max_position_ - offset_), -max_position_ - offset_); RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 250, "Target: %f = %d", requested_steering_angle, - target); + target); this->target_position(target); } @@ -112,9 +112,9 @@ void SteeringActuator::can_callback(const driverless_msgs::msg::Can::SharedPtr m uint16_t status_word = (msg->data[5] << 8 | msg->data[4]); current_state_ = parse_state(status_word); RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 100, "Status word: %s", - std::bitset<16>(status_word).to_string().c_str()); + std::bitset<16>(status_word).to_string().c_str()); RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 100, "Current state: %s", - current_state_.name.c_str()); + current_state_.name.c_str()); if (motor_enabled_) { if (current_state_ == states[RTSO]) { @@ -212,7 +212,7 @@ void SteeringActuator::read_steering_data(uint16_t obj_index) { can_pub_->publish(std::move(_d_2_f(id, 0, out, sizeof(out)))); } -SteeringActuator::SteeringActuator(const rclcpp::NodeOptions & options) : Node("steering_actuator_node", options) { +SteeringActuator::SteeringActuator(const rclcpp::NodeOptions &options) : Node("steering_actuator_node", options) { // Steering parameters this->declare_parameter("velocity", 10000); this->declare_parameter("velocity_centering", 100); @@ -262,14 +262,13 @@ SteeringActuator::SteeringActuator(const rclcpp::NodeOptions & options) : Node(" state_sub_ = this->create_subscription( "/system/as_status", QOS_ALL, std::bind(&SteeringActuator::as_state_callback, this, _1), control_cb_opt); - steering_update_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), - std::bind(&SteeringActuator::pre_op_centering, this), - sensor_cb_group_); + steering_update_timer_ = this->create_wall_timer( + std::chrono::milliseconds(50), std::bind(&SteeringActuator::pre_op_centering, this), sensor_cb_group_); // Create state request and config timers - state_request_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), - std::bind(&SteeringActuator::c5e_state_request_callback, this), - control_cb_group_); + state_request_timer_ = + this->create_wall_timer(std::chrono::milliseconds(200), + std::bind(&SteeringActuator::c5e_state_request_callback, this), control_cb_group_); // Create publisher to topic "canbus_carbound" can_pub_ = this->create_publisher("/can/canbus_carbound", QOS_ALL); diff --git a/src/hardware/vehicle_supervisor/include/component_supervisor.hpp b/src/hardware/vehicle_supervisor/include/component_supervisor.hpp index 328dc0df1..1763325d6 100644 --- a/src/hardware/vehicle_supervisor/include/component_supervisor.hpp +++ b/src/hardware/vehicle_supervisor/include/component_supervisor.hpp @@ -14,11 +14,11 @@ #include "driverless_common/common.hpp" #include "driverless_msgs/msg/can.hpp" #include "driverless_msgs/msg/driving_dynamics1.hpp" +#include "driverless_msgs/msg/float32_stamped.hpp" #include "driverless_msgs/msg/res.hpp" #include "driverless_msgs/msg/shutdown.hpp" #include "driverless_msgs/msg/state.hpp" #include "driverless_msgs/msg/system_status.hpp" -#include "driverless_msgs/msg/float32_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" @@ -83,16 +83,15 @@ class ASSupervisor : public rclcpp::Node, public CanInterface { void dvl_heartbeat_timer_callback(); void res_alive_timer_callback(); void dataLogger_timer_callback(); - + void run_fsm(); void publish_heartbeart(); void reset_dataLogger(); public: - ASSupervisor(const rclcpp::NodeOptions & options, std::string name="vehicle_supervisor_node"); - + ASSupervisor(const rclcpp::NodeOptions& options, std::string name = "vehicle_supervisor_node"); }; } // namespace vehicle_supervisor -#endif // VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_HPP_ \ No newline at end of file +#endif // VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_HPP_ diff --git a/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp b/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp index 343454f92..d998aecb8 100644 --- a/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp +++ b/src/hardware/vehicle_supervisor/include/component_supervisor_launch.hpp @@ -6,16 +6,16 @@ namespace vehicle_supervisor { class ASSupervisorLaunch : public ASSupervisor { - private: + private: void run_fsm(); void launch_mission(); void dvl_heartbeat_timer_callback(); public: - ASSupervisorLaunch(const rclcpp::NodeOptions & options); + ASSupervisorLaunch(const rclcpp::NodeOptions& options); }; } // namespace vehicle_supervisor -#endif // VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_LAUNCH_HPP_ \ No newline at end of file +#endif // VEHICLE_SUPERVISOR__COMPONENT_SUPERVISOR_LAUNCH_HPP_ diff --git a/src/hardware/vehicle_supervisor/src/component_supervisor.cpp b/src/hardware/vehicle_supervisor/src/component_supervisor.cpp index 2ea9396bb..a5c33b823 100644 --- a/src/hardware/vehicle_supervisor/src/component_supervisor.cpp +++ b/src/hardware/vehicle_supervisor/src/component_supervisor.cpp @@ -1,4 +1,5 @@ #include "component_supervisor.hpp" + #include "rclcpp_components/register_node_macro.hpp" namespace vehicle_supervisor { @@ -26,8 +27,8 @@ void ASSupervisor::canopen_callback(const driverless_msgs::msg::Can::SharedPtr m Parse_RES_Heartbeat((uint8_t *)&msg->data[0], &this->RES_status); // Log RES state RCLCPP_DEBUG(this->get_logger(), "RES Status: [SW, BT]: %i, %i -- [EST]: %i, -- [RAD_QUAL]: %i", - this->RES_status.sw_k2, this->RES_status.bt_k3, this->RES_status.estop, - this->RES_status.radio_quality); + this->RES_status.sw_k2, this->RES_status.bt_k3, this->RES_status.estop, + this->RES_status.radio_quality); // // publish RES status // driverless_msgs::msg::RES res_msg; // res_msg.sw_k2 = this->RES_status.sw_k2; @@ -69,7 +70,7 @@ void ASSupervisor::can_callback(const driverless_msgs::msg::Can::SharedPtr msg) Parse_SW_Heartbeat(data, &this->SW_heartbeat); RCLCPP_DEBUG(this->get_logger(), "SW State: %02x Mission Id: %d", this->SW_heartbeat.stateID, - this->SW_heartbeat.missionID); + this->SW_heartbeat.missionID); } } @@ -100,8 +101,8 @@ void ASSupervisor::lap_counter_callback(const std_msgs::msg::UInt8::SharedPtr ms this->DVL_systemStatus._fields.lap_counter = msg->data; } -void ASSupervisor::steering_state_callback(const std_msgs::msg::Bool::SharedPtr msg) { - this->ros_state.navigation_ready = msg->data; +void ASSupervisor::steering_state_callback(const std_msgs::msg::Bool::SharedPtr msg) { + this->ros_state.navigation_ready = msg->data; } void ASSupervisor::shutdown_callback(const driverless_msgs::msg::Shutdown::SharedPtr msg) { @@ -121,8 +122,7 @@ void ASSupervisor::publish_heartbeart() { or_DVL_heartbeat.stateID = DVL_STATES::DVL_STATE_DRIVING; or_DVL_heartbeat.missionID = DVL_MISSION::DVL_MISSION_SELECTED; auto heartbeat = Compose_DVL_Heartbeat(&or_DVL_heartbeat); - this->can_pub_->publish(std::move( - this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data)))); + this->can_pub_->publish(std::move(this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data)))); driverless_msgs::msg::State::UniquePtr or_ros_state(new driverless_msgs::msg::State()); or_ros_state->state = or_DVL_heartbeat.stateID; @@ -136,8 +136,7 @@ void ASSupervisor::publish_heartbeart() { } // CAN publisher auto heartbeat = Compose_DVL_Heartbeat(&this->DVL_heartbeat); - this->can_pub_->publish(std::move( - this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data)))); + this->can_pub_->publish(std::move(this->_d_2_f(heartbeat.id, true, heartbeat.data, sizeof(heartbeat.data)))); // ROS publisher driverless_msgs::msg::State::UniquePtr state_msg(new driverless_msgs::msg::State()); @@ -170,8 +169,8 @@ void ASSupervisor::res_alive_timer_callback() { void ASSupervisor::dataLogger_timer_callback() { // system status auto systemStatusMsg = Compose_DVL_SystemStatus(&this->DVL_systemStatus); - this->can_pub_->publish(std::move( - this->_d_2_f(systemStatusMsg.id, false, systemStatusMsg.data, sizeof(systemStatusMsg.data)))); + this->can_pub_->publish( + std::move(this->_d_2_f(systemStatusMsg.id, false, systemStatusMsg.data, sizeof(systemStatusMsg.data)))); driverless_msgs::msg::SystemStatus systemStatus_ROSmsg; systemStatus_ROSmsg.as_state = this->DVL_systemStatus._fields.AS_state; @@ -385,7 +384,7 @@ void ASSupervisor::reset_dataLogger() { this->DVL_drivingDynamics1._fields.motor_moment_target = 0; } -ASSupervisor::ASSupervisor(const rclcpp::NodeOptions & options, std::string name) : Node(name, options) { +ASSupervisor::ASSupervisor(const rclcpp::NodeOptions &options, std::string name) : Node(name, options) { // Setup inital states this->ros_state.state = DVL_STATES::DVL_STATE_START; this->ros_state.mission = DVL_MISSION::DVL_MISSION_NONE; @@ -415,8 +414,7 @@ ASSupervisor::ASSupervisor(const rclcpp::NodeOptions & options, std::string name // Steering sub this->steering_angle_sub_ = this->create_subscription( - "/vehicle/steering_angle", QOS_LATEST, std::bind(&ASSupervisor::steering_angle_callback, this, _1), - sensor_opt); + "/vehicle/steering_angle", QOS_LATEST, std::bind(&ASSupervisor::steering_angle_callback, this, _1), sensor_opt); // Velocity sub this->velocity_sub_ = this->create_subscription( @@ -432,27 +430,23 @@ ASSupervisor::ASSupervisor(const rclcpp::NodeOptions & options, std::string name // steering ready sub this->steering_ready_sub_ = this->create_subscription( - "/system/steering_ready", QOS_LATEST, std::bind(&ASSupervisor::steering_state_callback, this, _1), - ctrl_opt); + "/system/steering_ready", QOS_LATEST, std::bind(&ASSupervisor::steering_state_callback, this, _1), ctrl_opt); // Shutdown emergency this->shutdown_sub_ = this->create_subscription( "/system/shutdown", 10, std::bind(&ASSupervisor::shutdown_callback, this, _1), timer_opt); // AS Heartbeat - this->heartbeat_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), - std::bind(&ASSupervisor::dvl_heartbeat_timer_callback, this), - timer_cb_group_); + this->heartbeat_timer_ = this->create_wall_timer( + std::chrono::milliseconds(20), std::bind(&ASSupervisor::dvl_heartbeat_timer_callback, this), timer_cb_group_); // RES Alive - this->res_alive_timer_ = - this->create_wall_timer(std::chrono::milliseconds(4000), std::bind(&ASSupervisor::res_alive_timer_callback, this), - timer_cb_group_); + this->res_alive_timer_ = this->create_wall_timer( + std::chrono::milliseconds(4000), std::bind(&ASSupervisor::res_alive_timer_callback, this), timer_cb_group_); // Data Logger - this->dataLogger_timer_ = - this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&ASSupervisor::dataLogger_timer_callback, this), - timer_cb_group_); + this->dataLogger_timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), std::bind(&ASSupervisor::dataLogger_timer_callback, this), timer_cb_group_); // Publishers this->logging_drivingDynamics1_pub_ = diff --git a/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp b/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp index b36338119..29526a5a7 100644 --- a/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp +++ b/src/hardware/vehicle_supervisor/src/component_supervisor_launch.cpp @@ -1,4 +1,5 @@ #include "component_supervisor_launch.hpp" + #include "rclcpp_components/register_node_macro.hpp" namespace vehicle_supervisor { @@ -191,9 +192,11 @@ void ASSupervisorLaunch::dvl_heartbeat_timer_callback() { publish_heartbeart(); } -ASSupervisorLaunch::ASSupervisorLaunch(const rclcpp::NodeOptions & options) : ASSupervisor(options, "vehicle_supervisor_launch_node") { +ASSupervisorLaunch::ASSupervisorLaunch(const rclcpp::NodeOptions& options) + : ASSupervisor(options, "vehicle_supervisor_launch_node") { // override the heartbeat timer - this->heartbeat_timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&ASSupervisorLaunch::dvl_heartbeat_timer_callback, this)); + this->heartbeat_timer_ = this->create_wall_timer( + std::chrono::milliseconds(20), std::bind(&ASSupervisorLaunch::dvl_heartbeat_timer_callback, this)); } } // namespace vehicle_supervisor