diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e8b072e --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build +install +log \ No newline at end of file diff --git a/toid_navigation/launch/mock_control_launch.py b/toid_navigation/launch/mock_control_launch.py new file mode 100644 index 0000000..d001734 --- /dev/null +++ b/toid_navigation/launch/mock_control_launch.py @@ -0,0 +1,100 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node, LifecycleNode +from launch_ros.substitutions import FindPackageShare +import os + +def generate_launch_description(): + pkg_share = FindPackageShare("").find('toid_navigation') + params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') + map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml') + default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz') + + + description_pkg_share = FindPackageShare("").find('toid_bot_description') + default_model_path = os.path.join( + description_pkg_share, + 'src', + 'toid_bot_description.urdf' + ) + + starting_position = LaunchConfiguration("start_position") + visualize = LaunchConfiguration("visualize") + + starting_position_arg = DeclareLaunchArgument( + 'start_position', + default_value='0 0 0 0 0 0', + description="Set initial robot position" + ) + + visualize_arg = DeclareLaunchArgument( + 'visualize', + default_value='True', + description="Whether to launch rviz2" + ) + + odom_broadcast = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='map_to_odom_broadcaster', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters = [{'robot_description': Command(['xacro ', default_model_path])}] + ) + + controller_manager = Node( + package='controller_manager', + executable='ros2_control_node', + output='screen', + parameters = [params] + ) + + joint_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + output='screen', + arguments= ["joint_state_broadcaster"] + ) + + diffbot_base_controller = Node( + package='controller_manager', + executable='spawner', + output='both', + arguments= [ + "diffbot_base_controller", + "-p", + params, + "--controller-ros-args", + "-r diffbot_base_controller/cmd_vel:=/cmd_vel", + "--controller-ros-args", + "-r diffbot_base_controller/odom:=/odom" + ] + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', default_rviz_config_path], + condition=IfCondition(visualize) + ) + + return LaunchDescription([ + starting_position_arg, + visualize_arg, + odom_broadcast, + robot_state_publisher, + controller_manager, + joint_state_broadcaster, + diffbot_base_controller, + rviz_node + ]) \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 9291f8d..87e21fb 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -58,8 +58,6 @@ diffbot_base_controller: cmd_vel_timeout: 0.5 - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* linear.x.has_velocity_limits: true linear.x.has_acceleration_limits: true linear.x.has_jerk_limits: false diff --git a/toid_navigation/rviz/ros_control.rviz b/toid_navigation/rviz/ros_control.rviz index 1cb92b5..c9ca39a 100644 --- a/toid_navigation/rviz/ros_control.rviz +++ b/toid_navigation/rviz/ros_control.rviz @@ -45,52 +45,6 @@ Visualization Manager: Plane Cell Count: 64 Reference Frame: Value: true - - Alpha: 1 - Binary representation: false - Binary threshold: 100 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates - Use Timestamp: false - Value: true - - Alpha: 0.30000001192092896 - Binary representation: false - Binary threshold: 100 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: true - Enabled: true - Name: GlobalCostmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /costmap_updates - Use Timestamp: false - Value: true - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false @@ -196,21 +150,26 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.2665987014770508e-07 - Class: rviz_default_plugins/TopDownOrtho + Class: rviz_default_plugins/XYOrbit + Distance: 2.6907618045806885 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 176.13690185546875 + Pitch: 0.4453992545604706 Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: 0.03602159023284912 - Y: -0.12757755815982819 + Value: XYOrbit (rviz_default_plugins) + Yaw: 0.8803985118865967 Saved: ~ Window Geometry: Displays: