Arch Linux with ROS Melodic and Gazebo 10 -- no movement

[KomaGR]

I am trying to make this work with this setup. To my knowledge I've installed all the dependencies and I've understood how message passing works in the hovering example.

Running the hovering example launches gazebo succesfully, messages look to be exchanged succesfully, but there is no movement from the drone after the waypoint is published. The drone simply remains motionless. Any pointers would be appreciated.

Here is the build.log from catkin build. There are also some warnings there about empy but I didn't think it'd be much of a problem.

Find below the log from the example, which looks OK minus a warning about a deprecated function.

➜  catkin_wc roslaunch rotors_gazebo crazyflie2_hovering_example.launch
... logging to /home/koma/.ros/log/6a3842ae-6ead-11ea-892d-10c37b95af0c/roslaunch-TheMothership-26486.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
/opt/ros/melodic/lib/python3.8/site-packages/roslaunch/loader.py:412: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
  data = yaml.load(text)
started roslaunch server http://TheMothership:42757/

SUMMARY
========

PARAMETERS
 * /crazyflie2/position_controller_node/attitude_gain_ki/phi: 0.0349
 * /crazyflie2/position_controller_node/attitude_gain_ki/theta: 0.0349
 * /crazyflie2/position_controller_node/attitude_gain_kp/phi: 0.0611
 * /crazyflie2/position_controller_node/attitude_gain_kp/theta: 0.0611
 * /crazyflie2/position_controller_node/csvFilesStoring: False
 * /crazyflie2/position_controller_node/csvFilesStoringTime: 15.0
 * /crazyflie2/position_controller_node/enable_state_estimator: False
 * /crazyflie2/position_controller_node/hovering_gain_kd/z: 373
 * /crazyflie2/position_controller_node/hovering_gain_ki/z: 3.15
 * /crazyflie2/position_controller_node/hovering_gain_kp/z: 70
 * /crazyflie2/position_controller_node/inertia/xx: 1.657171e-05
 * /crazyflie2/position_controller_node/inertia/xy: 0.0
 * /crazyflie2/position_controller_node/inertia/xz: 0.0
 * /crazyflie2/position_controller_node/inertia/yy: 1.657171e-05
 * /crazyflie2/position_controller_node/inertia/yz: 0.0
 * /crazyflie2/position_controller_node/inertia/zz: 2.9261652e-05
 * /crazyflie2/position_controller_node/mass: 0.027
 * /crazyflie2/position_controller_node/rate_gain_ki/p: 0
 * /crazyflie2/position_controller_node/rate_gain_ki/q: 0
 * /crazyflie2/position_controller_node/rate_gain_ki/r: 95.6839
 * /crazyflie2/position_controller_node/rate_gain_kp/p: 1000
 * /crazyflie2/position_controller_node/rate_gain_kp/q: 1000
 * /crazyflie2/position_controller_node/rate_gain_kp/r: 1000
 * /crazyflie2/position_controller_node/rotor_configuration/0/angle: -3.14159265359
 * /crazyflie2/position_controller_node/rotor_configuration/0/arm_length: 0.046
 * /crazyflie2/position_controller_node/rotor_configuration/0/direction: -1.0
 * /crazyflie2/position_controller_node/rotor_configuration/0/rotor_force_constant: 1.28192e-08
 * /crazyflie2/position_controller_node/rotor_configuration/0/rotor_moment_constant: 0.005964552
 * /crazyflie2/position_controller_node/rotor_configuration/1/angle: 1.57079632679
 * /crazyflie2/position_controller_node/rotor_configuration/1/arm_length: 0.046
 * /crazyflie2/position_controller_node/rotor_configuration/1/direction: 1.0
 * /crazyflie2/position_controller_node/rotor_configuration/1/rotor_force_constant: 1.28192e-08
 * /crazyflie2/position_controller_node/rotor_configuration/1/rotor_moment_constant: 0.005964552
 * /crazyflie2/position_controller_node/rotor_configuration/2/angle: 3.14159265359
 * /crazyflie2/position_controller_node/rotor_configuration/2/arm_length: 0.046
 * /crazyflie2/position_controller_node/rotor_configuration/2/direction: -1.0
 * /crazyflie2/position_controller_node/rotor_configuration/2/rotor_force_constant: 1.28192e-08
 * /crazyflie2/position_controller_node/rotor_configuration/2/rotor_moment_constant: 0.005964552
 * /crazyflie2/position_controller_node/rotor_configuration/3/angle: -1.57079632679
 * /crazyflie2/position_controller_node/rotor_configuration/3/arm_length: 0.046
 * /crazyflie2/position_controller_node/rotor_configuration/3/direction: 1.0
 * /crazyflie2/position_controller_node/rotor_configuration/3/rotor_force_constant: 1.28192e-08
 * /crazyflie2/position_controller_node/rotor_configuration/3/rotor_moment_constant: 0.005964552
 * /crazyflie2/position_controller_node/user_account: giuseppe
 * /crazyflie2/position_controller_node/xy_gain_ki/x: 5.72958
 * /crazyflie2/position_controller_node/xy_gain_ki/y: -5.72958
 * /crazyflie2/position_controller_node/xy_gain_kp/x: 3.594
 * /crazyflie2/position_controller_node/xy_gain_kp/y: -3.594
 * /crazyflie2/position_controller_node/yaw_gain_ki/yaw: 0
 * /crazyflie2/position_controller_node/yaw_gain_kp/yaw: 0.0914
 * /crazyflie2/robot_description: <?xml version="1....
 * /crazyflie2/tf_prefix: 
 * /gazebo/enable_ros_network: True
 * /rosdistro: melodic
 * /rosversion: 1.14.3
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
  /crazyflie2/
    hovering_example (rotors_gazebo/hovering_example)
    joint_state_publisher (joint_state_publisher/joint_state_publisher)
    position_controller_node (rotors_control/position_controller_node)
    quaternion_to_rpy (rotors_gazebo/quaternion_to_rpy)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_crazyflie2 (gazebo_ros/spawn_model)

