ros-controls/gazebo_ros2_control

Wrappers, tools and additional API's for using ros2_control with Gazebo Classic

gazebo_ros2_control's Issues

gazebo_system.cpp crashes when trying to change multiple joint's PID values

Hi,

I am trying to test the simulated RRBOT robot without moveit! and I am facing some issues when trying to adjust the PID paremeters through the xacro file. As a disclaimer, I followed the documentation provided with the difference of having a 2 DOF robot instead of using the slider_to_cart (which I tested and it works fine).

I believe everything is set up correctly including having only one joint per transmission. I can indeed adjust the PID parameters for the first joint but it fails when trying to declare the parameters inside of the gazebo_system.cpp.
More specifically I am working in the branch ahcorde/update/foxy created by @ahcorde .

On https://github.com/ros-simulation/gazebo_ros2_control/blob/7334f06cb849cc49b222f9ce674c8bbd87a7ab28/gazebo_ros2_control/src/gazebo_system.cpp I can see that the DOF, transmissions and the names of the joints are correctly retrieved from the .xacro file that I am using.

On line 61, when the iterator j is equal to 0 it will declare the PID parameters and initialize the PID for that joint. That works. Then when j is equal to 1, an exception rises. More specifically I believe that it crashes on line 180

A quick test that I did to avoid the issue was to comment out line 164, line 165, line 166, line 167, line 168, line 169 and instead declaring the PID parameters in a different loop just before line 61.
Something like this:

  for (unsigned int k = 0; k < this->n_dof_; k++) {
    nh_->declare_parameter(transmissions[k].joints[0].name + ".p");
    RCLCPP_WARN_STREAM(nh_->get_logger(),"--------------" << k);
    nh_->declare_parameter(transmissions[k].joints[0].name + ".i");
    RCLCPP_WARN_STREAM(nh_->get_logger(),"--------------" << k);
    nh_->declare_parameter(transmissions[k].joints[0].name + ".d");
    RCLCPP_WARN_STREAM(nh_->get_logger(),"--------------" << k);
    nh_->declare_parameter(transmissions[k].joints[0].name + ".i_clamp_max");
    RCLCPP_WARN_STREAM(nh_->get_logger(),"--------------" << k);
    nh_->declare_parameter(transmissions[k].joints[0].name + ".i_clamp_min");
    RCLCPP_WARN_STREAM(nh_->get_logger(),"--------------" << k);
    nh_->declare_parameter(transmissions[k].joints[0].name + ".antiwindup", false);
    RCLCPP_WARN_STREAM(nh_->get_logger(),"--------------" << k);
  }

Of course this is not the most efficient way but it does the job of initializing the PIDs for both joints. I am not sure if the problem is coming from the control_toolbox package or something related to gazebo_ros2_control.

Anyone going through a similar issue?

Thanks in advance for the help.

Kind regards,
Carlos

Loading Controller Crashes Gazebo Server

I am trying to utilize the gazebo_ros2_control package for controlling the position of a revolute joint from a URDF file. However following along with the package README (which thankfully is better than most ROS2 pkg README's, so thank you for that), it seems something in the ros2_control tag is causing gazebo to crash, as the adding the libgazebo_ros2_control.so file with the gazebo tags alone without the ros2_control tags does not crash gazebo.

My URDF file spawned in fine prior to adding the following to the bottom within the <robot> tags. I configured it as follows:

<ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    <joint name="camera_pole_joint">
      <command_interface name="position">
        <param name="min">-3.14159</param>
        <param name="max">3.14159</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </ros2_control>


  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_param>robot_description</robot_param>
      <robot_param_node>robot_state_publisher</robot_param_node>
      <parameters>$(find ros2_sim_pkg)/config/cam_bot.yaml</parameters>
    </plugin>
  </gazebo>

With my config file having:

controller_manager:
  ros__parameters:
    update_rate: 50  # Hz

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_controller:
      type: joint_state_controller/JointStateController

joint_trajectory_controller:
  ros__parameters:
    joints:
      - camera_pole_joint
    interface_name: position

Basically I am trying to create a position controller to interact with my joint called camera_pole_joint

OS: Ubuntu 20.04
ROS Distro: Foxy (apt install)

Output of terminal running gazebo:

ray@ubuntu:~/Workspaces/udemy_ros2_ws$ ros2 launch gazebo_ros gazebo.launch.py 
[INFO] [launch]: All log files can be found below /home/ray/.ros/log/2021-03-31-00-28-41-487954-ubuntu-21664
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [gzserver-1]: process started with pid [21666]
[INFO] [gzclient   -2]: process started with pid [21669]
[gzserver-1] [INFO] [1617186529.478072601] [camera_controller]: Publishing camera info to [/camera1/camera_info]
[gzserver-1] [INFO] [1617186529.587212547] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-1] [INFO] [1617186529.600678750] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-1] [INFO] [1617186529.603022856] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-1] [INFO] [1617186529.622931749] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-1] [INFO] [1617186529.624571891] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-1] [INFO] [1617186529.641574313] [gazebo_ros2_control]: Loading joint: camera_pole_joint
[gzserver-1] [INFO] [1617186529.642266984] [gazebo_ros2_control]: 	Command:
[gzserver-1] [INFO] [1617186529.642590928] [gazebo_ros2_control]: 		 position
[gzserver-1] [INFO] [1617186529.642890565] [gazebo_ros2_control]: 	State:
[gzserver-1] [INFO] [1617186529.643170214] [gazebo_ros2_control]: 		 position
[gzserver-1] [INFO] [1617186529.643638167] [gazebo_ros2_control]: 		 velocity
[gzserver-1] [INFO] [1617186529.644139749] [gazebo_ros2_control]: 		 effort
[gzserver-1] [INFO] [1617186529.646519580] [gazebo_ros2_control]: Loading controller_manager
[gzserver-1] terminate called without an active exception
[gzserver-1] Aborted (core dumped)
[ERROR] [gzserver-1]: process has died [pid 21666, exit code 134, cmd 'gzserver                                                                    -s libgazebo_ros_init.so   -s libgazebo_ros_factory.so       '].

Terminal output of spawning in the URDF does not seem to throw any errors:

ay@ubuntu:~/Workspaces/udemy_ros2_ws$ ros2 launch ros2_sim_pkg cam_bot_world.launch.py 
[INFO] [launch]: All log files can be found below /home/ray/.ros/log/2021-03-31-00-28-47-357037-ubuntu-21761
[INFO] [launch]: Default logging verbosity is set to INFO
urdf_file_name : urdf/camera_bot.xacro
[INFO] [robot_state_publisher-1]: process started with pid [21766]
[INFO] [spawn_entity.py-2]: process started with pid [21768]
[robot_state_publisher-1] [WARN] [1617186527.537782915] [robot_state_publisher]: No robot_description parameter, but command-line argument available.  Assuming argument is name of URDF file.  This backwards compatibility fallback will be removed in the future.
[robot_state_publisher-1] Parsing robot urdf xml string.
[robot_state_publisher-1] Link base had 5 children
[robot_state_publisher-1] Link camera_pole had 1 children
[robot_state_publisher-1] Link camera_link had 0 children
[robot_state_publisher-1] Link wheel_BL had 0 children
[robot_state_publisher-1] Link wheel_BR had 0 children
[robot_state_publisher-1] Link wheel_FL had 0 children
[robot_state_publisher-1] Link wheel_FR had 0 children
[robot_state_publisher-1] [INFO] [1617186527.550467733] [robot_state_publisher]: got segment base
[robot_state_publisher-1] [INFO] [1617186527.550830406] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-1] [INFO] [1617186527.551082150] [robot_state_publisher]: got segment camera_pole
[robot_state_publisher-1] [INFO] [1617186527.551316623] [robot_state_publisher]: got segment robot_footprint
[robot_state_publisher-1] [INFO] [1617186527.552871455] [robot_state_publisher]: got segment wheel_BL
[robot_state_publisher-1] [INFO] [1617186527.553206308] [robot_state_publisher]: got segment wheel_BR
[robot_state_publisher-1] [INFO] [1617186527.553421784] [robot_state_publisher]: got segment wheel_FL
[robot_state_publisher-1] [INFO] [1617186527.553628807] [robot_state_publisher]: got segment wheel_FR
[spawn_entity.py-2] [INFO] [1617186528.166923053] [urdf_spawner]: Spawn Entity started
[spawn_entity.py-2] [INFO] [1617186528.167815622] [urdf_spawner]: Loading entity published on topic /robot_description
[spawn_entity.py-2] [INFO] [1617186528.170817956] [urdf_spawner]: Waiting for entity xml on /robot_description
[spawn_entity.py-2] [INFO] [1617186528.263362879] [urdf_spawner]: Waiting for service /spawn_entity, timeout = 5
[spawn_entity.py-2] [INFO] [1617186528.264286665] [urdf_spawner]: Waiting for service /spawn_entity
[spawn_entity.py-2] [INFO] [1617186528.273687918] [urdf_spawner]: Calling service /spawn_entity
[spawn_entity.py-2] [INFO] [1617186528.582535103] [urdf_spawner]: Spawn status: SpawnEntity: Successfully spawned entity [cam_bot]
[INFO] [spawn_entity.py-2]: process has finished cleanly [pid 21768]

