Skip to content

Commit

Permalink
fix launch file
Browse files Browse the repository at this point in the history
  • Loading branch information
@meshvaD
meshvaD committed Jul 11, 2024
1 parent 736a359 commit 2396c92
Show file tree
Hide file tree
Showing 3 changed files with 141 additions and 66 deletions.
1 change: 0 additions & 1 deletion ...terfacing/sensor_interfacing/radar_conti_ars408/include/radar_conti_ars_408_component.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -249,7 +249,6 @@ namespace watonomous

private:
// ##############Task2################
// std::unique_ptr<polymath::socketcan::SocketcanAdapter> socketcan_adapter_;
// create Publisher
rclcpp::QoS qos{10};
std::string radar_detection_topic_name_;
Expand Down
166 changes: 141 additions & 25 deletions src/interfacing/sensor_interfacing/radar_conti_ars408/launch/radar_driver.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -31,39 +39,147 @@ 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

radar_node = Node(
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
40 changes: 0 additions & 40 deletions src/interfacing/sensor_interfacing/radar_conti_ars408/src/radar_conti_ars_408_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; */
};
}

Expand Down

0 comments on commit 2396c92

Please sign in to comment.