auto-starting new master
process[master]: started with pid [26497]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 6a3842ae-6ead-11ea-892d-10c37b95af0c
process[rosout-1]: started with pid [26507]
started core service [/rosout]
process[gazebo-2]: started with pid [26514]
process[gazebo_gui-3]: started with pid [26516]
process[crazyflie2/spawn_crazyflie2-4]: started with pid [26521]
process[crazyflie2/position_controller_node-5]: started with pid [26525]
[ INFO] [1585150168.883067230]: Started position controller
process[crazyflie2/hovering_example-6]: started with pid [26527]
[ INFO] [1585150168.887735026]: Got param 'user_account': giuseppe
[ INFO] [1585150168.888530591]: Got param 'csvFilesStoring': 0
[ INFO] [1585150168.888835357]: Got param 'csvFilesStoringTime': 15.000000
[ INFO] [1585150168.893081567]: Got param 'enable_state_estimator': 0
process[crazyflie2/robot_state_publisher-7]: started with pid [26532]
process[crazyflie2/joint_state_publisher-8]: started with pid [26537]
[ INFO] [1585150168.908906982]: Started hovering example.
[ INFO] [1585150168.909726553]: Wait for 1 second before trying to unpause Gazebo again.
process[crazyflie2/quaternion_to_rpy-9]: started with pid [26539]
[ INFO] [1585150169.207817714]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1585150169.209420010]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1585150169.248267394]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1585150169.249062280]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[INFO] [1585150169.442521, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1585150169.447799, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1585150169.914299979]: Unpaused the Gazebo simulation.
[ INFO] [1585150169.914344841]: Wait for 15 seconds to let the Gazebo GUI show up.
[ INFO] [1585150169.939644353, 0.025000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1585150169.963495079, 0.049000000]: Physics dynamic reconfigure ready.
[INFO] [1585150170.051158, 0.125000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1585150170.209848, 0.238000]: Spawn status: SpawnModel: Successfully spawned entity
[crazyflie2/spawn_crazyflie2-4] process has finished cleanly
log file: /home/koma/.ros/log/6a3842ae-6ead-11ea-892d-10c37b95af0c/crazyflie2-spawn_crazyflie2-4*.log
[ INFO] [1585150187.638864568, 15.123000000]: Stop waiting.
[ INFO] [1585150187.639955674, 15.124000000]: Publishing waypoint on namespace /crazyflie2: [0.000000, 0.000000, 1.000000].
[ INFO] [1585150187.640127342, 15.124000000]: PositionController got first MultiDOFJointTrajectory message.
[crazyflie2/hovering_example-6] process has finished cleanly
log file: /home/koma/.ros/log/6a3842ae-6ead-11ea-892d-10c37b95af0c/crazyflie2-hovering_example-6*.log

