Gazebo ros2 controller, cannot moving drone

Help
,
1

Hello Gazebo_community,

I am trying to make a drone with meshes and i want to controll it with Ros2-Controller (Keyboard)
just to make sure i am a beginner with it.
The problem i got is that i cannot controll my drone at all. i searched everywhere and tried everything i can but i could not find the problem.

Can someone help me to make it move at least in +Z Axis and -Z Axis so i can say i finally did something.

i am using Ubuntu Humble with Ros2 and Gazebo.
i hope i gave every needed details so someone can help me out.

rsp.launch.py:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.substitutions import Command, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
use_sim_time = LaunchConfiguration(‘use_sim_time’)

pkg_path = FindPackageShare('my_bot')
xacro_file = PathJoinSubstitution([pkg_path, 'description', 'robot.urdf.xacro'])

robot_description_config = Command(['xacro ', xacro_file])
params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time}

node_robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    output='screen',
    parameters=[params]
)

return LaunchDescription([
    DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use sim time if true'
    ),
    node_robot_state_publisher
])

launcher_sim.launch.py:

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

def generate_launch_description():
package_name = ‘my_bot’

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

gazebo = IncludeLaunchDescription(
    PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py'
    )]),
)

spawn_entity = Node(
    package='gazebo_ros', executable='spawn_entity.py',
    arguments=['-topic', 'robot_description', '-entity', 'my_bot'],
    output='screen'
)

joint_state_broadcaster_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["joint_state_broadcaster"],
    output="screen"
)

velocity_controller_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["velocity_controller"],
    output="screen"
)

use_sim_time = LaunchConfiguration('use_sim_time')

joy_params = os.path.join(get_package_share_directory('my_bot'), 'config', 'joystick.yaml')

joy_node = Node(
    package='joy',
    executable='joy_node',
    parameters=[joy_params, {'use_sim_time': use_sim_time}],
)

teleop_node = Node(
    package='teleop_twist_joy',
    executable='teleop_node',
    name='teleop_node',
    parameters=[joy_params, {'use_sim_time': use_sim_time}],
    remappings=[('/cmd_vel','/new_cmd_vel')]
)

teleop_twist_keyboard = Node(
    package='teleop_twist_keyboard',
    executable='teleop_twist_keyboard',
    name='teleop_twist_keyboard',
    output='screen',
    remappings=[('/cmd_vel', '/new_cmd_vel')]
)

return LaunchDescription([
    rsp,
    gazebo,
    spawn_entity,

    TimerAction(
        period=5.0,
        actions=[joint_state_broadcaster_spawner]
    ),
    TimerAction(
        period=10.0,
        actions=[velocity_controller_spawner]
    ),
    DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use sim time if true',
    ),
    joy_node,
    teleop_node,
    teleop_twist_keyboard,
])

drone.urdf.xacro:

robot_description robot_state_publisher /home/alba/dev_ws/src/my_bot/config/rotor_controller.yaml

<ros2_control name=“GazeboSystem” type=“system”>

gazebo_ros2_control/GazeboSystem

<joint name="base_to_b1">
    <command_interface name="velocity"/>
    <state_interface name="velocity"/>
</joint>
<joint name="base_to_b2">
    <command_interface name="velocity"/>
    <state_interface name="velocity"/>
</joint>
<joint name="base_to_b3">
    <command_interface name="velocity"/>
    <state_interface name="velocity"/>
</joint>
<joint name="base_to_b4">
    <command_interface name="velocity"/>
    <state_interface name="velocity"/>
</joint>

</ros2_control>

rotor_controller.yaml:

controller_manager:
ros__parameters:
update_rate: 50
use_sim_time: true
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
velocity_controller:
type: velocity_controllers/JointGroupVelocityController

velocity_controller:
ros__parameters:
joints:
- base_to_b1
- base_to_b2
- base_to_b3
- base_to_b4

joystick.yaml:

joy_node:
ros__parameters:
device_id: 0
deadzone: 0.05
autorepeat_rate: 20.0

teleop_node:
ros__parameters:
axis_linear:
z: 4
scale_linear:
z: 2.0
scale_linear_turbo:
z: 5.0

axis_linear:
  x: 0
scale_linear:
  x: 5.0
scale_linear_turbo:
  x: 10.0

