Robot Control works in RViz but not in Gazebo

[thisismoiself]

I used the champ_setup_assistant for a config for my own .urdf file of the Unitree Go1 Robot. When I launch the config via roslaunch go1_custom_config bringup.launch rviz:=true everything seems to work fine and i can control the robot through roslaunch champ_teleop teleop.launch and my keyboard.

However, when launching the same config with roslaunch go1_custom_config gazebo.launch the robot just lays on its belly and does nothing. I also see that the transforms for all of the joints seem to be missing from rviz and rosrun tf tf_monitor.

Any clues on why that is? Thank you for your help!

[morg1207]

Understanding the Issue with ROS2 Controllers and Controller Manager

I encountered a problem and here’s what I learned:

1. Launching ROS2 Controllers Requires controller_manager:
   To launch ROS2 controllers, the controller_manager must be running. There are two ways to achieve this:

2. First Method: Using ros2_control_node
   You can launch the controller_manager using the node provided by the controller_manager package, called ros2_control_node. This can be done with the following command:

   ros2 run controller_manager ros2_control_node

   This method is not used in CHAMP packages, since gazebo is in charge of launching this as we will see in the following lines.

3. Second Method: Using gazebo_ros2_control Plugin
   This method is used in CHAMP packages via the gazebo_ros2_control plugin. Here’s how it works:

   In the champ_urdf.xacro file, the gazebo_ros2_control plugin is loaded with the following configuration:

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

   When I checked the plugin’s repository, specifically this file, I found the following lines of code:

   // Create the controller manager
   RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loading controller_manager");
   rclcpp::NodeOptions options = controller_manager::get_cm_node_options();
   options.arguments(arguments);
   impl_->controller_manager_.reset(
       new controller_manager::ControllerManager(
           std::move(resource_manager_),
           impl_->executor_,
           controllerManagerNodeName,
           impl_->model_nh_->get_namespace(), options));

   These lines indicate that the controller_manager is being launched.

4. Error Encountered:
   When I tried to run the controllers, I received the following error:

   Could not contact service /controller_manager/load_controller
   [ERROR]: process has died [pid 127, exit code 1, cmd 'ros2 control load_controller --set-state active joint_states_controller'].

   This error message, "Could not contact service /controller_manager/load_controller", suggests that the service was not available at the time of the request. This happens because the controller_manager takes some time to start up.

   If you check the last lines of the terminal output:

   [gzserver-7] [INFO] [1724725219.832408974] [gazebo_ros2_control]: Loading controller_manager
   [gzserver-7] [WARN] [1724725219.918087562] [gazebo_ros2_control]: Desired controller update period (0.004 s) is slower than the gazebo simulation period (0.001 s).
   [gzserver-7] [INFO] [1724725219.918704120] [gazebo_ros2_control]: Loaded gazebo_ros2_control.

   You’ll notice that "Loading controller_manager" is logged towards the end, indicating that the controller_manager wasn’t ready when the controllers were trying to load.

5. Solution:
   To solve this problem, you can insert a delay in the launch file after start_gazebo_spawner_cmd. Specifically, you should modify the controller loading commands as follows:

   Replace:

   load_joint_trajectory_effort_controller,
   load_joint_trajectory_position_controller

   With:

   from launch.event_handlers import OnExecutionComplete
   from launch.actions import RegisterEventHandler, TimerAction
   
   TimerAction(period=100.0, actions=[load_joint_state_controller]),
   RegisterEventHandler(
       event_handler=OnExecutionComplete(
           target_action=load_joint_state_controller,
           on_completion=[load_joint_trajectory_effort_controller],
       )
   ),

   This change will introduce a delay, ensuring that the controller_manager is fully loaded before attempting to load the controllers.