^C[crazyflie2/quaternion_to_rpy-9] killing on exit
[crazyflie2/joint_state_publisher-8] killing on exit
[crazyflie2/robot_state_publisher-7] killing on exit
[crazyflie2/position_controller_node-5] killing on exit
[gazebo-2] killing on exit
[gazebo_gui-3] killing on exit
terminate called after throwing an instance of 'boost::wrapexcept<boost::lock_error>'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
[gazebo_gui-3] escalating to SIGTERM
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

[welcome]

Thanks for opening your first issue here! Make sure that no other issues on the same topic have already been opened!

[gsilano]

Hi @KomaGR! I've never tried to run the system with an OS distribution other than Ubuntu. However, if it builds this means that it is not a problem related to the code itself.

Reading your message, it looks like a problem that I just faced in #25. I suggest to remove and then to install again all gazebo9 packages. Thus, run these commands

$ sudo apt-get remove ros-melodic-gazebo* gazebo*
$ sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
$ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
$ sudo apt-get update
$ sudo apt-get install gazebo9 gazebo9-* ros-melodic-gazebo-*
$ sudo apt upgrade

I'm pretty sure this is the problem because if you see the log file, no message appears regarding the odometry message. I mean, "PositionController got first odometry message."

Try, and give me back.

[KomaGR]

Hi @gsilano, thanks for your swift reply. I've rebuilt all the packages but nothing changes. Could it be an incompatibility between gazebo9 and gazebo10?

Regardless, based on what you said about the messages appearing in the logs I started looking on the route the message must take to arrive to the line you mentioned. It can be verified by the logs that the message leaves the hovering_example on the COMMAND_TRAJECTORY topic and is received by MultiDofJointTrajectoryCallback() here. As you say, no odometry message. Yet.

However:
I do not understand who publishes on the mav_msgs::default_topics::ODOMETRY topic. I see multiple subscribers, position_controller_node is among them with the OdometryCallback(). But I can't seem to find who publishes at that topic.

Any ideas on where I should be looking next for this would be appreciated.

[gsilano]

It could be, it could be. I have never tested the package with Gazebo 10. When I read the issue, I realized that you were working with Melodic on Arch and that's it.
Why would you want to use Gazebo 10 instead of Gazebo 9? Is there a particular reason to use it?

The RotorS package used the basis for CrazyS is very complex and, unfortunately, there are no guides on how to use it correctly or how the software is structured. However, the rotors_gazebo_plugins folder contains all the plugins used for the motors, odometry sensor, etc. You could start from there.

If you find something interesting, please let me know. The problem may lie in the CMakeList.txt file because I fought hard to make the package compatible with Gazebo 9.

[KomaGR]

Hi @gsilano. I've been intermittently trying to solve this issue, in order to use the simulation for some experiments. I have studied the source code of the involved nodes. I have some questions that would greatly help if answered:

In what module does the robot read the commands?

I can see that the PositionControllerNode::MultiDofJointTrajectoryCallback is receiving a message (you can see it in the log above). I also noticed that in the Kinetic VM you provide, it is the same callback (and not the PositionControllerNode::OdometryCallback).
A trajectory is calculated from that message and then pushed in a command queue:

CrazyS/rotors_control/src/nodes/position_controller_node.cpp

Lines 88 to 94 in 59f227f

	mav_msgs::EigenTrajectoryPoint eigen_reference;
	mav_msgs::eigenTrajectoryPointFromMsg(msg->points.front(), &eigen_reference);
	commands_.push_front(eigen_reference);
	// We can trigger the first command immediately.
	position_controller_.SetTrajectoryPoint(eigen_reference);
	commands_.pop_front();

The position controller sets trajectory point, but this only raises a boolean flag:

CrazyS/rotors_control/src/library/position_controller.cpp

