From 18548e98416fcd22441fea9d0ba8cb6e8b4b0734 Mon Sep 17 00:00:00 2001 From: Meshva Desai Date: Thu, 11 Jul 2024 05:18:27 +0000 Subject: [PATCH] fix launch file --- .../include/radar_conti_ars_408_component.hpp | 1 - .../launch/radar_driver.launch.py | 166 +++++++++++++++--- .../src/radar_conti_ars_408_component.cpp | 40 ----- .../test/radar_velocity_detection_test.cpp | 27 +++ 4 files changed, 168 insertions(+), 66 deletions(-) create mode 100644 src/interfacing/sensor_interfacing/radar_conti_ars408/test/radar_velocity_detection_test.cpp diff --git a/src/interfacing/sensor_interfacing/radar_conti_ars408/include/radar_conti_ars_408_component.hpp b/src/interfacing/sensor_interfacing/radar_conti_ars408/include/radar_conti_ars_408_component.hpp index a421f970..b1d84fb5 100644 --- a/src/interfacing/sensor_interfacing/radar_conti_ars408/include/radar_conti_ars_408_component.hpp +++ b/src/interfacing/sensor_interfacing/radar_conti_ars408/include/radar_conti_ars_408_component.hpp @@ -249,7 +249,6 @@ namespace watonomous private: // ##############Task2################ - // std::unique_ptr socketcan_adapter_; // create Publisher rclcpp::QoS qos{10}; std::string radar_detection_topic_name_; diff --git a/src/interfacing/sensor_interfacing/radar_conti_ars408/launch/radar_driver.launch.py b/src/interfacing/sensor_interfacing/radar_conti_ars408/launch/radar_driver.launch.py index 48ba664a..2e4bcf45 100644 --- a/src/interfacing/sensor_interfacing/radar_conti_ars408/launch/radar_driver.launch.py +++ b/src/interfacing/sensor_interfacing/radar_conti_ars408/launch/radar_driver.launch.py @@ -1,10 +1,18 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription +from launch.actions import (DeclareLaunchArgument, EmitEvent, + RegisterEventHandler, IncludeLaunchDescription) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessStart from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import LifecycleNode from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.events.lifecycle import ChangeState +from launch.events import matches_action +from launch_ros.event_handlers import OnStateTransition +from lifecycle_msgs.msg import Transition + from pathlib import Path import os @@ -31,11 +39,113 @@ def generate_launch_description(): default_value="False", description=str("Use sim time argument for whether to force it")) - namespace = LaunchConfiguration('namespace') - namespace_arg = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') + # config args + interface_arg = DeclareLaunchArgument('interface', default_value='vcan0') + enable_can_fd_arg = DeclareLaunchArgument('enable_can_fd', default_value='false') + interval_sec_arg = DeclareLaunchArgument('interval_sec', default_value='0.01') + timeout_sec_msg = DeclareLaunchArgument('timeout_sec', default_value='0.01') + use_bus_time_arg = DeclareLaunchArgument('use_bus_time', default_value='false') + filters_arg = DeclareLaunchArgument('filters', default_value='0:0') + auto_configure_arg = DeclareLaunchArgument('auto_configure', default_value='true') + auto_activate_arg = DeclareLaunchArgument('auto_activate', default_value='true') + from_can_bus_topic_arg = DeclareLaunchArgument('from_can_bus_topic', default_value='from_can_bus') + to_can_bus_topic_msg = DeclareLaunchArgument('to_can_bus_topic', default_value='to_can_bus') + + # can sender + socket_can_sender_node = LifecycleNode( + package='ros2_socketcan', + executable='socket_can_sender_node_exe', + name='socket_can_sender', + namespace=TextSubstitution(text=''), + parameters=[{ + 'interface': LaunchConfiguration('interface'), + 'enable_can_fd': LaunchConfiguration('enable_can_fd'), + 'timeout_sec': + LaunchConfiguration('timeout_sec'), + }], + remappings=[('to_can_bus', LaunchConfiguration('to_can_bus_topic'))], + output='screen') + + socket_can_sender_configure_event_handler = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=socket_can_sender_node, + on_start=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(socket_can_sender_node), + transition_id=Transition.TRANSITION_CONFIGURE, + ), + ), + ], + ), + condition=IfCondition(LaunchConfiguration('auto_configure')), + ) + + socket_can_sender_activate_event_handler = RegisterEventHandler( + event_handler=OnStateTransition( + target_lifecycle_node=socket_can_sender_node, + start_state='configuring', + goal_state='inactive', + entities=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(socket_can_sender_node), + transition_id=Transition.TRANSITION_ACTIVATE, + ), + ), + ], + ), + condition=IfCondition(LaunchConfiguration('auto_activate')), + ) + + # can receiver + socket_can_receiver_node = LifecycleNode( + package='ros2_socketcan', + executable='socket_can_receiver_node_exe', + name='socket_can_receiver', + namespace=TextSubstitution(text=''), + parameters=[{ + 'interface': LaunchConfiguration('interface'), + 'enable_can_fd': LaunchConfiguration('enable_can_fd'), + 'interval_sec': + LaunchConfiguration('interval_sec'), + 'filters': LaunchConfiguration('filters'), + 'use_bus_time': LaunchConfiguration('use_bus_time'), + }], + remappings=[('from_can_bus', LaunchConfiguration('from_can_bus_topic'))], + output='screen') + + socket_can_receiver_configure_event_handler = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=socket_can_receiver_node, + on_start=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(socket_can_receiver_node), + transition_id=Transition.TRANSITION_CONFIGURE, + ), + ), + ], + ), + condition=IfCondition(LaunchConfiguration('auto_configure')), + ) + + socket_can_receiver_activate_event_handler = RegisterEventHandler( + event_handler=OnStateTransition( + target_lifecycle_node=socket_can_receiver_node, + start_state='configuring', + goal_state='inactive', + entities=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(socket_can_receiver_node), + transition_id=Transition.TRANSITION_ACTIVATE, + ), + ), + ], + ), + condition=IfCondition(LaunchConfiguration('auto_activate')), + ) # Nodes launching commands @@ -43,27 +153,33 @@ def generate_launch_description(): package='radar_conti_ars408', executable='radar_conti_ars408_composition', name='radar_node', - namespace=LaunchConfiguration('namespace'), output='screen', parameters=[LaunchConfiguration('params_file')]) - # 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) + ld = LaunchDescription([ + interface_arg, + enable_can_fd_arg, + interval_sec_arg, + use_bus_time_arg, + filters_arg, + auto_configure_arg, + auto_activate_arg, + from_can_bus_topic_arg, + to_can_bus_topic_msg, + params_file_arg, + autostart_arg, + use_sim_time_arg, + + radar_node, + + socket_can_receiver_node, + socket_can_receiver_configure_event_handler, + socket_can_receiver_activate_event_handler, + socket_can_sender_node, + socket_can_sender_configure_event_handler, + socket_can_sender_activate_event_handler, + ]) + # ld.add_action(socketcan_sender_launch) return ld diff --git a/src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_component.cpp b/src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_component.cpp index dd592b9d..2e78d5a4 100644 --- a/src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_component.cpp +++ b/src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_component.cpp @@ -421,52 +421,12 @@ namespace watonomous // for each Obj_2_Quality message the existing object in the map has to be updated if (Get_MsgID0_From_MsgID(frame->id) == ID_Obj_2_Quality) { - /* - int id = GET_Obj_2_Quality_Obj_ID(frame->data); - - object_map_list_[sensor_id][id].object_quality.obj_distlong_rms.data = - CALC_Obj_2_Quality_Obj_DistLong_rms(GET_Obj_2_Quality_Obj_DistLong_rms(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_quality.obj_distlat_rms.data = - CALC_Obj_2_Quality_Obj_DistLat_rms(GET_Obj_2_Quality_Obj_DistLat_rms(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_quality.obj_vrellong_rms.data = - CALC_Obj_2_Quality_Obj_VrelLong_rms(GET_Obj_2_Quality_Obj_VrelLong_rms(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_quality.obj_vrellat_rms.data = - CALC_Obj_2_Quality_Obj_VrelLat_rms(GET_Obj_2_Quality_Obj_VrelLat_rms(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_quality.obj_probofexist.data = - CALC_Obj_2_Quality_Obj_ProbOfExist(GET_Obj_2_Quality_Obj_ProbOfExist(frame->data), 1.0); - */ } // Object Extended Information // for each Obj_3_ExtInfo message the existing object in the map has to be updated if (Get_MsgID0_From_MsgID(frame->id) == ID_Obj_3_Extended) { - - /* int id = GET_Obj_3_Extended_Obj_ID(frame->data); - - object_map_list_[sensor_id][id].object_extended.obj_arellong.data = - CALC_Obj_3_Extended_Obj_ArelLong(GET_Obj_3_Extended_Obj_ArelLong(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_extended.obj_arellat.data = - CALC_Obj_3_Extended_Obj_ArelLat(GET_Obj_3_Extended_Obj_ArelLat(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_extended.obj_class.data = - CALC_Obj_3_Extended_Obj_Class(GET_Obj_3_Extended_Obj_Class(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_extended.obj_orientationangle.data = - CALC_Obj_3_Extended_Obj_OrientationAngle(GET_Obj_3_Extended_Obj_OrientationAngle(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_extended.obj_length.data = - CALC_Obj_3_Extended_Obj_Length(GET_Obj_3_Extended_Obj_Length(frame->data), 1.0); - - object_map_list_[sensor_id][id].object_extended.obj_width.data = - CALC_Obj_3_Extended_Obj_Width(GET_Obj_3_Extended_Obj_Width(frame->data), 1.0); - - object_count = object_count + 1; */ }; } diff --git a/src/interfacing/sensor_interfacing/radar_conti_ars408/test/radar_velocity_detection_test.cpp b/src/interfacing/sensor_interfacing/radar_conti_ars408/test/radar_velocity_detection_test.cpp new file mode 100644 index 00000000..3eaeeb58 --- /dev/null +++ b/src/interfacing/sensor_interfacing/radar_conti_ars408/test/radar_velocity_detection_test.cpp @@ -0,0 +1,27 @@ +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "continental_pointcloud_filter.hpp" + + +class RadarVelocityDetectionTest : public ::testing::Test +{ +public: + +protected: + filtering::ContinentalPointCloudFilter pointcloudfilter; + filtering::ContinentalPointCloudFilter::filter_parameters parameters; +}; + +TEST_F(RadarVelocityDetectionTest, CheckCanMessageParsing) +{ + can_msgs::msg::Frame frame_msg(rosidl_runtime_cpp::MessageInitialization::ZERO); + frame_msg.header.frame_id = "can"; + receive(frame_msg.data.data(), interval_ns_) + + can_receive_callback(frame); +}