ros-controls / ros2_control

Generic and simple controls framework for ROS 2
https://control.ros.org
Apache License 2.0
471 stars 287 forks source link

/controller_manager is not loading up #1648

Open nisathav opened 1 month ago

nisathav commented 1 month ago

Describe the bug I am trying to run a differential drive robot in my computer. The controls are working fine when used gazebo_controls but when I am switching to ros2_controls, the following errors are received. I am using ros2 humble version.

nisat@nisat:~/Desktop/ros2_ws$ ros2 launch mangobee_differentialdrivewheel_robot launch_sim.launch.py 
[INFO] [launch]: All log files can be found below /home/nisat/.ros/log/2024-07-31-16-11-02-560368-nisat-20693
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [20697]
[INFO] [twist_mux-2]: process started with pid [20699]
[INFO] [gzserver-3]: process started with pid [20701]
[INFO] [gzclient-4]: process started with pid [20703]
[INFO] [spawn_entity.py-5]: process started with pid [20705]
[INFO] [spawner-6]: process started with pid [20707]
[INFO] [spawner-7]: process started with pid [20709]
[robot_state_publisher-1] [INFO] [1722422463.063742555] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-1] [INFO] [1722422463.063798690] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1722422463.063803619] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-1] [INFO] [1722422463.063806124] [robot_state_publisher]: got segment camera_link_optical
[robot_state_publisher-1] [INFO] [1722422463.063808478] [robot_state_publisher]: got segment caster_wheel
[robot_state_publisher-1] [INFO] [1722422463.063810718] [robot_state_publisher]: got segment chassis
[robot_state_publisher-1] [INFO] [1722422463.063813059] [robot_state_publisher]: got segment laser_frame
[robot_state_publisher-1] [INFO] [1722422463.063815196] [robot_state_publisher]: got segment left_wheel
[robot_state_publisher-1] [INFO] [1722422463.063817373] [robot_state_publisher]: got segment right_wheel
[twist_mux-2] [INFO] [1722422463.401782829] [twist_mux]: Topic handler 'topics.joystick' subscribed to topic 'cmd_vel': timeout = 0.500000s , priority = 100.
[twist_mux-2] [INFO] [1722422463.403815181] [twist_mux]: Topic handler 'topics.navigation' subscribed to topic 'cmd_vel': timeout = 0.500000s , priority = 10.
[twist_mux-2] [INFO] [1722422463.404142094] [twist_mux]: Topic handler 'topics.tracker' subscribed to topic 'cmd_vel_tracker': timeout = 0.500000s , priority = 20.
[spawn_entity.py-5] [INFO] [1722422463.434962560] [spawn_entity]: Spawn Entity started
[spawn_entity.py-5] [INFO] [1722422463.435144421] [spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-5] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[spawn_entity.py-5]   warnings.warn(
[spawn_entity.py-5] [INFO] [1722422463.436495744] [spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-5] [INFO] [1722422463.447771137] [spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-5] [INFO] [1722422463.447998624] [spawn_entity]: Waiting for service /spawn_entity
[spawn_entity.py-5] [INFO] [1722422463.952209711] [spawn_entity]: Calling service /spawn_entity
[gzserver-3] [INFO] [1722422464.780959597] [camera_controller]: Publishing camera info to [/camera/camera_info]
[gzserver-3] [WARN] [1722422464.786500354] [rcl]: Found remap rule '~/out:=scan'. This syntax is deprecated. Use '--ros-args --remap ~/out:=scan' instead.
[gzserver-3] [WARN] [1722422464.788452744] [rcl]: Found remap rule '~/out:=scan'. This syntax is deprecated. Use '--ros-args --remap ~/out:=scan' instead.
[spawn_entity.py-5] [INFO] [1722422464.881699912] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [mangobee_differentialdrivewheel_robot]
[INFO] [spawn_entity.py-5]: process has finished cleanly [pid 20705]
[spawner-7] [INFO] [1722422465.440287175] [spawner_diff_cont]: Waiting for '/controller_manager' node to exist
[spawner-6] [INFO] [1722422465.442477638] [spawner_joint_broad]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1722422467.452992102] [spawner_diff_cont]: Waiting for '/controller_manager' node to exist
[spawner-6] [INFO] [1722422467.454625794] [spawner_joint_broad]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1722422469.468981534] [spawner_diff_cont]: Waiting for '/controller_manager' node to exist
[spawner-6] [INFO] [1722422469.469326153] [spawner_joint_broad]: Waiting for '/controller_manager' node to exist
[spawner-6] [INFO] [1722422471.483041239] [spawner_joint_broad]: Waiting for '/controller_manager' node to exist
[spawner-7] [INFO] [1722422471.483713395] [spawner_diff_cont]: Waiting for '/controller_manager' node to exist
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[robot_state_publisher-1] [INFO] [1722422471.595004976] [rclcpp]: signal_handler(signum=2)
[twist_mux-2] [INFO] [1722422471.595089902] [rclcpp]: signal_handler(signum=2)
[spawner-6] Traceback (most recent call last):
[spawner-6]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/lib/controller_manager/spawner", line 33, in <module>
[spawner-6]     sys.exit(load_entry_point('controller-manager==2.42.0', 'console_scripts', 'spawner')())
[spawner-6]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 222, in main
[spawner-6]     if not wait_for_controller_manager(
[spawner-6]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 106, in wait_for_controller_manager
[spawner-6]     node_and_namespace = wait_for_value_or(
[spawner-6]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 66, in wait_for_value_or
[spawner-6]     time.sleep(0.2)
[spawner-6] KeyboardInterrupt
[spawner-7] Traceback (most recent call last):
[spawner-7]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/lib/controller_manager/spawner", line 33, in <module>
[spawner-7]     sys.exit(load_entry_point('controller-manager==2.42.0', 'console_scripts', 'spawner')())
[spawner-7]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 222, in main
[spawner-7]     if not wait_for_controller_manager(
[spawner-7]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 106, in wait_for_controller_manager
[spawner-7]     node_and_namespace = wait_for_value_or(
[spawner-7]   File "/home/nisat/Desktop/ros2_ws/install/controller_manager/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 66, in wait_for_value_or
[spawner-7]     time.sleep(0.2)
[spawner-7] KeyboardInterrupt
[ERROR] [spawner-7]: process has died [pid 20709, exit code -2, cmd '/home/nisat/Desktop/ros2_ws/install/controller_manager/lib/controller_manager/spawner diff_cont --controller-manager /controller_manager --ros-args'].
[ERROR] [spawner-6]: process has died [pid 20707, exit code -2, cmd '/home/nisat/Desktop/ros2_ws/install/controller_manager/lib/controller_manager/spawner joint_broad --controller-manager /controller_manager --ros-args'].
[INFO] [robot_state_publisher-1]: process has finished cleanly [pid 20697]
[INFO] [twist_mux-2]: process has finished cleanly [pid 20699]
[INFO] [gzserver-3]: process has finished cleanly [pid 20701]
[INFO] [gzclient-4]: process has finished cleanly [pid 20703]

here is my launch file,

  1. launch_sim.launch.py
    
    import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node

def generate_launch_description():

# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!

package_name='mangobee_differentialdrivewheel_robot' #<--- CHANGE ME

rsp = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory(package_name),'launch','rsp.launch.py'
            )]), launch_arguments={'use_sim_time': 'true'}.items()
)

