cleanup

src/interfacing/sensor_interfacing/radar_conti_ars408/CMakeLists.txt

@@ -95,8 +95,8 @@ ament_target_dependencies(
   ros2_socketcan
 )
 
-rclcpp_components_register_nodes(radar_conti_ars408_component "FHAC::radar_conti_ars408")
-set(node_plugins "${node_plugins}FHAC::radar_conti_ars408;$<TARGET_FILE:radar_conti_ars408_component>\n")
+rclcpp_components_register_nodes(radar_conti_ars408_component "watonomous::radar_conti_ars408")
+set(node_plugins "${node_plugins}watonomous::radar_conti_ars408;$<TARGET_FILE:radar_conti_ars408_component>\n")
 
 # since the package installs libraries without exporting them
 # it needs to make sure that the library path is being exported

src/interfacing/sensor_interfacing/radar_conti_ars408/include/radar_conti_ars_408_component.hpp

@@ -50,7 +50,7 @@
 #include <unordered_map>
 #include <random>
 
-#define CAN_MAX_DLC 10
+#define CAN_MAX_DLC 8
 
 // Enum class definition
 enum class FilterType {
@@ -81,7 +81,7 @@ typedef unsigned short int uword;
 using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
 using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy;
 
-namespace FHAC
+namespace watonomous
 {
 
     class radar_conti_ars408 : public rclcpp_lifecycle::LifecycleNode
@@ -262,6 +262,7 @@ namespace FHAC
         std::vector<rclcpp_lifecycle::LifecyclePublisher<radar_conti_ars408_msgs::msg::FilterStateCfg>::SharedPtr> filter_config_publishers_;
         rclcpp_lifecycle::LifecyclePublisher<radar_msgs::msg::RadarPacket>::SharedPtr radar_packet_publisher_;
         rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr can_frame_subscriber_;
+        rclcpp_lifecycle::LifecyclePublisher<can_msgs::msg::Frame>::SharedPtr can_frame_publisher_;
 
         rclcpp::Service<radar_conti_ars408_msgs::srv::SetFilter>::SharedPtr set_filter_service_;
 

src/interfacing/sensor_interfacing/radar_conti_ars408/launch/radar_driver.launch.py

@@ -47,18 +47,23 @@ def generate_launch_description():
         output='screen',
         parameters=[LaunchConfiguration('params_file')])
 
-    # launch socketcan node
+    # launch socketcan receiver & publisher node
     socketcan_dir = get_package_share_directory('ros2_socketcan')
     socketcan_receiver_launch = IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                     socketcan_dir + '/launch/socket_can_receiver.launch.py'))
 
+    socketcan_sender_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                    socketcan_dir + '/launch/socket_can_sender.launch.py'))
+
     ld = LaunchDescription()
     ld.add_action(params_file_arg)
     ld.add_action(autostart_arg)
     ld.add_action(use_sim_time_arg)
     ld.add_action(namespace_arg)
     ld.add_action(radar_node)
     ld.add_action(socketcan_receiver_launch)
+    ld.add_action(socketcan_sender_launch)
 
     return ld

src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_component.cpp

@@ -11,13 +11,14 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "ros2_socketcan/socket_can_receiver.hpp"
+#include "ros2_socketcan/socket_can_sender.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
 using std::placeholders::_1;
 using namespace std::chrono_literals;
 