Here is my launch file in case it is of use:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

  use_sim_time = LaunchConfiguration('use_sim_time', default='false')
  urdf_file_name = 'urdf/camera_bot.xacro'

  print("urdf_file_name : {}".format(urdf_file_name))

  urdf = os.path.join(
      get_package_share_directory('ros2_sim_pkg'),
      urdf_file_name)

  return LaunchDescription([

        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        # ExecuteProcess(
        #     cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
        #     output='screen'),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=[urdf]),

        # Node(
        #     package='joint_state_publisher',
        #     executable='joint_state_publisher',
        #     name='joint_state_publisher',
        #     output='screen',
        #     parameters=[{'use_sim_time': use_sim_time}]
        #     ),

        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            name='urdf_spawner',
            output='screen',
            arguments=["-topic", "/robot_description", "-entity", "cam_bot"])
  ])

I currently have the joint state publisher and auto launching gazebo commented out, as I was trying to make sure those were not interfering with the libgazebo_ros2_control.so.

hardware_interface/EffortJointInterface hasn't been implemented yet?

Hi,

As the question says, hardware_interface/EffortJointInterface seems to not work yet. Changing it to hardware_interface/PositionJointInterface makes everything work but that is not what I need to use in my software. Any advice?

Thank you very much,
Jaehyun

CMake Error at CMakeLists.txt:6 (find_package): "controller_manager"

Hi,

/gazebo_ros2_control_demos builds,
but not /gazebo_ros2_control

Starting >>> gazebo_ros2_control
--- stderr: gazebo_ros2_control
CMake Error at CMakeLists.txt:6 (find_package):
  By not providing "Findcontroller_manager.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "controller_manager", but CMake did not find one.

  Could not find a package configuration file provided by
  "controller_manager" with any of the following names:

    controller_managerConfig.cmake
    controller_manager-config.cmake

  Add the installation prefix of "controller_manager" to CMAKE_PREFIX_PATH or
  set "controller_manager_DIR" to a directory containing one of the above
  files.  If "controller_manager" provides a separate development package or
  SDK, be sure it has been installed.


---
Failed   <<< gazebo_ros2_control [1.77s, exited with code 1]

Summary: 0 packages finished [1.87s]
  1 package failed: gazebo_ros2_control
  1 package had stderr output: gazebo_ros2_control

any ideas?

Can't build on Foxy distro

I tried to build this package. However, it is impossible to build because the package compatibility is not appropriate. If you try to build by cloning the latest version of the ros2_control package, you cannot build it because there is no joint_state_handle package.
Is there any way to solve this problem?

Fix compile warnings

There are some compile warnings right now. They show up on CI, but it's not setup to fail if there are warnings.

In file included from /home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp:64,
                 from /home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/src/default_robot_hw_sim.cpp:38:
/home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp: In member function _virtual hardware_interface::hardware_interface_ret_t gazebo_ros2_control::RobotHWSim::init()_:
/home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp:117:64: warning: no return statement in function returning non-void [-Wreturn-type]
  117 |   virtual hardware_interface::hardware_interface_ret_t init() {}
      |                                                                ^
/home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp: In member function _virtual hardware_interface::hardware_interface_ret_t gazebo_ros2_control::RobotHWSim::read()_:
/home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp:118:64: warning: no return statement in function returning non-void [-Wreturn-type]
  118 |   virtual hardware_interface::hardware_interface_ret_t read() {}
      |                                                                ^
/home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp: In member function _virtual hardware_interface::hardware_interface_ret_t gazebo_ros2_control::RobotHWSim::write()_:
/home/chapulina/ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp:119:65: warning: no return statement in function returning non-void [-Wreturn-type]
  119 |   virtual hardware_interface::hardware_interface_ret_t write() {}
      |                                                                 ^

rocker instruction does not work

Hi,

This rocker instruction does not work. The below line is the error message from the terminal.

Failed to build detector image
WARNING unable to detect os for base image 'gazebo_ros2_control:latest', maybe the base image does not exist

Regards,
Jaehyun

Loader for controller "effort_controllers" not found

Hi,
Apologies if this is a simple fix or a known bug, (or if I've misread the docs 😛 ), but I cant seem to run the effort demonstration that you have here in the repo (the ones via docker). I can run the position and velocity demos running fine but the effort one eventually terminates with the error [controller_manager]: Loader for controller 'effort_controllers' not found.

My understanding is that I run:

docker run -it --rm --name gazebo_ros2_control_demo --net host gazebo_ros2_control ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py gui:=false

Where we have gazebo_ros2_control_demos cart_example_effort.launch.py rather than gazebo_ros2_control_demos cart_example_position.launch.py.

I am not sure where to look to fix it (it might be the package.xml file missing the right exec_depends?)
I hope to see this demo run since I plan on implementing torque/effort controllers on 4 individual revolute joints on a rover/car model that I have in Gazebo.

Thanks in advance for any insight you can provide!

-Lachlan

Use rosdep to install dependencies in CI

Right now several dependencies are being explicitly installed in CI:

https://github.com/ros-simulation/gazebo_ros2_control/blob/6b3028db81291666c7d615d3c51351ecb92c559a/.github/workflows/ci.yaml#L25-L31

It should be possible to use rosdep to install all dependencies as listed on package.xml, this way when new dependencies are added CI doesn't need to be updated.

For example:

rosdep install --from-paths ./ -i -y --rosdistro foxy \
  --skip-keys=ros2_control \
  ...

This is just a suggestion, feel free to ignore.

Prismatic joint drops although the position command says "do not move"!

Hi all,

I have a vertical prismatic joint that I want to control using the "position control" interface. My position command requires the joint to stay at a point for a while, but the joint creeps downwards despite a constant position command. See the video clip.

Falling_Prismatic_Joint_Gazebo_ros2_control_demo_01.mp4

Why is this happening?

Here is my urdf:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="cartopole">
  <link name="world"/>
  <link name="slideBar">
    <visual>
      <geometry>
        <box size="30 0.05 0.05"/>
      </geometry>
      <origin xyz="0 0 0"/>
      <material name="green">
        <color rgba="0 0.8 .8 1"/>
      </material>
    </visual>
    <inertial>
      <mass value="100"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="cart">
    <visual>
      <geometry>
        <box size="0.5 0.5 0.2"/>
      </geometry>
      <origin xyz="0 0 0"/>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="world_to_base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 1"/>
    <parent link="world"/>
    <child link="slideBar"/>
  </joint>
  <joint name="slider_to_cart" type="prismatic">
    <axis xyz="0 0 1"/>
    <origin xyz="0.0 0.0 0.0"/>
    <parent link="slideBar"/>
    <child link="cart"/>
    <limit effort="1000.0" lower="0" upper="15" velocity="30"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    <joint name="slider_to_cart">
      <command_interface name="position">
        <param name="min">0</param>
        <param name="max">15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </ros2_control>

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

Here is my controller config:

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

joint_trajectory_controller:
  ros__parameters:
    joints:
      - slider_to_cart
    interface_name: position
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

Here is my cpp driving command:

// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "control_msgs/action/follow_joint_trajectory.hpp"

std::shared_ptr<rclcpp::Node> node;
bool common_goal_accepted = false;
rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;

void common_goal_response(
  std::shared_future<rclcpp_action::ClientGoalHandle
  <control_msgs::action::FollowJointTrajectory>::SharedPtr> future)
{
  RCLCPP_DEBUG(
    node->get_logger(), "common_goal_response time: %f",
    rclcpp::Clock().now().seconds());
  auto goal_handle = future.get();
  if (!goal_handle) {
    common_goal_accepted = false;
    printf("Goal rejected\n");
  } else {
    common_goal_accepted = true;
    printf("Goal accepted\n");
  }
}

void common_result_response(
  const rclcpp_action::ClientGoalHandle
  <control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
{
  printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
  common_resultcode = result.code;
  common_action_result_code = result.result->error_code;
  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      printf("SUCCEEDED result code\n");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      printf("Goal was aborted\n");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      printf("Goal was canceled\n");
      return;
    default:
      printf("Unknown result code\n");
      return;
  }
}

void common_feedback(
  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr,
  const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback)
{
  std::cout << "feedback->desired.positions :";
  for (auto & x : feedback->desired.positions) {
    std::cout << x << "\t";
  }
  std::cout << std::endl;
  std::cout << "feedback->desired.velocities :";
  for (auto & x : feedback->desired.velocities) {
    std::cout << x << "\t";
  }
  std::cout << std::endl;
}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);

  node = std::make_shared<rclcpp::Node>("trajectory_test_node");

  std::cout << "node created" << std::endl;

  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
  action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
    node->get_node_base_interface(),
    node->get_node_graph_interface(),
    node->get_node_logging_interface(),
    node->get_node_waitables_interface(),
    "/joint_trajectory_controller/follow_joint_trajectory");

  bool response =
    action_client->wait_for_action_server(std::chrono::seconds(1));
  if (!response) {
    throw std::runtime_error("could not get action server");
  }
  std::cout << "Created action server" << std::endl;

  std::vector<std::string> joint_names = {"slider_to_cart"};

  std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
  trajectory_msgs::msg::JointTrajectoryPoint point;
  point.time_from_start = rclcpp::Duration::from_seconds(0.0);  // start asap
  point.positions.resize(joint_names.size());

  point.positions[0] = 0.0;

  trajectory_msgs::msg::JointTrajectoryPoint point2;
  point2.time_from_start = rclcpp::Duration::from_seconds(5.0);
  point2.positions.resize(joint_names.size());
  point2.positions[0] = 1.0;

  trajectory_msgs::msg::JointTrajectoryPoint point3;
  point3.time_from_start = rclcpp::Duration::from_seconds(10.0);
  point3.positions.resize(joint_names.size());
  point3.positions[0] = 1.0;

  trajectory_msgs::msg::JointTrajectoryPoint point4;
  point4.time_from_start = rclcpp::Duration::from_seconds(15.0);
  point4.positions.resize(joint_names.size());
  point4.positions[0] = 1.0;

  points.push_back(point);
  points.push_back(point2);
  points.push_back(point3);
  points.push_back(point4);

  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
  opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1);
  opt.result_callback = std::bind(common_result_response, std::placeholders::_1);
  opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2);

  control_msgs::action::FollowJointTrajectory_Goal goal_msg;
  goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
  goal_msg.trajectory.joint_names = joint_names;
  goal_msg.trajectory.points = points;

  auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);

  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
    rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
    return 1;
  }
  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");

  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
    goal_handle = goal_handle_future.get();
  if (!goal_handle) {
    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
    return 1;
  }
  RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");

  // Wait for the server to be done with the goal
  auto result_future = action_client->async_get_result(goal_handle);
  RCLCPP_INFO(node->get_logger(), "Waiting for result");
  if (rclcpp::spin_until_future_complete(node, result_future) !=
    rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
    return 1;
  }

  std::cout << "async_send_goal" << std::endl;
  rclcpp::shutdown();

  return 0;
}