axis_angular:
  yaw: 1
scale_angular:
  yaw: 5.0
scale_angular_turbo:
  yaw: 10.0

axis_linear:
  y: 3
scale_linear:
  y: 5.0
scale_linear_turbo:
  y: 10.0

enable_turbo_button: 1

require_enable_button: false

and just in case you need the log to make sure:

[INFO] [launch]: All log files can be found below /home/alba/.ros/log/2024-09-19-14-49-18-788212-alba-4163
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [4166]
[INFO] [gzserver-2]: process started with pid [4168]
[INFO] [gzclient-3]: process started with pid [4170]
[INFO] [spawn_entity.py-4]: process started with pid [4172]
[INFO] [joy_node-5]: process started with pid [4174]
[INFO] [teleop_node-6]: process started with pid [4176]
[teleop_node-6] [INFO] [1726750161.682192441] [TeleopTwistJoy]: Turbo on button 1.
[teleop_node-6] [INFO] [1726750161.682370041] [TeleopTwistJoy]: Linear axis x on 0 at scale 5.000000.
[teleop_node-6] [INFO] [1726750161.682422641] [TeleopTwistJoy]: Turbo for linear axis x is scale 10.000000.
[teleop_node-6] [INFO] [1726750161.682471641] [TeleopTwistJoy]: Linear axis y on 3 at scale 5.000000.
[teleop_node-6] [INFO] [1726750161.682518341] [TeleopTwistJoy]: Turbo for linear axis y is scale 10.000000.
[teleop_node-6] [INFO] [1726750161.682565141] [TeleopTwistJoy]: Linear axis z on 4 at scale 2.000000.
[teleop_node-6] [INFO] [1726750161.682612841] [TeleopTwistJoy]: Turbo for linear axis z is scale 5.000000.
[teleop_node-6] [INFO] [1726750161.682659641] [TeleopTwistJoy]: Angular axis yaw on 1 at scale 5.000000.
[teleop_node-6] [INFO] [1726750161.682956741] [TeleopTwistJoy]: Turbo for angular axis yaw is scale 10.000000.
[robot_state_publisher-1] [WARN] [1726750161.806138669] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-1] [INFO] [1726750161.806511869] [robot_state_publisher]: got segment b1
[robot_state_publisher-1] [INFO] [1726750161.806995869] [robot_state_publisher]: got segment b2
[robot_state_publisher-1] [INFO] [1726750161.807079069] [robot_state_publisher]: got segment b3
[robot_state_publisher-1] [INFO] [1726750161.807158569] [robot_state_publisher]: got segment b4
[robot_state_publisher-1] [INFO] [1726750161.807236669] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1726750161.807315069] [robot_state_publisher]: got segment cam
[robot_state_publisher-1] [INFO] [1726750161.807393269] [robot_state_publisher]: got segment land
[spawn_entity.py-4] [INFO] [1726750163.485488247] [spawn_entity]: Spawn Entity started
[spawn_entity.py-4] [INFO] [1726750163.490059849] [spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-4] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[spawn_entity.py-4]   warnings.warn(
[spawn_entity.py-4] [INFO] [1726750163.511889853] [spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-4] [INFO] [1726750163.548037562] [spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-4] [INFO] [1726750163.552380363] [spawn_entity]: Waiting for service /spawn_entity
[gzclient-3] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzclient-3] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzclient-3] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzclient-3] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzserver-2] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzserver-2] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzserver-2] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[gzserver-2] Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP address is 127.0.0.1.  This should work for local processes, but will almost certainly not work if you have remote processes.Report to the disc-zmq development team to seek a fix.
[spawn_entity.py-4] [INFO] [1726750165.862268983] [spawn_entity]: Calling service /spawn_entity
[INFO] [spawner-7]: process started with pid [4321]
[gzclient-3] 
[gzclient-3] libcurl: (6) Could not resolve host: fuel.ignitionrobotics.org
[spawn_entity.py-4] [INFO] [1726750166.805391196] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [my_bot]
[INFO] [spawn_entity.py-4]: process has finished cleanly [pid 4172]
[spawner-7] [INFO] [1726750167.796919320] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[gzserver-2] [INFO] [1726750167.913570246] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-2] [INFO] [1726750167.948584454] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-2] [INFO] [1726750167.949257654] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-2] [INFO] [1726750167.968664058] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-2] [INFO] [1726750167.976233760] [gazebo_ros2_control]: Received urdf from param server, parsing...
[gzserver-2] [INFO] [1726750167.976625460] [gazebo_ros2_control]: Loading parameter files /home/alba/dev_ws/src/my_bot/config/rotor_controller.yaml
[gzserver-2] [INFO] [1726750168.034078373] [gazebo_ros2_control]: Loading joint: base_to_b1
[gzserver-2] [INFO] [1726750168.034458373] [gazebo_ros2_control]: 	State:
[gzserver-2] [INFO] [1726750168.034875573] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.035126073] [gazebo_ros2_control]: 	Command:
[gzserver-2] [INFO] [1726750168.035369373] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.035621373] [gazebo_ros2_control]: Loading joint: base_to_b2
[gzserver-2] [INFO] [1726750168.036117173] [gazebo_ros2_control]: 	State:
[gzserver-2] [INFO] [1726750168.036366973] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.036611973] [gazebo_ros2_control]: 	Command:
[gzserver-2] [INFO] [1726750168.037036873] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.037300074] [gazebo_ros2_control]: Loading joint: base_to_b3
[gzserver-2] [INFO] [1726750168.037549074] [gazebo_ros2_control]: 	State:
[gzserver-2] [INFO] [1726750168.037991274] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.038238174] [gazebo_ros2_control]: 	Command:
[gzserver-2] [INFO] [1726750168.038482574] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.038944574] [gazebo_ros2_control]: Loading joint: base_to_b4
[gzserver-2] [INFO] [1726750168.039244774] [gazebo_ros2_control]: 	State:
[gzserver-2] [INFO] [1726750168.039493774] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.039933974] [gazebo_ros2_control]: 	Command:
[gzserver-2] [INFO] [1726750168.040179774] [gazebo_ros2_control]: 		 velocity
[gzserver-2] [INFO] [1726750168.040526874] [resource_manager]: Initialize hardware 'GazeboSystem' 
[gzserver-2] [INFO] [1726750168.041151274] [resource_manager]: Successful initialization of hardware 'GazeboSystem'
[gzserver-2] [INFO] [1726750168.042096974] [resource_manager]: 'configure' hardware 'GazeboSystem' 
[gzserver-2] [INFO] [1726750168.042257675] [resource_manager]: Successful 'configure' of hardware 'GazeboSystem'
[gzserver-2] [INFO] [1726750168.042413775] [resource_manager]: 'activate' hardware 'GazeboSystem' 
[gzserver-2] [INFO] [1726750168.042566575] [resource_manager]: Successful 'activate' of hardware 'GazeboSystem'
[gzserver-2] [INFO] [1726750168.043140975] [gazebo_ros2_control]: Loading controller_manager
[gzserver-2] [WARN] [1726750168.233546918] [gazebo_ros2_control]:  Desired controller update period (0.02 s) is slower than the gazebo simulation period (0.001 s).
[gzserver-2] [INFO] [1726750168.236298719] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-2] [INFO] [1726750168.314883936] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-7] [INFO] [1726750168.461951169] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[gzserver-2] [INFO] [1726750168.471040171] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[gzserver-2] [INFO] [1726750168.471481972] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-7] [INFO] [1726750168.587148598] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[INFO] [spawner-7]: process has finished cleanly [pid 4321]
[INFO] [spawner-8]: process started with pid [4358]
[gzclient-3] context mismatch in svga_surface_destroy
[gzclient-3] context mismatch in svga_surface_destroy
[gzserver-2] [INFO] [1726750173.484798102] [controller_manager]: Loading controller 'velocity_controller'
[spawner-8] [INFO] [1726750173.815989176] [spawner_velocity_controller]: Loaded velocity_controller
[gzserver-2] [INFO] [1726750173.820977778] [controller_manager]: Configuring controller 'velocity_controller'
[gzserver-2] [INFO] [1726750173.830970380] [velocity_controller]: configure successful
[gzserver-2] [INFO] [1726750173.869233289] [velocity_controller]: activate successful
[spawner-8] [INFO] [1726750173.902301196] [spawner_velocity_controller]: Configured and activated velocity_controller
[INFO] [spawner-8]: process has finished cleanly [pid 4358]

Command what i use to turn it on:

ros2 launch my_bot launch_sim.launch.py

remapping
`

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/new_cmd_vel

i thought i have to turn the velocity controller by myself so the rotors start to move:

ros2 topic pub /velocity_controller/commands std_msgs/msg/Float64MultiArray “data: [50.5, 50.5, 50.5, 50.5]”

2

Hello Alba, and welcome to the community!

As long as I see, you know how to use ros2 and simulators (nice launchfile, xacro file, everything ok).

It seems like you are not launching ros_gz_bridge. In case you don’t know, gazebo works with a communication layer similar to ros2, called transport. It has topics, messages, nodes and so on. To connect ros2 nodes with the simulation, you have to specify which ros2 topics connect with their respective transport topics, in which direction (ros2->simulation, ros2<-simulation or bi-directional).

If that is the case, check if you have installed ros_gz. Then, list all topics and messages that must be connected to the simulation (sensing, actuation and tf tree), use the table in the ros_gz_bridge github to identify the required transport messages, and write a yaml file to configure the ros_gz_bridge node following the example in the github page. The node would be launched by launcher_sim.launch.py file.

AFAIK there’s no rqt_graph for transport, but you have terminal tools for debugging, like ign topic -l (similar to ros2 topic list), ign topic -e (similar to ros2 topic echo) and ign topic -p (similar to ros2 topic pub). You can check if sensors are sending data and if actuators really move the drone.

Note: You didn’t specify your Gazebo version. I am supposing you are using Fortress, because it is recommended for Humble. If you are using a more recent version, all commands start by “gz” instead of “ign”.

Good luck with your project!

Kind regards,
Diego

3

Hello diegocubillo
First of all thank you for answering on my issue. it kinda gives me a hope to do it until the end.

I am using for my project: Ubunuto 22.04 humble Jammy, Gazebo classic, (it’s a part of my project for my essay i am not allowad to use another one)

i worked on it for about 4 months and i am still trying to make it better, thanks ^^
i read about ros_gz_bridge but i did not understand it at all.
i tried to use another project (a car) and i could control it with teleop_twist_keyboard without using ros_gz_bridge (from articulated robot)
so i still can not figure it out how does it really work.

the only thing i need to be able to do so i can get through my essay i have to be able to move the drone (that is my project) at least up and down. it does not matter if i use keyboard or i use terminal commands. i can move the joints like i said before but when i put (100) for all joints it moves over all the place and randomly.

If you have another idea for me i would be grateful to you and i am already grateful for answering and trying to help me out. i will try to understand gz_ros_btidge cuz i want to keep learning using gazebo and ros2. its interessting for me

Kind regards,
Alba

4

Hi Alba,

If you are using gazebo classic then forget about everything I said, from start to finish.

This means that nodes are connected to the correct topic. We can accept that you can “control” your drone with the keyboard.

The possible problems that come to my mind are:

  • Conflict with teleop libraries: You are launching teleop_twist_joy and teleop_twist_keyboard. One may be cancelling the other by sending 0. Launch just the one you will use in your test, just in case.
  • Wrong driver plugin: If you make propellers move but there’s no aerodynamics simulation, it won’t move up. It may rotate because of angular momentum and inertia.

I think this last point is your case. The gazebo_ros2_control system moves the propellers (confirm if they turn although the drone doesn’t take off) but with basic physics simulation they don’t generate lift.

In this drone case they are using AdvancedLiftDragPlugin to extend physics to use propellers. Maybe it is the way to go.

Best regards,
Diego

5

i meant there i could control the car from the e.g. articulated robot.

I removed the node of “joy” cuz i dont want to use any joystick anymore. the same problem was there. i think i might have a problem with the physics (Inertial, inertia) i am still trying to find it out.

I can move the drone with the command:

ros2 topic pub /velocity_controller/commands std_msgs/msg/Float64MultiArray “data: [50.5, 50.5, 50.5, 50.5]”

with 50.5 the propellers move without going high. but with 100 it moves randomly and all over the place (it leaves the whole world too )

I am going to try the link you posted. it might work some how.

and again i am so grateful for your helping out. i still have hope to do it.
if i dont get though it, it might chase me in my nightmares

Best regards,
Alba