-namespace FHAC
+namespace watonomous
 {
 
   const std::vector<FilterType> filterTypes = {
@@ -64,10 +65,6 @@ namespace FHAC
     number_of_radars_ = 0;
     do
     {
-      // Build the string in the form of "radar_link_X", where X is the sensor ID of
-      // the rader on the CANBUS, then check if we have any parameters with that value. Users need
-      // to make sure they don't have gaps in their configs (e.g.,footprint0 and then
-      // footprint2)
       std::stringstream ss;
       ss << "radar_" << topic_ind;
       std::string radar_name = ss.str();
@@ -169,6 +166,7 @@ namespace FHAC
     } while (more_params);
 
     can_frame_subscriber_ = this->create_subscription<can_msgs::msg::Frame>("/from_can_bus", 10, std::bind(&radar_conti_ars408::can_receive_callback, this, std::placeholders::_1));    
+    can_frame_publisher_ = this->create_publisher<can_msgs::msg::Frame>("/to_can_bus", 10);
     radar_packet_publisher_ = this->create_publisher<radar_msgs::msg::RadarPacket>("/radar_packet", transient_local_qos);
 
     object_count = 0.0;
@@ -212,12 +210,6 @@ namespace FHAC
       const rclcpp_lifecycle::State &)
   {
     RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called.");
-    // socketcan_adapter_->joinReceptionThread();
-    // if (!socketcan_adapter_->closeSocket())
-    // {
-    //   RCLCPP_ERROR(this->get_logger(), "Unable to close socket");
-    // }
-
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
   }
 
@@ -498,7 +490,6 @@ namespace FHAC
       std::shared_ptr<radar_conti_ars408_msgs::srv::SetFilter::Response> response)
   {
     auto req = *request;
-    // Add small delay so the CAN on Orin does not fault
     std::this_thread::sleep_for(std::chrono::milliseconds(1));
     if (!setFilter(req.sensor_id,
                    FilterCfg_FilterCfg_Active_active,
@@ -513,12 +504,9 @@ namespace FHAC
 
   bool radar_conti_ars408::setFilter(const int &sensor_id, const int &active, const int &valid, const int &type, const int &index, const int &min_value, const int &max_value)
   {
-    // polymath::socketcan::CanFrame frame;
-    // frame.set_len(DLC_FilterCfg);
 
     uint32_t msg_id = ID_FilterCfg;
     Set_SensorID_In_MsgID(msg_id, sensor_id);
-    // frame.set_can_id(msg_id);
 
     std::array<unsigned char, CAN_MAX_DLC> data;
     SET_FilterCfg_FilterCfg_Active(data, active);
@@ -612,13 +600,9 @@ namespace FHAC
     RCLCPP_DEBUG(this->get_logger(), "min_value is: %i", min_value);
     RCLCPP_DEBUG(this->get_logger(), "max_value is: %i", max_value);
 
-    // frame.set_data(data);
-    // auto err = socketcan_adapter_->send(frame);
-    // if (err.has_value())
-    // {
-    //   RCLCPP_ERROR(this->get_logger(), "Error sending frame: %s", err.value().c_str());
-    //   return false;
-    // }
+    can_msgs::msg::Frame frame;
+    frame.data = data;
+    can_frame_publisher_->publish(frame);
 
     return true;
   }
@@ -737,9 +721,4 @@ namespace FHAC
 } // end namespace
 
 #include "rclcpp_components/register_node_macro.hpp"
-// Register the component with class_loader.
-// This acts as a sort of entry point, allowing the component to be discoverable when its library
-// is being loaded into a running process.
-// CLASS_LOADER_REGISTER_CLASS(FHAC::radar_conti_ars408, rclcpp_lifecycle::LifecycleNode)
-
-RCLCPP_COMPONENTS_REGISTER_NODE(FHAC::radar_conti_ars408)
+RCLCPP_COMPONENTS_REGISTER_NODE(watonomous::radar_conti_ars408)

src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_composition.cpp

@@ -25,7 +25,7 @@ int main(int argc, char * argv[])
     rclcpp::executors::SingleThreadedExecutor exec;
 
     rclcpp::NodeOptions options;
-    auto radar_conti_ars408_node = std::make_shared<FHAC::radar_conti_ars408>(options);
+    auto radar_conti_ars408_node = std::make_shared<watonomous::radar_conti_ars408>(options);
 
     exec.add_node(radar_conti_ars408_node->get_node_base_interface());
     exec.spin();

src/wato_msgs/radar_msgs/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(std_msgs REQUIRED)
 set(msg_files
     msg/RadarDetection.msg
     msg/RadarPacket.msg)
+
 rosidl_generate_interfaces(radar_msgs
     ${msg_files}
     DEPENDENCIES std_msgs)