ROS2 Foxy mimic joint control

How to use ROS2 Foxy + MoveIt2 + Gazebo to realized a mimic joint control?
I only see the master branch has a demo of mimic joint control for the parallel gripper. But I can't build the master branch on ROS2 Foxy.

Only support Gazebo 9+

This package builds on top of gazebo_ros_pkgs, and that package's ROS 2 integration has been explicitly targeting only Gazebo versions above 9. On the contribution guide we recommend "Remove all ifdefs for GAZEBO_MAJOR_VERSION for Gazebo 8 and below."

So I recommend doing the same on this package, on parts like this:

https://github.com/ros-simulation/gazebo_ros2_control/blob/a1318f8b1da9285ea3f647f575c1b37c1ef2975d/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L206-L211

Waiting for /controller_manager services issue

Not sure if this is the right place to ask for the foxy ros2 control API.

I added the tag on URDF with this section:

<gazebo> 
	<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control"> 
		<robotNamespace>/rrbot</robotNamespace>
		<parameters>$(find my_robot_description)/config/gazebo_ros_control_params.yaml</parameters> 
	</plugin> 
</gazebo>

The result from terminal

[spawner.py-6] [INFO] [1629924862.520367523] [spawner_joint_state_broadcaster]: Waiting for /controller_manager services
[spawner.py-7] [INFO] [1629924862.614812317] [spawner_wheel_velocity_controller]: Waiting for /controller_manager services

full screenshot:

Is this a bug or am I just missing on something?

Default joint position for simulated robot

Hi there! I just have seen a long-awaited feature with commit 26216b4 to set the initial joint position by @destogl
The example works fine, but it seems that it works only with a position command interface?

Just to be sure that I got that right: Setting an initial position in test_cart_effort.xacro.urdf does not have the desired effect when launching with
ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py
despite the console output

Loading joint: slider_to_cart
[gzserver-1] [INFO] [1644310803.234515970] [gazebo_ros2_control]: State:
[gzserver-1] [INFO] [1644310803.234586772] [gazebo_ros2_control]: position
[gzserver-1] [INFO] [1644310803.235033020] [gazebo_ros2_control]: found initial value: 1.000000

If this is the (intended) case, it is no direct replacement for the -J option for gazebo_ros spawn_entity.py? E.g., as requested here
ros-simulation/gazebo_ros_pkgs#1233 or https://answers.gazebosim.org/question/27584/ros2-setting-initial-joint-position-at-spawn/

Is there any other known workaround available you could point me to? Thanks.

Can't lower the controller manager update rate

As it is right now it's not possible to lower the controller manager update rate without affecting the simulation. This is because the frequency at which the positions are written to gazebo is coupled with the controller manager update rate.

You can easily reproduce the issue by lowering the rate to 100hz in the rrbot_moveit_demo_nodes. You'll see the arm trembling: position is set, arm falls for 10ms (instead of 1ms), position is set, etc.

Given that not all controllers need to or can't simply run at 1Khz this a problem.

One simple fix is to make the write() independent from the controller manager update rate. Like this:

// Called by the world update start event
void GazeboRosControlPrivate::Update()
{
  // Get the simulation time and period
  gazebo::common::Time gz_time_now = parent_model_->GetWorld()->SimTime();
  rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
  rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;

  if (sim_period >= control_period_) {
    last_update_sim_time_ros_ = sim_time_ros;
    controller_manager_->read();
    controller_manager_->update();
  }

  // always do this
  controller_manager_->write();
}

Though I don't know what other possible implications this could have.

Gazebo Control Plugin Does Not Care Whether a Resource is Claimed or Not

Adding in a position command interface in the ros2_control tag inside the URDF stops the velocity controller from operating.
Position and Joint Trajectory Controllers still operate fine.

More Details:
Ubuntu 20.04
ROS2 Foxy
Build ros2-control from source Main Branch (pulled 6/30/21)

Attribute the problem to the write function in gazebo_system.cpp. Cycles through defined command interfaces and sets directly.

for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
    if (this->dataPtr->sim_joints_[j]) {
      if (this->dataPtr->joint_control_methods_[j] & POSITION) {
        this->dataPtr->sim_joints_[j]->SetPosition(
          0, this->dataPtr->joint_position_cmd_[j],
          true);
      }
      if (this->dataPtr->joint_control_methods_[j] & VELOCITY) {
        this->dataPtr->sim_joints_[j]->SetVelocity(
          0,
          this->dataPtr->joint_velocity_cmd_[j]);
      }
      if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
        const double effort =
          this->dataPtr->joint_effort_cmd_[j];
        this->dataPtr->sim_joints_[j]->SetForce(0, effort);
      }
    }
  } 

Proposed fix is to first check whether a resource is claimed before setting.

Unstable behaviour of Position Controller in UR10

Hello,

I recently tested your gazebo_ros2_control plugin (master branch) using position interface with a ur10 robot + MoveIt

by just modifying the rrbot moveit demo for the ur10

I noticed that the joint_trajectory_controller was unstable causing the robot to take a long time to go in the default position
and breaking up like so

Reducing the inertial values by a factor of 100 seemed to fix the problem in the beginning but it becomes unstable after a while
or after using Moveit to move it
(i haven't tested effort or velocity controllers yet since i don't want to tune pid's)

Any ideas?

Thanks a lot!

Joint velocity limits in position control

Hi! Iam trying to implement a custom controller with ros2_control, which uses multiple joints both position and velocity interfaces.

When I request a new position on one joint (controlled in position), the gazebo link is moving instantaneously to its new position. This does not happen in ros_control in noetic where the gazebo link will move according to the joint limit set in the URDF.

Since the gazebo_ros_control and gazebo_ros2_control plugins both use the same function SetPosition() in the gazebo API, the only difference I can spot is the absence of a joint_control_interface like implemented here: https://github.com/ros-simulation/gazebo_ros_pkgs/blob/noetic-devel/gazebo_ros_control/src/default_robot_hw_sim.cpp#L302

Am I correct to say that joints' limits are not implemented yet? Is it in the roadmap? I find this feature quite important to reproduce the effect on a real position controlled joint on a robot (for instance with a stepper motor). Thanks a lot!

Colcon build error, is this foxy supported?

When I tried to run colcon build inside the /gazebo_ros2_control, I received error like this

bwuk@robots:~/gazebo_ros2_control$ source /opt/ros/foxy/setup.bash 
bwuk@robots:~/gazebo_ros2_control$ colcon build --merge-install
Starting >>> gazebo_ros2_control
--- stderr: gazebo_ros2_control                         
CMake Error at CMakeLists.txt:6 (find_package):
  By not providing "Findcontroller_manager.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "controller_manager", but CMake did not find one.

  Could not find a package configuration file provided by
  "controller_manager" with any of the following names:

    controller_managerConfig.cmake
    controller_manager-config.cmake

  Add the installation prefix of "controller_manager" to CMAKE_PREFIX_PATH or
  set "controller_manager_DIR" to a directory containing one of the above
  files.  If "controller_manager" provides a separate development package or
  SDK, be sure it has been installed.


---
Failed   <<< gazebo_ros2_control [2.13s, exited with code 1]

Summary: 0 packages finished [2.41s]
  1 package failed: gazebo_ros2_control
  1 package had stderr output: gazebo_ros2_control
  1 package not processed

Is this supported on Foxy?

What did I miss with this one?

Add a diff_driver demo

Hi, can we add a diff_driver demo for gazebo_ros2_control, like they do in ros2_control repository? It will help a lot for beginners on ros2_control and gazebo_ros2_control like me! @v-lopez seems to achieve it in #44 (comment), thanks a lot if you can share your code in some ways!

cannot compile

when running

docker build -t gazebo_ros2_control .

i get this error

Step 5/8 : RUN cd /home/ros2_ws/   && . /opt/ros/foxy/setup.sh   && colcon build --merge-install
 ---> Running in 08c031c1e67a
Starting >>> gazebo_ros2_control
--- stderr: gazebo_ros2_control
In file included from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:21:
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:41:3: error: ‘CallbackReturn’ does not name a type
   41 |   CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info)
      |   ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:51:3: error: ‘CallbackReturn’ does not name a type
   51 |   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
      |   ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:54:3: error: ‘CallbackReturn’ does not name a type
   54 |   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
      |   ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:352:1: error: ‘CallbackReturn’ does not name a type
  352 | CallbackReturn
      | ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:373:1: error: ‘CallbackReturn’ does not name a type
  373 | CallbackReturn GazeboSystem::on_activate(const rclcpp_lifecycle::State & previous_state)
      | ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:378:1: error: ‘CallbackReturn’ does not name a type
  378 | CallbackReturn GazeboSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state)
      | ^~~~~~~~~~~~~~