#Include the twist_mux 
twist_mux_params = os.path.join(get_package_share_directory(package_name),'config','twist_mux.yaml')
twist_mux = Node(
        package="twist_mux",
        executable="twist_mux",
        parameters=[twist_mux_params, {'use_sim_time': True}],
        remappings=[('/cmd_vel_out','/diff_cont/cmd_vel_unstamped')]
    )
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([os.path.join(
                get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
         )

# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                    arguments=['-topic', 'robot_description',
                               '-entity', 'mangobee_differentialdrivewheel_robot'],
                    output='screen')

diff_drive_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["diff_cont","--controller-manager", "/controller_manager"]
)

joint_broad_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["joint_broad", "--controller-manager", "/controller_manager"],
)

# Launch them all!
return LaunchDescription([
    rsp,
    twist_mux,
    gazebo,
    spawn_entity,
    joint_broad_spawner,
    diff_drive_spawner,
])

  2. rsp.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node

import xacro

def generate_launch_description():

# Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')

# Process the URDF file
pkg_path = os.path.join(get_package_share_directory('mangobee_differentialdrivewheel_robot'))
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
robot_description_config = xacro.process_file(xacro_file)

# Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    output='screen',
    parameters=[params]
)

# Launch!
return LaunchDescription([
    DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use sim time if true'),

    node_robot_state_publisher
])"    
Here is my controller yaml file,
```yaml
  "controller_manager:
  ros__parameters:
    update_rate: 100.0
    use_sim_time: true

    diff_cont:
      type: diff_drive_controller/DiffDriveController

    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster

diff_cont:
  ros__parameters:

    publish_rate: 100.0 
    base_frame_id: base_link

    left_wheel_names: ['left_wheel_joint']
    right_wheel_names: ['right_wheel_joint']
    wheel_separation: 0.35
    wheel_radius: 0.05

    use_stamped_vel: false
christophfroehlich commented 1 month ago

Please have a look at our demos. You need to run the controller_manager from the ros2_control_node, which is done by gazebo in the gazebo_ros2_control plugin.

sees1 commented 1 month ago

ummm... He did it. He parse xacro file which include ros2_control tag along with gazebo_ros2_control plugin. robot.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"  name="robot">

    <xacro:arg name="use_ros2_control" default="true"/>
    <xacro:arg name="sim_mode" default="false"/>

    <xacro:include filename="robot_core.xacro" />

    <xacro:if value="$(arg use_ros2_control)">
        <xacro:include filename="ros2_control.xacro" />
    </xacro:if>
    <xacro:unless value="$(arg use_ros2_control)">
        <xacro:include filename="gazebo_control.xacro" />
    </xacro:unless>
    <xacro:include filename="lidar.xacro" />
    <xacro:include filename="camera.xacro" />
    <!-- <xacro:include filename="depth_camera.xacro" /> -->

    <xacro:include filename="face.xacro" />

</robot>

and here ros2_control.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:unless value="$(arg sim_mode)">
        <ros2_control name="RealRobot" type="system">
            <hardware>
                <plugin>diffdrive_arduino/DiffDriveArduinoHardware</plugin>
                <param name="left_wheel_name">left_wheel_joint</param>
                <param name="right_wheel_name">right_wheel_joint</param>
                <param name="loop_rate">30</param>
                <param name="device">/dev/ttyUSB0</param>
                <param name="baud_rate">57600</param>
                <param name="timeout_ms">1000</param>
                <param name="enc_counts_per_rev">3436</param>
            </hardware>
            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
        </ros2_control>
    </xacro:unless>

    <xacro:if value="$(arg sim_mode)">
        <ros2_control name="GazeboSystem" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="velocity"/>
                <state_interface name="position"/>
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-10</param>
                    <param name="max">10</param>
                </command_interface>
                <state_interface name="velocity"/>
                <state_interface name="position"/>
            </joint>
        </ros2_control>
    </xacro:if>

    <gazebo>
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>$(find articubot_one)/config/my_controllers.yaml</parameters>
            <parameters>$(find articubot_one)/config/gaz_ros2_ctl_use_sim.yaml</parameters>
        </plugin>
    </gazebo>

</robot>

for full code check repo: https://github.com/joshnewans/articubot_one. i think that code in question from here.

christophfroehlich commented 1 month ago

ummm... He did it. He parse xacro file which include ros2_control tag along with gazebo_ros2_control plugin. robot.urdf.xacro

No he did not? At least from the launch files he added here. See the launch_robot.launch.py in the linked repo

https://github.com/joshnewans/articubot_one/blob/cd1eb0f503012a65552fd088cdbd9cf8ac133cbc/launch/launch_robot.launch.py#L53-L58

sees1 commented 1 month ago

Maybe he didn't install gazebo_ros2_control package? Because, when i check install folder, i didn't find it, after installing my error is gone, and controller_manager load. But, you point to a launch, that make sense on real robot, not for gazebo simulation. Here 2 launch file, launch_sim.launch.py and launch_robot.launch.py P.S. Sorry for my bad english.