diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 16058de..cb23d4a 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -39,7 +39,9 @@ "/opt/ros/rolling/include/control_toolbox", "${workspaceFolder}/include", "/opt/ros/rolling/include/rclcpp_lifecycle", - "/opt/ros/rolling/include/lifecycle_msgs" + "/opt/ros/rolling/include/lifecycle_msgs", + "/opt/ros/rolling/include/rcl_lifecycle", + "/opt/ros/rolling/include/rosidl_dynamic_typesupport" ], "defines": [], "compilerPath": "/usr/bin/gcc", diff --git a/config/pid.yaml b/config/pid.yaml index 1dd1f16..394baca 100644 --- a/config/pid.yaml +++ b/config/pid.yaml @@ -2,25 +2,25 @@ ros__parameters: x_pid: antiwindup: True - p: 5.0 - i: 1.5 - d: 0.3 - i_clamp_max: 0.05 - i_clamp_min: -0.05 + p: 1.0 + i: 0.5 + d: 0.2 + i_clamp_max: 0.25 + i_clamp_min: -0.25 y_pid: antiwindup: True - p: 5.0 - i: 1.5 - d: 0.3 - i_clamp_max: 0.05 - i_clamp_min: -0.05 + p: 1.0 + i: 0.5 + d: 0.2 + i_clamp_max: 0.25 + i_clamp_min: -0.25 w_pid: antiwindup: True - p: 5.0 - i: 1.5 - d: 0.3 - i_clamp_max: 0.05 - i_clamp_min: -0.05 + p: 1.0 + i: 0.5 + d: 0.2 + i_clamp_max: 0.25 + i_clamp_min: -0.25 x_pid_icp: antiwindup: True p: 1.5 diff --git a/launch/icp_nav_follow_simul.launch.py b/launch/icp_nav_follow_simul.launch.py deleted file mode 100644 index c217cd1..0000000 --- a/launch/icp_nav_follow_simul.launch.py +++ /dev/null @@ -1,78 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node,SetRemap -from launch.actions import DeclareLaunchArgument, TimerAction -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory -import os - -def generate_launch_description(): - - declared_arguments = [] - - - log_level = LaunchConfiguration('log_level') - - declared_arguments.append( - DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level')) - - config = os.path.join( - get_package_share_directory('nav_follow'), - 'config', - 'pid.yaml' - ) - - node1 = [Node( - package='nav_follow', - executable='nav_follow_node', - namespace="sweepee_2", - output='screen', - # prefix=['xterm -e gdb -ex run --args'], - # prefix=['valgrind --tool=memcheck --leak-check=yes --show-reachable=yes '], - arguments=['--ros-args', '--log-level', log_level], - parameters=[{"laser_scan_slave": "/sweepee_2/front_laser_plugin/out"}, - {"laser_scan_master": "/sweepee_1/front_laser_plugin/out"}, - {"cmd_vel_topic": "/sweepee_2/cmd_vel"}, - {"frame_name_laser_slave": "sweepee_2/front_laser"}, - {"frame_name_laser_master": "sweepee_1/front_laser"}, - {"frame_name_base_slave": "sweepee_2/base_footprint"}, - {"frame_name_base_master": "sweepee_1/base_footprint"}, - {"icp_iterations" : 15}, - {"icp_TransformationEpsilon" : 1e-9}, - {"icp_EuclideanFitnessEpsilon" : 1.0}, - {"icp_RANSACOutlierRejectionThreshold": 1.5}, - {"icp_MaxCorrespondenceDistance" : 100.0}, - {"use_sim_time" : True}, - {"enable_tf" : True}, - {"enable_vel_feedforward" : True}, - {"enable_icp" : False}, - {"cmd_vel_topic_master" : "/sweepee_1/cmd_vel"}, - {"autostart" : True}, - - config])] - - node2 = [Node( - package='nav_follow', - executable='nav_follow_node', - output='screen', - namespace="sweepee_3", - # prefix=['xterm -e gdb -ex run --args'], - # prefix=['valgrind --tool=memcheck --leak-check=yes --show-reachable=yes '], - arguments=['--ros-args', '--log-level', log_level], - parameters=[{"laser_scan_slave": "/sweepee_3/front_laser_plugin/out"}, - {"laser_scan_master": "/sweepee_1/front_laser_plugin/out"}, - {"cmd_vel_topic": "/sweepee_3/cmd_vel"}, - {"frame_name_laser_slave": "sweepee_3/front_laser"}, - {"frame_name_laser_master": "sweepee_1/front_laser"}, - {"frame_name_base_slave": "sweepee_3/base_footprint"}, - {"frame_name_base_master": "sweepee_1/base_footprint"}, - {"icp_iterations" : 15}, - {"icp_TransformationEpsilon" : 1e-9}, - {"icp_EuclideanFitnessEpsilon" : 1.0}, - {"icp_RANSACOutlierRejectionThreshold": 1.5}, - {"icp_MaxCorrespondenceDistance" : 100.0}, - {"use_sim_time" : True}, - config])] - - return LaunchDescription(declared_arguments + node1 ) diff --git a/launch/icp_nav_follow.launch.py b/launch/nav_follow.launch.py similarity index 75% rename from launch/icp_nav_follow.launch.py rename to launch/nav_follow.launch.py index a7d1ee4..a7067a6 100644 --- a/launch/icp_nav_follow.launch.py +++ b/launch/nav_follow.launch.py @@ -18,14 +18,14 @@ def generate_launch_description(): description='log level')) config = os.path.join( - get_package_share_directory('icp_nav_follow'), + get_package_share_directory('nav_follow'), 'config', 'pid.yaml' ) node1 = [Node( - package='icp_nav_follow', - executable='icp_nav_follow_node', + package='nav_follow', + executable='nav_follow_node', output='screen', # prefix=['xterm -e gdb -ex run --args'], # prefix=['valgrind --tool=memcheck --leak-check=yes --show-reachable=yes '], @@ -42,6 +42,12 @@ def generate_launch_description(): {"icp_EuclideanFitnessEpsilon" : 0.00001}, {"icp_RANSACOutlierRejectionThreshold": 1.5}, {"icp_MaxCorrespondenceDistance" : 100.0}, + {"use_sim_time" : False}, + {"enable_tf" : True}, + {"enable_vel_feedforward" : True}, + {"enable_icp" : False}, + {"cmd_vel_topic_master" : "/omron/cmd_vel"}, + {"autostart" : True}, config])] return LaunchDescription(declared_arguments + node1) diff --git a/launch/nav_follow_simul.launch.py b/launch/nav_follow_simul.launch.py new file mode 100644 index 0000000..9494f27 --- /dev/null +++ b/launch/nav_follow_simul.launch.py @@ -0,0 +1,115 @@ +from launch import LaunchDescription +from launch_ros.actions import Node,SetRemap +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch.event_handlers.on_shutdown import OnShutdown +from launch.actions import EmitEvent +from launch.actions import RegisterEventHandler +from launch_ros.events.lifecycle import ChangeState +from launch_ros.events import matches_node_name +from launch.actions import LogInfo +import lifecycle_msgs.msg +import os + +def generate_launch_description(): + + declared_arguments = [] + + + log_level = LaunchConfiguration('log_level') + + declared_arguments.append( + DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level')) + + config = os.path.join( + get_package_share_directory('nav_follow'), + 'config', + 'pid.yaml' + ) + + node1 = [Node( + package='nav_follow', + executable='nav_follow_node', + namespace="sweepee_2", + output='screen', + # prefix=['xterm -e gdb -ex run --args'], + # prefix=['valgrind --tool=memcheck --leak-check=yes --show-reachable=yes '], + arguments=['--ros-args', '--log-level', log_level], + parameters=[{"laser_scan_slave": "/sweepee_2/front_laser_plugin/out"}, + {"laser_scan_master": "/sweepee_1/front_laser_plugin/out"}, + {"cmd_vel_topic": "/sweepee_2/cmd_vel"}, + {"frame_name_laser_slave": "sweepee_2/front_laser"}, + {"frame_name_laser_master": "sweepee_1/front_laser"}, + {"frame_name_base_slave": "sweepee_2/base_footprint"}, + {"frame_name_base_master": "sweepee_1/base_footprint"}, + {"icp_iterations" : 15}, + {"icp_TransformationEpsilon" : 1e-9}, + {"icp_EuclideanFitnessEpsilon" : 1.0}, + {"icp_RANSACOutlierRejectionThreshold": 1.5}, + {"icp_MaxCorrespondenceDistance" : 100.0}, + {"use_sim_time" : True}, + {"enable_tf" : True}, + {"enable_vel_feedforward" : True}, + {"enable_icp" : False}, + {"cmd_vel_topic_master" : "/sweepee_1/cmd_vel"}, + + config])] + + foll_manager = [Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + namespace="sweepee_2", + name='lifecycle_manager_follow', + output='screen', + arguments=['--ros-args', '--log-level', "info"], + parameters=[{'autostart': True}, + {'node_names': ['nav_follow']}, + {'bond_timeout': 0.0}, + {"use_sim_time": True}])] + + shutdown_event = [RegisterEventHandler( + OnShutdown( + on_shutdown=[ + EmitEvent(event=ChangeState( + lifecycle_node_matcher=matches_node_name(node_name="sweepee_2/node_name"), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN, + )), + LogInfo( + msg="[LifecycleLaunch] Ouster driver node is exiting."), + ], + ) + )] + + # node2 = [Node( + # package='nav_follow', + # executable='nav_follow_node', + # namespace="sweepee_3", + # output='screen', + # # prefix=['xterm -e gdb -ex run --args'], + # # prefix=['valgrind --tool=memcheck --leak-check=yes --show-reachable=yes '], + # arguments=['--ros-args', '--log-level', log_level], + # parameters=[{"laser_scan_slave": "/sweepee_3/front_laser_plugin/out"}, + # {"laser_scan_master": "/sweepee_1/front_laser_plugin/out"}, + # {"cmd_vel_topic": "/sweepee_3/cmd_vel"}, + # {"frame_name_laser_slave": "sweepee_3/front_laser"}, + # {"frame_name_laser_master": "sweepee_1/front_laser"}, + # {"frame_name_base_slave": "sweepee_3/base_footprint"}, + # {"frame_name_base_master": "sweepee_1/base_footprint"}, + # {"icp_iterations" : 15}, + # {"icp_TransformationEpsilon" : 1e-9}, + # {"icp_EuclideanFitnessEpsilon" : 1.0}, + # {"icp_RANSACOutlierRejectionThreshold": 1.5}, + # {"icp_MaxCorrespondenceDistance" : 100.0}, + # {"use_sim_time" : True}, + # {"enable_tf" : True}, + # {"enable_vel_feedforward" : True}, + # {"enable_icp" : False}, + # {"cmd_vel_topic_master" : "/sweepee_1/cmd_vel"}, + # {"autostart" : True}, + + # config])] + + return LaunchDescription(declared_arguments + node1 + foll_manager + shutdown_event) diff --git a/src/nav_follow_node.cpp b/src/nav_follow_node.cpp index 0b3a511..78f1034 100644 --- a/src/nav_follow_node.cpp +++ b/src/nav_follow_node.cpp @@ -132,6 +132,23 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn nav_follow_class::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(this->get_logger(), "Deactivating"); + _th_cmd_vel.join(); + if (_cmd_vel_pub_rt->trylock()) + { + auto & msg = _cmd_vel_pub_rt->msg_; + + msg.linear.z = 0.0; + msg.angular.x = 0.0; + msg.angular.y = 0.0; + + + msg.linear.x = 0.0 ; + msg.linear.y = 0.0 ; + msg.angular.z = 0.0 ; + + // std::cout << msg.linear.x << std::flush; + _cmd_vel_pub_rt->unlockAndPublish(); + } this->_tf_controlling = false; this->_icp_controlling = false; RCLCPP_INFO(this->get_logger(), "Deactivating Done"); @@ -177,7 +194,7 @@ void nav_follow_class::cmd_vel_thread() msg.linear.y = _cmd_vel_tf_msg.linear.y + _cmd_vel_feed_msg.linear.y + _cmd_vel_icp_msg.linear.y ; msg.angular.z = _cmd_vel_tf_msg.angular.z + _cmd_vel_feed_msg.angular.z + _cmd_vel_icp_msg.angular.z ; } - std::cout << msg.linear.x << std::flush; + // std::cout << msg.linear.x << std::flush; _cmd_vel_pub_rt->unlockAndPublish(); } _rate_cmd_vel->sleep();