Lines 250 to 252 in 59f227f

	void PositionController::SetTrajectoryPoint(const mav_msgs::EigenTrajectoryPoint& command_trajectory) {
	command_trajectory_= command_trajectory;
	controller_active_= true;

1. Do you know where does the command go from here?
2. Is PositionController::CallbackHightLevelControl supposed to be running on a loop?

---

Why would you want to use Gazebo 10 instead of Gazebo 9? Is there a particular reason to use it?

For better or for worse, this is what my distro provides.

[gsilano]

I don't know if I really understand what you are trying to say, so I will try to be as clear as possible.

The position, i.e., the hovering coordinates (x, y, z, yaw) comes from the hovering_example.cpp file, in rotors_gazebo\src folder. The message is published on the mav_msgs::default_topics::COMMAND_TRAJECTORY topic (here you can find the full name of the topics) to which the PositionControllerNode is subscribed. Whenever a message is published on this topic (for the hovering example only once) the PositionControllerNode::MultiDofJointTrajectoryCallback method is called. This essentially moves the information from the node to the controller algorithm which, in order to facilitate the reuse of the code, has been implemented as a shared library.

The boolean flag in the controller algorithm enables the controller. I mean, the propellers' angular velocities are equals to zero until the boolean flag rises.

CrazyS/rotors_control/src/library/position_controller.cpp

Lines 255 to 309 in 59f227f

	void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities) {
	assert(rotor_velocities);
	// This is to disable the controller if we do not receive a trajectory
	if(!controller_active_){
	*rotor_velocities = Eigen::Vector4d::Zero(rotor_velocities->rows());
	return;
	}
	double PWM_1, PWM_2, PWM_3, PWM_4;
	ControlMixer(&PWM_1, &PWM_2, &PWM_3, &PWM_4);
	double omega_1, omega_2, omega_3, omega_4;
	omega_1 = ((PWM_1 * ANGULAR_MOTOR_COEFFICIENT) + MOTORS_INTERCEPT);
	omega_2 = ((PWM_2 * ANGULAR_MOTOR_COEFFICIENT) + MOTORS_INTERCEPT);
	omega_3 = ((PWM_3 * ANGULAR_MOTOR_COEFFICIENT) + MOTORS_INTERCEPT);
	omega_4 = ((PWM_4 * ANGULAR_MOTOR_COEFFICIENT) + MOTORS_INTERCEPT);
	//The omega values are saturated considering physical constraints of the system
	if(!(omega_1 < MAX_PROPELLERS_ANGULAR_VELOCITY && omega_1 > 0))
	if(omega_1 > MAX_PROPELLERS_ANGULAR_VELOCITY)
	omega_1 = MAX_PROPELLERS_ANGULAR_VELOCITY;
	else
	omega_1 = 0;
	if(!(omega_2 < MAX_PROPELLERS_ANGULAR_VELOCITY && omega_2 > 0))
	if(omega_2 > MAX_PROPELLERS_ANGULAR_VELOCITY)
	omega_2 = MAX_PROPELLERS_ANGULAR_VELOCITY;
	else
	omega_2 = 0;
	if(!(omega_3 < MAX_PROPELLERS_ANGULAR_VELOCITY && omega_3 > 0))
	if(omega_3 > MAX_PROPELLERS_ANGULAR_VELOCITY)
	omega_3 = MAX_PROPELLERS_ANGULAR_VELOCITY;
	else
	omega_3 = 0;
	if(!(omega_4 < MAX_PROPELLERS_ANGULAR_VELOCITY && omega_4 > 0))
	if(omega_4 > MAX_PROPELLERS_ANGULAR_VELOCITY)
	omega_4 = MAX_PROPELLERS_ANGULAR_VELOCITY;
	else
	omega_4 = 0;
	if(dataStoring_active_){
	// Saving drone attitude in a file
	std::stringstream tempPropellersVelocity;
	tempPropellersVelocity << omega_1 << "," << omega_2 << "," << omega_3 << "," << omega_4 << ","
	<< odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n";
	listPropellersVelocity_.push_back(tempPropellersVelocity.str());
	}
	ROS_DEBUG("Omega_1: %f Omega_2: %f Omega_3: %f Omega_4: %f", omega_1, omega_2, omega_3, omega_4);
	*rotor_velocities = Eigen::Vector4d(omega_1, omega_2, omega_3, omega_4);
	}

Ever time a new Odometry message arrives (it contains the drone state, i.e., position, attitude, the linear and angular velocity of the drone) a new set of the propellers angular velocity is computed by the controller algorithm (position_controlller.cpp) in a loop and it is sent to the node (`PositionControllerNode') which forward the message to Gazebo.

I hope this helps.

[KomaGR]

Thanks for your quick reply! I'm sorry if I did not explain it clearly. What you just said clarified a lot of things for me. I also looked at #10 for more information.

I think my problem is that I can't find anywhere messages being published on the Odometry topic (either on odometry_pub_topic_ or on mav_msgs::default_topics::ODOMETRY) inside the source code .

Do you know if this is something that is being handled internally in Gazebo?

...a new set of the propellers angular velocity is computed by the controller algorithm...

For me PositionController::CalculateRotorVelocities is called only when I set enable_state_estimator to true, and it is called from PositionControllerNode::CallbackHightLevelControl, still without any Odometry messages. Nevertheless, there is still no movement.

[gsilano]

It could be, it could be. You have to consider that the gazebo_odometry_plugin.cpp file in rotors_gazebo_plugins\src manages the interface between Gazebo and the Odometry message. Thus, if the code is unable to interface with Gazebo, no Odometry messages will be published on the topic.

[KomaGR]

I enabled verbosity in the example launch file and this is what I find:

[Err] [Plugin.hh:187] Failed to load plugin 
/opt/ros/melodic/share/gazebo_plugins/../../lib/librotors_gazebo_ros_interface_plugin.so: 
libmav_msgs.so: cannot open shared object file: No such file or directory
[Err] [Plugin.hh:187] Failed to load plugin 
/opt/ros/melodic/share/gazebo_plugins/../../lib/librotors_gazebo_multirotor_base_plugin.so: 
libmav_msgs.so: cannot open shared object file: No such file or directory
...

...and so on for all librotors_gazebo* shared objects.

This could be very well be the source of the problem. It looks like it is similar to ethz-asl/rotors_simulator#506 where you mention in ethz-asl/rotors_simulator#506 (comment) that the drone doesn't take off. Even though your CMakeLists.txt contains the fix in ethz-asl/rotors_simulator@787f8b5, it doesn't seem to work correctly.

Copying the libmav_msgs library in the directory of the librotors_gazebo*.so fixed it temporarily.

cp build/rotors_gazebo_plugins/libmav_msgs.so devel/lib/

Of course, this is not a good fix. The shared objects should ideally know to look for the library where it's located. Using ldd shows that the shared objects actually do know where it is. I do not know why the can't find it on runtime.

ldd catkin_wc/devel/lib/librotors_gazebo_motor_model.so | grep libmav
libmav_msgs.so => /home/koma/catkin_wc/build/rotors_gazebo_plugins/libmav_msgs.so 
(0x00007f633c6d5000)

---

For any other Arch users out there, the next problem I faced was with opencv versioning. A common situation, usually resolved by linking the previous version to the newest:

[Err] [Plugin.hh:187] Failed to load plugin 
/opt/ros/melodic/share/gazebo_plugins/../../lib/librotors_gazebo_odometry_plugin.so: 
libopencv_imgproc.so.4.2: cannot open shared object file: No such file or directory
...

ln -s /usr/lib/libopencv_imgproc.so.4.3  /usr/lib/libopencv_imgproc.so.4.2

And there was flight

[gsilano]

Ok, awesome! I am happy to know that you have finally fixed the issue. So, you can run CrazyS with Gazebo 10 on an Arch Linux distro. Am I right?

I'm thinking it would be great to put these instructions on the CrazyS and BebopS Wiki (also use RotorS as the basis for the package).

[KomaGR]

Ok, awesome! I am happy to know that you have finally fixed the issue. So, you can run CrazyS with Gazebo 10 on an Arch Linux distro. Am I right?

Yes. Current Arch Linux kernel version is 5.6.3-arch1-1 and Gazebo version is 10.1.0.

I'm thinking it would be great to put these instructions on the CrazyS and BebopS Wiki (also use RotorS as the basis for the package).

I'm still checking to see if there are any problems left. That said, wouldn't it be best if this was addressed at the CMakeLists.txt? Though, I'm not sure yet why the lib is not linking correctly.

[gsilano]

Yes, of course. It would be better if we could find a way to include the library in the CMakeList.txt file.