In file included from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp:43:
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:41:3: error: ‘CallbackReturn’ does not name a type
   41 |   CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info)
      |   ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:51:3: error: ‘CallbackReturn’ does not name a type
   51 |   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
      |   ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:54:3: error: ‘CallbackReturn’ does not name a type
   54 |   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
      |   ^~~~~~~~~~~~~~
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp: In member function ‘virtual void gazebo_ros2_control::GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr, sdf::v9::ElementPtr)’:
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp:305:85: error: no matching function for call to ‘hardware_interface::ResourceManager::import_component(std::remove_reference<std::unique_ptr<gazebo_ros2_control::GazeboSystemInterface>&>::type, __gnu_cxx::__alloc_traits<std::allocator<hardware_interface::HardwareInfo>, hardware_interface::HardwareInfo>::value_type&)’
  305 |     resource_manager_->import_component(std::move(gazeboSystem), control_hardware[i]);
      |                                                                                     ^
In file included from /opt/ros/foxy/include/controller_manager/controller_manager.hpp:39,
                 from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp:42,
                 from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp:42:
/opt/ros/foxy/include/hardware_interface/resource_manager.hpp:151:8: note: candidate: ‘void hardware_interface::ResourceManager::import_component(std::unique_ptr<hardware_interface::ActuatorInterface>)’
  151 |   void import_component(std::unique_ptr<ActuatorInterface> actuator);
      |        ^~~~~~~~~~~~~~~~
/opt/ros/foxy/include/hardware_interface/resource_manager.hpp:151:8: note:   candidate expects 1 argument, 2 provided
/opt/ros/foxy/include/hardware_interface/resource_manager.hpp:169:8: note: candidate: ‘void hardware_interface::ResourceManager::import_component(std::unique_ptr<hardware_interface::SensorInterface>)’
  169 |   void import_component(std::unique_ptr<SensorInterface> sensor);
      |        ^~~~~~~~~~~~~~~~
/opt/ros/foxy/include/hardware_interface/resource_manager.hpp:169:8: note:   candidate expects 1 argument, 2 provided
/opt/ros/foxy/include/hardware_interface/resource_manager.hpp:187:8: note: candidate: ‘void hardware_interface::ResourceManager::import_component(std::unique_ptr<hardware_interface::SystemInterface>)’
  187 |   void import_component(std::unique_ptr<SystemInterface> system);
      |        ^~~~~~~~~~~~~~~~
/opt/ros/foxy/include/hardware_interface/resource_manager.hpp:187:8: note:   candidate expects 1 argument, 2 provided
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp: In member function ‘void gazebo_ros2_control::GazeboRosControlPrivate::Update()’:
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp:373:57: error: no matching function for call to ‘controller_manager::ControllerManager::update(rclcpp::Time&, rclcpp::Duration&)’
  373 |     controller_manager_->update(sim_time_ros, sim_period);
      |                                                         ^
In file included from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp:42,
                 from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp:42:
/opt/ros/foxy/include/controller_manager/controller_manager.hpp:133:37: note: candidate: ‘controller_interface::return_type controller_manager::ControllerManager::update()’
  133 |   controller_interface::return_type update();
      |                                     ^~~~~~
/opt/ros/foxy/include/controller_manager/controller_manager.hpp:133:37: note:   candidate expects 0 arguments, 2 provided
In file included from /opt/ros/foxy/include/class_loader/class_loader_core.hpp:57,
                 from /opt/ros/foxy/include/class_loader/class_loader.hpp:55,
                 from /opt/ros/foxy/include/pluginlib/class_list_macros.hpp:40,
                 from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:455:
/opt/ros/foxy/include/class_loader/meta_object.hpp: In instantiation of ‘B* class_loader::impl::MetaObject<C, B>::create() const [with C = gazebo_ros2_control::GazeboSystem; B = gazebo_ros2_control::GazeboSystemInterface]’:
/opt/ros/foxy/include/class_loader/meta_object.hpp:216:7:   required from here
/opt/ros/foxy/include/class_loader/meta_object.hpp:218:12: error: invalid new-expression of abstract class type ‘gazebo_ros2_control::GazeboSystem’
  218 |     return new C;
      |            ^~~~~
In file included from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:21:
/home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:37:7: note:   because the following virtual functions are pure within ‘gazebo_ros2_control::GazeboSystem’:
   37 | class GazeboSystem : public GazeboSystemInterface
      |       ^~~~~~~~~~~~
In file included from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp:27,
                 from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp:25,
                 from /home/ros2_ws/src/gazebo_ros2_control/gazebo_ros2_control/src/gazebo_system.cpp:21:
/opt/ros/foxy/include/hardware_interface/system_interface.hpp:48:23: note: 	‘virtual hardware_interface::return_type hardware_interface::SystemInterface::configure(const hardware_interface::HardwareInfo&)’
   48 |   virtual return_type configure(const HardwareInfo & system_info) = 0;
      |                       ^~~~~~~~~
/opt/ros/foxy/include/hardware_interface/system_interface.hpp:116:23: note: 	‘virtual hardware_interface::return_type hardware_interface::SystemInterface::start()’
  116 |   virtual return_type start() = 0;
      |                       ^~~~~
/opt/ros/foxy/include/hardware_interface/system_interface.hpp:122:23: note: 	‘virtual hardware_interface::return_type hardware_interface::SystemInterface::stop()’
  122 |   virtual return_type stop() = 0;
      |                       ^~~~
/opt/ros/foxy/include/hardware_interface/system_interface.hpp:128:23: note: 	‘virtual std::string hardware_interface::SystemInterface::get_name() const’
  128 |   virtual std::string get_name() const = 0;
      |                       ^~~~~~~~
/opt/ros/foxy/include/hardware_interface/system_interface.hpp:134:18: note: 	‘virtual hardware_interface::status hardware_interface::SystemInterface::get_status() const’
  134 |   virtual status get_status() const = 0;
      |                  ^~~~~~~~~~
make[2]: *** [CMakeFiles/gazebo_hardware_plugins.dir/build.make:63: CMakeFiles/gazebo_hardware_plugins.dir/src/gazebo_system.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:80: CMakeFiles/gazebo_hardware_plugins.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/gazebo_ros2_control.dir/build.make:63: CMakeFiles/gazebo_ros2_control.dir/src/gazebo_ros2_control_plugin.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:107: CMakeFiles/gazebo_ros2_control.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< gazebo_ros2_control [12.2s, exited with code 2]

Plugin won't load with Moveit sourced

I've been trying to get this library working when I also have Moveit sourced in Foxy. If I run

# Download repos
mkdir ~/control_ws/src -p
cd ~/control_ws/src
git clone https://github.com/ros-simulation/gazebo_ros2_control 
git clone https://github.com/ros-controls/ros2_control 
git clone https://github.com/ros-controls/ros2_controllers

# Switch GR2C to latest PR
cd gazebo_ros2_control
git fetch origin pull/44/head
git checkout FETCH_HEAD
cd ../..

# Build repos
. /opt/ros/foxy/setup.bash
colcon build --merge-install --symlink-install
. install/setup.bash

# Run demo
ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py

# Seperate terminal
ros2 run gazebo_ros2_control_demos example_position 

the demo runs great. However, once I source my moveit workspace that I'm currently using for motion planning, the plugin can no longer be imported

. ~/moveit_ws/install/setup.bash

ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py &
ros2 run gazebo_ros2_control_demos example_position 

gives

node created
terminate called after throwing an instance of 'std::runtime_error'
  what():  could not get action server

On further inspection, trying it with Gazebo set to verbose output, it says that it is failing to load the plugin:

[spawn_entity.py-2] [INFO] [1610565015.851695000] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [cartpole]
[Err] [Plugin.hh:178] Failed to load plugin libgazebo_ros2_control.so: /home/ros/control_ws/install/lib/libgazebo_ros2_control.so: undefined symbol: _ZN18controller_manager17ControllerManager17switch_controllerERKSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS7_EESB_ibRKN6rclcpp8DurationE

Another error I had earlier (which I'm struggling to recreate now) returned an error in the plugin Load function.

My assumption is that the historic versions of ros2_control and ros2_controllers specified in Moveit's version control are the issue. As I see you've been testing with Moveit @ahcorde, I'd be interested to know how you've been getting them to play nicely!

Colcon build error

Hey Guys,

I'm trying to build this package in ROS-Foxy on ubuntu 20.04 but the build is failing as follows:

`--- stderr: gazebo_ros2_control
CMake Error at /usr/share/cmake-3.16/Modules/CMakeFindDependencyMacro.cmake:47 (find_package):
Could not find a configuration file for package "ignition-common3-graphics"
that exactly matches requested version "3.10.0".

The following configuration files were considered but not accepted:

/usr/lib/x86_64-linux-gnu/cmake/ignition-common3-graphics/ignition-common3-graphics-config.cmake, version: 3.6.0
/lib/x86_64-linux-gnu/cmake/ignition-common3-graphics/ignition-common3-graphics-config.cmake, version: 3.6.0

Call Stack (most recent call first):
/usr/lib/x86_64-linux-gnu/cmake/ignition-common3/ignition-common3-config.cmake:203 (find_dependency)
/usr/lib/x86_64-linux-gnu/cmake/gazebo/gazebo-config.cmake:209 (find_package)
/opt/ros/foxy/share/gazebo_dev/cmake/gazebo_dev-extras.cmake:2 (find_package)
/opt/ros/foxy/share/gazebo_dev/cmake/gazebo_devConfig.cmake:41 (include)
CMakeLists.txt:7 (find_package)

---

Failed <<< gazebo_ros2_control [7.52s, exited with code 1]
`
Any help here will be great !

Joints unable to maintain static position under PositionJointInterface

@ahcorde

Opening an issue thread as advised in this #44 (comment)

According to @Briancbn's comment, the change made in the last commit around joint_limit_interface caused a problem that you can see in the videos from this comment which I also see from my simulated robot on Gazebo.

Any advice?
Jaehyun

Wrong config value for velocity and effort yaml

Dear @ahcorde thanks for the great work !
I believed that https://github.com/ros-simulation/gazebo_ros2_control/blob/8bdfcf12c690a75ffc2a76ec931d8daae94c3157/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml#L15 should be
effort instead of position
The same goes for the velocity example.

Migrate PID controllers

PID controllers are not migrated yet

Memory corruption/leaks in episodal training simulation

This is an elusive bug and I could use some advice tracking it down. I wrote a Hyperopt py script that optimizes my Gazebo friction/surface/joint/etc parameters. It uses spawn_entity/delete_entity service calls repeatedly to run a simulation of my robot collecting IMU and odometry to compute an optimization score.

It worked great! ...but along the way I've discovered there are a few hiccups to the repeated cycling of a model entity. Something in the tear-down is causing memory corruption that eventually causes a SEGV (in mostly random places unfortunately)....or causes some kind of mutex lock to fail and block forever. I believe the tear-down is complicated by the fact there are a lot of shared-ptrs that may be causing some objects to stay alive longer than expected. I've been banging on the problem for a while and I made some progress, measured in how many training episodes I could do before getting a crash.

I am currently pondering:

• because I am calling in with delete_entity (a service call) that it may be holding onto node references in the service call, which cascade to references on the controllers/plugins which interfere with their tear-down? maybe.
• @ahcorde I don't totally have my head around the controller spawning and executer/spin-thread interaction so perhaps we can review that?
• I dont think this is much of an issue in non-episodal simulations when there is only a single tear-down...i.e. "who cares if it crashes on termination"

I apologize for the insanely long text. Feel free to skip First and Second issue....go to Unresolved.

I am on Ubuntu 20.04. I have tested this on foxy (apt binaries) and latest master branch of ros2 as well.

First Issue | 1-5 episodes
GazeboRosControlPlugin was crashing sometimes due to gazebo events coming in during tear down. I think we should be at least disconnecting from Gazebo first so we dont get any events coming in while we are destructing. I then explicitly call executor->cancel() and thread_executer_spin.join(). The spin thread was set to only exit if rclcpp::ok() was false so I added a flag there to exit the loop.

Second Issue | ~15 episodes
Nodes (rclcpp ones) have many sub-interfaces and many sub-interfaces depend on other subs especially base-node-interface. Thus these base interfaces are constructed in a certain order, but destruction is left up to the compiler. So I modified rclcpp lib and explicitly destructed sub-interfaces in reverse order. This is documented in rclcpp Issue 1469.

Unresolved Issue
I am still getting a crash around 40-60 episodes. This is enough that I've been able to tune my parameters well but I would like to take the opportunity to resolve this issue anyway. There is a little bug somewhere between gazebo plugin and rclcpp just calling out for a fix. :)

Destruction of of Parameter sub-interface after BaseInterface was destroyed
I get this in the log output sometimes during tear-down:
[gzserver-1] [ERROR] [1606372728.971798611] [rclcpp]: Error in destruction of rcl service handle imu_sensor/get_parameters: the Node Handle was destructed too early. You will leak memory
This repeats for all the parameter sub-interface services (get_parameter_types, set_parameters, set_parameters_atomically, describe_parameters, list_parameters) and it's not always the gazebo imu plugin, it can be the odometry (p3d) or one of the ros2_control instantiated controllers.
Tracked this down to the rclcpp Node destruction flow. When the destructor on the Parameter SubInterface is finally called the BaseInterface it requires to tear-down the parameter service interfaces has already been destroyed. The parameter interface holds only a weak-ptr to BaseInterface therefor it doesnt prevent it from being destroyed...it aborts safely but logs and service-interfaces are not properly torn down. I assume something else must be holding on to the Parameter interface but not the BaseInterface or Node itself...but what? ...and is it even a problem with this repo, idk?

Stopping controllers via Controller Manager
I figured I could lessen the tear-down contention by stopping all controllers in the GazeboRosControlPlugin destructor using this code. This code runs after I disconnect from Gazebo events, but the executor and spin thread is still running fine.

  std::vector<std::string> start_controllers, stop_controllers;
  auto controllers = impl_->controller_manager_->get_loaded_controllers();
  std::transform(controllers.begin(), controllers.end(),
     std::back_inserter(stop_controllers),
     [](const controller_manager::ControllerSpec& spec) { return spec.info.name; });
  impl_->controller_manager_->switch_controller(
      start_controllers, stop_controllers,
      2, true,
      rclcpp::Duration(10, 0));

For some reason this blocks forever in switch_controller() call in the "wait until switch is finished" loop, controller_manager.cpp:406. It stepped through and does correctly put my two ros2 controllers in the stop_controllers collection so not sure why it never completes. It doesnt respect the 10s timeout argument either. Unless I am doing something dumb this should probably be moved out into ros2_controls issue.

Unnecessary parameter in demo

As the title says, the below lines should be removed. (it does not cause any problem now since they are removed afterward by this line)

https://github.com/ros-simulation/gazebo_ros2_control/blob/master/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml#L15

https://github.com/ros-simulation/gazebo_ros2_control/blob/master/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml#L15

Error running gazebo_ros2_control_demo package: `error: argument Call` invalid choice: control

Expected Behavior

• The container runs and the ros2 logs show that the cart was spawned successfully into the Gazebo process.

Actual Behavior

• The ros2 logs show the loading of the publishers and controllers but then crashes with the following error message:

[INFO] [ros2-6]: process started with pid [127]
[ros2-6] usage: ros2 [-h] Call ros2 <command> -h for more detailed usage. ...
[ros2-6] ros2: error: argument Call ros2 <command> -h for more detailed usage.: invalid choice: 'control' (choose from 'action', 'bag', 'component', 'daemon', 'doctor', 'extension_points', 'extensions', 'interface', 'launch', 'lifecycle', 'multicast', 'node', 'param', 'pkg', 'run', 'security', 'service', 'topic', 'wtf')

Steps to Reproduce

• git clone https://github.com/ros-simulation/gazebo_ros2_control
• cd gazebo_ros2_control/Docker
• docker build -t gazebo_ros2_control_demos:test .
• docker run gazebo_ros2_control_demos:test

Observations

I believe this is due to the fact that there is no explicit dependency for ros2_control in the gazebo_ros2_control_demos/package.xml

The issue is fixed locally by adding this dependency and then rebuilding the container, as rosdep is then able to install ros-foxy-ros2-control

Difference bet. gazebo_ros_control and gazebo_ros2_control ?

Hi,

Can anyone clarify the difference between:

ros-simulation/gazebo_ros_pkgs/gazebo_ros_control which has a ROS2 Foxy branch
https://github.com/ros-simulation/gazebo_ros_pkgs/tree/foxy/gazebo_ros_control

and this repo ros-simulation/gazebo_ros2_control ?

Define the node to get the robotParam.

Parameters in ROS 2 are inside each node. By default the plugin check this parameter in a node called robot_state_publisher and the parameter is called robot_description

State::interface not created.

Dear @ahcorde,

I believe that
https://github.com/ros-simulation/gazebo_ros2_control/blob/79207cc7d7630a8aeced1b9f5bba985e0805e538/gazebo_ros2_control/src/gazebo_system.cpp#L171

should be

this->dataPtr->joint_pos_state_[j] = std::make_shared<hardware_interface::StateInterface>( joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_[j]);

The same goes for velocity and effort.

Do you agree or did I misunderstood ?

Add ros2_controllers as exec_dependency in next Foxy synchronization

Reference to this comment #44 (comment)

Add effort_controller, velocity_controller and joint trajectory controller as exec_dependency

Segfault: sim_joint_ out of range

Bug Report

I have a multiple position joint interface setup, mainly porting over what ur_description(melodic-staging branch) have in ROS 1. I have encounter the following error. Here is my gdb log. This is using latest on #34

[INFO] [launch]: All log files can be found below /home/cypressiea/.ros/log/2020-10-20-09-52-41-898669-cypressiea-v-306273
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [gzserver-1]: process started with pid [306275]
[gzserver-1] GNU gdb (Ubuntu 9.2-0ubuntu1~20.04) 9.2
[gzserver-1] Copyright (C) 2020 Free Software Foundation, Inc.
[gzserver-1] License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
[gzserver-1] This is free software: you are free to change and redistribute it.
[gzserver-1] There is NO WARRANTY, to the extent permitted by law.
[gzserver-1] Type "show copying" and "show warranty" for details.
[gzserver-1] This GDB was configured as "x86_64-linux-gnu".
[gzserver-1] Type "show configuration" for configuration details.
[gzserver-1] For bug reporting instructions, please see:
[gzserver-1] <http://www.gnu.org/software/gdb/bugs/>.
[gzserver-1] Find the GDB manual and other documentation resources online at:
[gzserver-1]     <http://www.gnu.org/software/gdb/documentation/>.
[gzserver-1] 
[gzserver-1] For help, type "help".
[gzserver-1] Type "apropos word" to search for commands related to "word"...
[gzserver-1] Reading symbols from gzserver...
[gzserver-1] (No debugging symbols found in gzserver)
[gzserver-1] Starting program: /usr/bin/gzserver --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so
[gzserver-1] [Thread debugging using libthread_db enabled]
[gzserver-1] Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[gzserver-1] [New Thread 0x7fffde9a3700 (LWP 306283)]
[gzserver-1] Gazebo multi-robot simulator, version 11.2.0
[gzserver-1] Copyright (C) 2012 Open Source Robotics Foundation.
[gzserver-1] Released under the Apache 2 License.
[gzserver-1] http://gazebosim.org
[gzserver-1] 
[gzserver-1] [New Thread 0x7fffddb75700 (LWP 306284)]
...
...
[gzserver-1] [New Thread 0x7ffe9ffff700 (LWP 306379)]
[gzserver-1] [INFO] [1603158768.493129392] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-1] [INFO] [1603158768.494957603] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-1] [INFO] [1603158768.495054374] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-1] [ERROR] [1603158768.496429109] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-1] [ERROR] [1603158768.496445350] [gazebo_ros2_control]: param_name robot_description
[gzserver-1] [ERROR] [1603158768.497058957] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-1] Parsing robot urdf xml string.
[gzserver-1] [INFO] [1603158768.605069728] [gazebo_ros2_control]: Loading joint 'shoulder_pan_joint' of type 'hardware_interface/PositionJointInterface'
[gzserver-1] 
[gzserver-1] Thread 93 "gzserver" received signal SIGSEGV, Segmentation fault.
[gzserver-1] [Switching to Thread 0x7ffef4ff5700 (LWP 306378)]
[gzserver-1] 0x00007ffff6b26c2e in gazebo::physics::Joint::Position(unsigned int) const ()
[gzserver-1]    from /lib/x86_64-linux-gnu/libgazebo_physics.so.11

Proposed Solution

I managed to resolve the error by changing the following part
https://github.com/ros-simulation/gazebo_ros2_control/blob/a6030db928b6fcecff636769ebe9d9eca1f1fee5/gazebo_ros2_control/src/default_robot_hw_sim.cpp#L302-L304
to

 for (unsigned int j = 0; j < sim_joints_.size(); j++) { 
   // Gazebo has an interesting API... 
   double position = sim_joints_[j]->Position(0); 

It seems that unlike other class variables, sim_joints_ is not resized at the start and is created through push_pack. I am not sure this is the best solution.

PID - cart_example_effort

Hi,

I am testing cart_example_effort:

There are no PID parameters :

cartpole_controller_effort.yaml

controller_manager:
ros__parameters:
update_rate: 100 # Hz

effort_controllers:
  type: effort_controllers/JointGroupEffortController

joint_state_broadcaster:
  type: joint_state_broadcaster/JointStateBroadcaster

effort_controllers:
ros__parameters:
joints:
- slider_to_cart
command_interfaces:
- effort
state_interfaces:
- position
- velocity
- effort

---

However in ROS1 has PID parameters (https://github.com/ros-simulation/gazebo_ros_demos):

rrbot_control.yaml

rrbot:

joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

joint1_position_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 0.01, d: 10.0}
joint2_position_controller:
type: effort_controllers/JointPositionController
joint: joint2
pid: {p: 100.0, i: 0.01, d: 10.0}

---

Could indicate the equivalent PID parameters in gazebo_ros2_control.

Thank you in advance.

[Meta-ticket] Port gazebo_ros2_control to ROS 2

Not all functionality from ROS 1 has been ported to ROS 2 yet.

For ROS 1 see http://gazebosim.org/tutorials?tut=ros_control

This package provides a plugin which uses the ros2_control API to control joints inside Gazebo simulations.

The gazebo_ros_control tag also has the following optional child elements:

• robotNamespace: The ROS namespace to be used for this instance of the plugin, defaults to robot name in URDF/SDF
• controlPeriod: The period of the controller update (in seconds), defaults to Gazebo's period
• robotParam: The location of the robot_description (URDF) on the parameter server.
  • Parameters in ROS 2 are inside each node. By default the plugin check this parameter in a node called robot_state_publisher and the parameter is called robot_description
• robotSimType: The pluginlib name of a custom robot sim interface to be used (see below for more details), defaults to 'DefaultRobotHWSim'

TODO

• Extend parameters:
  • Define the node to get the robotParam. #4
• Add demos for all ported functionality.

Already added

• ROS 2's style guidelines and linters.
• Add demos for all ported functionality.

Not implemented yet

• PID #5

Status

Package		Status
gazebo_ros_control
	default_robot_hw_sim	🧑‍🏭 partially ported
	gazebo_ros_control_plugin	🧑‍🏭 partially ported
	robot_hw_sim	🧑‍🏭 partially ported

Custom parameter parsing not working

The custom logic for parsing parameters in the gazebo_plugin does not work as expect. When launching the system with the following parameters:

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_controller:
      type: joint_state_controller/JointStateController

joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2

I expect to see the parameters set at the controller manager, however ros2 param list yields nothing:

 ➭ ros2 param list
...
/controller_manager:
  use_sim_time

Reproduced current status of gazebo_ros2_control

I have created two PRs where I have added:

• gazebo_ros2_control - preliminary version #1
• gazebo_ros2_control_demos - preliminary demo package with a cart in a 30 meter rail. #2

The ROS 2 distro used for this tutorial is Foxy

Run the demos:

mkdir -p ~/ros2_control_ws/src 
cd ~/ros2_control_ws/src
git clone https://github.com/ros-simulation/gazebo_ros2_control/ -b ahcorde/test
git clone https://github.com/ros/xacro -b dashing-devel
git clone https://github.com/ros-simulation/gazebo_ros_pkgs -b ros2
git clone https://github.com/ros-controls/realtime_tools -b ros2_devel
git clone https://github.com/ros-controls/ros2_control
git clone https://github.com/ros-controls/ros2_controllers
git clone https://github.com/ddengster/ros2_control/ -b coffeebot_deps ros2_control_ddengster
touch ros2_control_ddengster/COLCON_IGNORE
cp -r ros2_control_ddengster/joint_limits_interface ros2_control_ddengster/transmission_interface  ros2_control
cd ~/ros2_control_ws
colcon build --merge-install

Running the examples

I created two examples: hardware_interface/PositionJointInterface and hardware_interface/VelocityJointInterface. I only made it work the PositionJointInterface example

Launch the Gazebo classic world with the gazebo_ros2_plugin

ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py

Call the action follow_joint_trajectories:

ros2 run gazebo_ros2_control_demos example_position

Check /joint_states or /state

ros2 topic echo /joint_states
ros2 topic echo /state

Dependencies

• xacro
• gazebo_ros_pkgs
• ros2_control
• ros2_controllers
• realtime_tools

@chapulina @Karsten1987 @ddengster

Segmentation fault if mimicked joint is not found

Just tried (by accident) what happens if a referenced joint is not found: the plugin generates a segmentation fault

[gzserver-1] Segmentation fault (core dumped)

without any further hints in the console nor with the gazebo log files

(1646065923 267684426) [Err] [Model.cc:1157] Exception occured in the Load function of plugin with name[gazebo_ros2_control] and filename[libgazebo_ros2_control.so]. This plugin will not run.
(1646065923 267702460) Exception occured in the Load function of plugin with name[gazebo_ros2_control] and filename[libgazebo_ros2_control.so]. This plugin will not run.

@destogl Where should this error message appear?

https://github.com/ros-simulation/gazebo_ros2_control/blob/698d01381423cea3232a78ab0ada7eb12cd6d40b/gazebo_ros2_control/src/gazebo_system.cpp#L180-L183

Symbol lookup error using demos

Hi,
I get the following error when running:
ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py

Error:

[gzserver-1] [INFO] [1620655169.697744698] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-1] gzserver: symbol lookup error: /opt/ros/foxy/lib/libgazebo_ros2_control.so: undefined symbol: _ZN18hardware_interface33parse_control_resources_from_urdfERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE
[ERROR] [gzserver-1]: process has died [pid 185163, exit code 127, cmd 'gzserver                                                                       -s libgazebo_ros_init.so   -s libgazebo_ros_factory.so   -s libgazebo_ros_force_system.so       '].

I tried to upgrade all related packages, but without success.

ros-foxy-hardware-interface is already the newest version (0.4.0-1focal.20210423.013029).
ros-foxy-controller-manager is already the newest version (0.4.0-1focal.20210423.020350).
ros-foxy-ros2-control is already the newest version (0.4.0-1focal.20210423.021142).
ros-foxy-ros2-controllers is already the newest version (0.2.0-1focal.20210423.021553).
ros-foxy-gazebo-ros2-control is already the newest version (0.0.2-1focal.20210423.021950).

Release for Galactic updates

Hey!
Any updates on the release for Galactic? It seems that ros2_control is coming soon as mentioned here

Thanks!

Remove legacyModeNS?

I saw that the legacyModeNS option has been migrated from ROS 1 to this package:

https://github.com/ros-simulation/gazebo_ros2_control/blob/a1318f8b1da9285ea3f647f575c1b37c1ef2975d/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L179-L194

Given that option's name, I'd expect it to be the kind of thing that doesn't need to be perpetuated into ROS 2. Is that still necessary?

Consider switching to snake_case inside XML

As described on #12 , the SDF parameters being passed to this plugin currently use camelCase, i.e.:

    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robotSimType>gazebo_ros2_control/DefaultRobotHWSim</robotSimType>
      <robotParamNode>robot_state_publisher</robotParamNode>
      <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
    </plugin>

Because both SDF and URDF use snake_case in their specs, when porting gazebo_ros_pkgs to ROS 2, we also changed its XML elements to use that style, see this example.

I recommend using the same here, but feel free to close this issue if you think it's not worth it.

problems when trying to build gazebo_ros2_control

Hello,

I am trying to follow the directions from the "docker file" in the following link:

https://github.com/ros-simulation/gazebo_ros2_control/pull/34/files

When I use the following line to install ros dependencies:

rosdep install --from-paths ./ -i -y --rosdistro foxy --ignore-src

I get the following:

ERROR: the following packages/stacks could not have their rosdep keys
resolved
to system dependencies:
gazebo_ros2_control: Cannot locate rosdep definition for
[joint_limits_interface]
gazebo_ros2_control_demos: Cannot locate rosdep definition for
[velocity_controllers]
ros2_control: Cannot locate rosdep definition for
[controller_parameter_server]

So, I can't proceed correctly. If I do proceed with the build, I get

Starting >>> hardware_interface
Starting >>> controller_manager_msgs
Starting >>> control_toolbox
Starting >>> transmission_interface
Finished <<< transmission_interface [0.23s]
Finished <<< hardware_interface [0.31s]
Starting >>> controller_interface
Starting >>> test_robot_hardware
Finished <<< control_toolbox [0.31s]
Finished <<< controller_interface [0.21s]
Finished <<< test_robot_hardware [0.23s]
Finished <<< controller_manager_msgs [0.72s]
Starting >>> controller_manager
Finished <<< controller_manager [0.35s]
Starting >>> gazebo_ros2_control
Starting >>> joint_state_controller
Starting >>> joint_trajectory_controller
Finished <<< joint_state_controller [0.27s]
Finished <<< joint_trajectory_controller [0.31s]
--- stderr: gazebo_ros2_control
CMake Error at CMakeLists.txt:11 (find_package):
By not providing "Findjoint_limits_interface.cmake" in CMAKE_MODULE_PATH
this project has asked CMake to find a package configuration file
provided
by "joint_limits_interface", but CMake did not find one.

Could not find a package configuration file provided by
"joint_limits_interface" with any of the following names:

 joint_limits_interfaceConfig.cmake
 joint_limits_interface-config.cmake

Add the installation prefix of "joint_limits_interface" to
CMAKE_PREFIX_PATH or set "joint_limits_interface_DIR" to a directory
containing one of the above files. If "joint_limits_interface"
provides a
separate development package or SDK, be sure it has been installed.

---

Failed <<< gazebo_ros2_control [1.99s, exited with code 1]

Summary: 9 packages finished [3.20s]
1 package failed: gazebo_ros2_control
1 package had stderr output: gazebo_ros2_control
1 package not processed

Any help is appreciated.

Thanks so much,

Farbod

About Gazebo crashing due to segmentation fault in libdefault_robot_hw_sim.so

Hello.
Do you have a solution to the problem I had with ROS2 Foxy?
I also use a DiffDriveController.

Computer specs

• ThinkPad T14
• CPU: Ryzen5 4650U
• RAM: 16GB
• OS: Linux Mint
• kernel: 5.7.7-050707-generic

ros2 log

[rviz2-1] qt5ct: using qt5ct plugin
[gazebo-2] Gazebo multi-robot simulator, version 11.0.0
[gazebo-2] Copyright (C) 2012 Open Source Robotics Foundation.
[gazebo-2] Released under the Apache 2 License.
[gazebo-2] http://gazebosim.org
[gazebo-2] 
[gazebo-2] Gazebo multi-robot simulator, version 11.0.0
[gazebo-2] Copyright (C) 2012 Open Source Robotics Foundation.
[gazebo-2] Released under the Apache 2 License.
[gazebo-2] http://gazebosim.org
[gazebo-2] 
[spawn_entity.py-3] [INFO] [1598339504.954850274] [urdf_spawner]: Spawn Entity started
[spawn_entity.py-3] [INFO] [1598339504.955676288] [urdf_spawner]: Loading entity XML from file /home/nyarn/Workspace/takao_x/install/takao_x_description/share/takao_x_description/urdf/takao_x.urdf
[spawn_entity.py-3] [INFO] [1598339504.960860964] [urdf_spawner]: Waiting for service /spawn_entity, timeout = 5
[spawn_entity.py-3] [INFO] [1598339504.961485346] [urdf_spawner]: Waiting for service /spawn_entity
[rviz2-1] [INFO] [1598339504.975179010] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-1] [INFO] [1598339504.975658681] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-1] qt5ct: D-Bus global menu: no
[rviz2-1] [INFO] [1598339505.014769026] [rviz2]: Stereo is NOT SUPPORTED
[gazebo-2] [INFO] [1598339505.480245536] [gazebo_ros_node]: ROS was initialized without arguments.
[spawn_entity.py-3] [INFO] [1598339505.720582710] [urdf_spawner]: Calling service /spawn_entity
[spawn_entity.py-3] [INFO] [1598339506.710534881] [urdf_spawner]: Spawn status: SpawnEntity: Successfully spawned entity [base_link]
[INFO] [spawn_entity.py-3]: process has finished cleanly [pid 34955]
[gazebo-2] [INFO] [1598339506.838296451] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gazebo-2] [INFO] [1598339506.848054761] [takao_x.gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /takao_x
[gazebo-2] [INFO] [1598339506.848379663] [takao_x.gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gazebo-2] [ERROR] [1598339506.858212214] [takao_x.gazebo_ros2_control]: connected to service!! /takao_x/robot_state_publisher
[gazebo-2] [ERROR] [1598339506.858283103] [takao_x.gazebo_ros2_control]: param_name robot_description
[gazebo-2] [ERROR] [1598339506.860119723] [takao_x.gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gazebo-2] Parsing robot urdf xml string.
[gazebo-2] [Msg] Waiting for master.
[gazebo-2] [Msg] Connected to gazebo master @ http://127.0.0.1:11345
[gazebo-2] [Msg] Publicized address: 172.18.34.232
[gazebo-2] Loading joint 'right_wheel_joint' of type 'hardware_interface/VelocityJointInterface'
[gazebo-2] [INFO] [1598339506.890981589] [takao_x.gazebo_ros2_control]: joint right_wheel_joint is configured in VELOCITY mode
[gazebo-2] [Msg] Waiting for master.
[gazebo-2] [Msg] Connected to gazebo master @ http://127.0.0.1:11345
[gazebo-2] [Msg] Publicized address: 172.18.34.232
[gazebo-2] [Dbg] [GuiIface.cc:111] using qt5ct plugin
[gazebo-2] [Dbg] [GuiIface.cc:111] custom style sheet is disabled
[gazebo-2] [Dbg] [GuiIface.cc:111] D-Bus global menu: no
[ERROR] [gazebo-2]: process has died [pid 34953, exit code 255, cmd 'gazebo --verbose -s libgazebo_ros_factory.so'].
[gazebo-2] 
[gazebo-2] 

syslog

Aug 25 16:11:46 nyarn kernel: [16718.163267] gzserver[35137]: segfault at 0 ip 00007f53880a108a sp 00007f5335fef2f0 error 4 in libdefault_robot_hw_sim.so[7f538808e000+35000]
Aug 25 16:11:46 nyarn kernel: [16718.163278] Code: e8 2b 5e ff ff 84 c0 74 07 c6 85 7f ff ff ff 01 48 8d 45 80 48 89 c7 e8 64 4f ff ff 48 05 c8 00 00 00 48 89 c7 e8 a6 8a ff ff <f2> 0f 10 00 f2 0f 11 45 b0 48 8d 45 80 48 89 c7 e8 41 4f ff ff 48
Aug 25 16:11:46 nyarn systemd[1]: Started Process Core Dump (PID 35222/UID 0).
Aug 25 16:11:49 nyarn systemd-coredump[35223]: Process 34989 (gzserver) of user 1000 dumped core.#012#012Stack trace of thread 35137:#012#0  0x00007f53880a108a n/a (/home/nyarn/Workspace/ros2_ws/build/gazebo_ros2_control/libdefault_robot_hw_sim.so + 0x6808a)#012#1  0x00007f538809ded9 n/a (/home/nyarn/Workspace/ros2_ws/build/gazebo_ros2_control/libdefault_robot_hw_sim.so + 0x64ed9)#012#2  0x00007f53883256e5 n/a (/home/nyarn/Workspace/ros2_ws/build/gazebo_ros2_control/libgazebo_ros2_control.so + 0x2456e5)#012#3  0x00007f545d36eda7 _ZN6gazebo7physics5Model10LoadPluginESt10shared_ptrIN3sdf2v97ElementEE (libgazebo_physics.so.11 + 0x269da7)#012#4  0x00007f545d370e10 _ZN6gazebo7physics5Model11LoadPluginsEv (libgazebo_physics.so.11 + 0x26be10)#012#5  0x00007f545d3c085a _ZN6gazebo7physics5World18ProcessFactoryMsgsEv (libgazebo_physics.so.11 + 0x2bb85a)#012#6  0x00007f545d3cb078 _ZN6gazebo7physics5World15ProcessMessagesEv (libgazebo_physics.so.11 + 0x2c6078)#012#7  0x00007f545d3cb5ee _ZN6gazebo7physics5World4StepEv (libgazebo_physics.so.11 + 0x2c65ee)#012#8  0x00007f545d3ce9b5 _ZN6gazebo7physics5World7RunLoopEv (libgazebo_physics.so.11 + 0x2c99b5)#012#9  0x00007f545db6ccb4 n/a (libstdc++.so.6 + 0xd6cb4)#012#10 0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#11 0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35063:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35068:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35023:#012#0  0x00007f545d969361 __GI___clock_nanosleep (libc.so.6 + 0xe0361)#012#1  0x00007f545df344fc _ZN6gazebo6common4Time5SleepERKS1_ (libgazebo_common.so.11 + 0xf64fc)#012#2  0x00007f545df3468c _ZN6gazebo6common4Time6MSleepEj (libgazebo_common.so.11 + 0xf668c)#012#3  0x00007f545e609185 _ZN6gazebo6Master3RunEv (libgazebo.so.11 + 0x48185)#012#4  0x00007f545db6ccb4 n/a (libstdc++.so.6 + 0xd6cb4)#012#5  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#6  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35022:#012#0  0x00007f545d9ab43e epoll_wait (libc.so.6 + 0x12243e)#012#1  0x00007f545d0a221b n/a (libgazebo_transport.so.11 + 0x4b21b)#012#2  0x00007f545d0a4611 n/a (libgazebo_transport.so.11 + 0x4d611)#012#3  0x00007f545d0bd4e2 n/a (libgazebo_transport.so.11 + 0x664e2)#012#4  0x00007f545c31443b n/a (libboost_thread.so.1.71.0 + 0x1143b)#012#5  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#6  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35059:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35049:#012#0  0x00007f545d4d07b1 futex_abstimed_wait_cancelable (libpthread.so.0 + 0x107b1)#012#1  0x00007f545d0b557e _ZN6gazebo9transport17ConnectionManager3RunEv (libgazebo_transport.so.11 + 0x5e57e)#012#2  0x00007f545c31443b n/a (libboost_thread.so.1.71.0 + 0x1143b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35002:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f545deff7db n/a (libgazebo_common.so.11 + 0xc17db)#012#2  0x00007f545def5f52 _ZN6gazebo6common13ModelDatabase16UpdateModelCacheEb (libgazebo_common.so.11 + 0xb7f52)#012#3  0x00007f545c31443b n/a (libboost_thread.so.1.71.0 + 0x1143b)#012#4  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#5  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35057:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35099:#012#0  0x00007f545d9ab43e epoll_wait (libc.so.6 + 0x12243e)#012#1  0x00007f5456433899 n/a (libzmq.so.5 + 0x2b899)#012#2  0x00007f5456475e5f n/a (libzmq.so.5 + 0x6de5f)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35066:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35064:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35080:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f545e4786eb n/a (libOgreMain.so.1.9.0 + 0x44b6eb)#012#2  0x00007f545e477a57 _ZN4Ogre16DefaultWorkQueue18waitForNextRequestEv (libOgreMain.so.1.9.0 + 0x44aa57)#012#3  0x00007f545e476a50 _ZN4Ogre16DefaultWorkQueue11_threadMainEv (libOgreMain.so.1.9.0 + 0x449a50)#012#4  0x00007f545c31443b n/a (libboost_thread.so.1.71.0 + 0x1143b)#012#5  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#6  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 34989:#012#0  0x00007f545d969361 __GI___clock_nanosleep (libc.so.6 + 0xe0361)#012#1  0x00007f545df344fc _ZN6gazebo6common4Time5SleepERKS1_ (libgazebo_common.so.11 + 0xf64fc)#012#2  0x00007f545df3468c _ZN6gazebo6common4Time6MSleepEj (libgazebo_common.so.11 + 0xf668c)#012#3  0x00007f545e5eca7c _ZN6gazebo6Server3RunEv (libgazebo.so.11 + 0x2ba7c)#012#4  0x000055f9ec6f5e25 n/a (gzserver-11.0.0 + 0x5e25)#012#5  0x00007f545d8b00b3 __libc_start_main (libc.so.6 + 0x270b3)#012#6  0x000055f9ec6f632e n/a (gzserver-11.0.0 + 0x632e)#012#012Stack trace of thread 35070:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35086:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f545e4786eb n/a (libOgreMain.so.1.9.0 + 0x44b6eb)#012#2  0x00007f545e477a57 _ZN4Ogre16DefaultWorkQueue18waitForNextRequestEv (libOgreMain.so.1.9.0 + 0x44aa57)#012#3  0x00007f545e476a50 _ZN4Ogre16DefaultWorkQueue11_threadMainEv (libOgreMain.so.1.9.0 + 0x449a50)#012#4  0x00007f545c31443b n/a (libboost_thread.so.1.71.0 + 0x1143b)#012#5  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#6  0x00007f545d9ab103 __clone (libc.so.6 + 0x122103)#012#012Stack trace of thread 35060:#012#0  0x00007f545d4d0376 futex_wait_cancelable (libpthread.so.0 + 0x10376)#012#1  0x00007f543edfe62b n/a (radeonsi_dri.so + 0x47c62b)#012#2  0x00007f543edfe23b n/a (radeonsi_dri.so + 0x47c23b)#012#3  0x00007f545d4c9609 start_thread (libpthread.so.0 + 0x9609)#012#4 
Aug 25 16:11:49 nyarn systemd[1]: [email protected]: Succeeded.

Consider using gazebo_ros::Node instead of rclcpp:Node

I just realized rclcpp::Node is being used:

https://github.com/ros-simulation/gazebo_ros2_control/blob/98834ef700c1c6ffd3012d2de0a81a09ab2d2235/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp#L64-L65

But if you used gazero_ros::Node, you'd get the advantages listed here and here, so you don't need to worry about executors or parsing common SDF params.

Regarding how to start up ROS controllers for the Gazebo simulatior in ROS2

Hi

When migrating to ROS2, there was a change in the way ROS controllers run in Gazebo. I would like to ask if there are specific reasons why ROS controllers are embedded in URDF files.
Current way: https://github.com/ros-simulation/gazebo_ros2_control/blob/master/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf#L67
Original way: https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_e_gazebo/launch/controller_utils.launch#L14

1. Hardware interfaces are developed and distributed by robot platform providers, but the controller to run on them is often determined by users. However, in the current form, not the users, but I have to decide which controller to use when releasing the hardware interface. Also, if I use someone else's robot description, the controller is already fixed in the released package, so I have to download the source package, modify it and compile it together every time.
2. Not in parallel to physical robots (https://github.com/ros-controls/ros2_control_demos/blob/master/ros2_control_demo_robot/launch/rrbot_system_position_only.launch.py#L47).

Considering the above two points, I would like to ask if it is possible to, at least, provide the original way with the current way together. (running controllers in a launch file instead of determining them in URDF files)
Thank you,
Jaehyun

Cannot changes namespaces or remap topics in controllers created by plugin

I tried adding <remappings> and <namespace> arguments inside the <urdf>/<sdf>. It can change the namespace of the gazebo node created through Get(sdf), but not the controllers created. For instance, for JointStateController, the publishing topic can only be /joint_states not /<namespace>/joint_states.

Other methods to change namespaces also doesn't work since the controller interface (reference) doesn't accept a namespace argument when instantiating a lifecycle node for the controller. This won't be a problem if we have access to ros2 launch or ros2 run, but doesn't work with gazebo.

This could be an enhancement for anybody wants to have two instances of gazebo_ros2_control under different namespaces like me. But there is probably not much I can do with the current controller interface.

Setup CI

I think there aren't tests yet, but it would be good to run some basic CI to make sure this package can be compiled and linters are clean.

The CI can also work as documentation on how to setup an environment, because there's a lot being pulled from various places right now.

example_position not working

Hi
My setup: Ubuntu 20.04 Focal Fossa , Ros Galactic
I tried cloning the repo and ran

    ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py

then

    ros2 run gazebo_ros2_control_demos example_position

but the cart just stayed still, I tried echo:

    ros2 topic echo /joint_trajectory_controller/state

and the desire position changed, the actual did not ( as shown on gazebo ) and the error position did followed along the position, which means the system did aware the cart did not move. the terminal that I used fro bringing up gazebo showed "Goal reached, success!" which is kind of weird . I installed all the needed packages and dependencies.
P/s the issue just happened today, I ran this repo last week from today and it worked perfectly , any help will be much appreciated