Skip to content

Commit

Permalink
added lifecycle manager to launcher
Browse files Browse the repository at this point in the history
  • Loading branch information
@muttistefano
muttistefano committed Jan 25, 2024
1 parent 432a982 commit f17844b
Show file tree
Hide file tree
Showing 6 changed files with 160 additions and 98 deletions.
4 changes: 3 additions & 1 deletion .vscode/c_cpp_properties.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
30 changes: 15 additions & 15 deletions config/pid.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
78 changes: 0 additions & 78 deletions launch/icp_nav_follow_simul.launch.py
View file

This file was deleted.

12 changes: 9 additions & 3 deletions launch/icp_nav_follow.launch.py → launch/nav_follow.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 '],
Expand All @@ -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)
Expand Down
115 changes: 115 additions & 0 deletions launch/nav_follow_simul.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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)
19 changes: 18 additions & 1 deletion src/nav_follow_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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();
Expand Down

0 comments on commit f17844b

Please sign in to comment.