Added launch file to start ros2 control

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
+    ])

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

toid_navigation/rviz/ros_control.rviz

@@ -45,52 +45,6 @@ Visualization Manager:
       Plane Cell Count: 64
       Reference Frame: <Fixed 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: <Fixed Frame>
-      Value: TopDownOrtho (rviz_default_plugins)
-      X: 0.03602159023284912
-      Y: -0.12757755815982819
+      Value: XYOrbit (rviz_default_plugins)
+      Yaw: 0.8803985118865967
     Saved: ~
 Window Geometry:
   Displays: