Unstable hector_quadrotor above a given height [closed]

edit

The simple controller plugin takes two parameters stateTopic and imuTopic, which can be used independently. These parameters are also documented here: http://www.ros.org/wiki/hector_quadrotor_controller#XML_Parameters.

If imuTopic (sensor_msgs/Imu) is set, the controller plugin subscribes to that topic and uses the orientation and angular rates within for control. The velocity and its derivation are taken from a (nav_msgs/Odometry) message on topic stateTopic. If only stateTopic is non-empty, the orientation and angular rates are also taken from the Odometry message. When one or both of the parameters are empty, the plugin uses ground truth information from the Gazebo API to control the quadrotor.

The plugin configuration provided by quadrotor_simple_controller.urdf.xacro sets stateTopic to ground_truth/state, which is published by the gazebo_ros_p3d plugin, and the imuTopic to raw_imu, which is published by the hector_gazebo_ros_imu plugin. Both plugins can add noise to make the quadrotor flight more realistic. Instead of using the ground truth state directly you can also use the hector_localization stack that implements an Extended Kalman Filter to fuse all available measurements (IMU, Magnetometer, Pressure, GPS) to a combined pose estimate which can be fed into the controller using the stateTopic parameter.

Coming back to the problem: If the quadrotor is instable when using ground_truth/state as stateTopic and stable if the parameter is empty, it seems that some messages are lost or there is too much latency between the p3d and the controller plugin. As the publisher and subscriber are running in the same process, roscpp uses intra-process links and until now I was not aware of problems with latency or dropped messages.

You could add a debug output in the GazeboQuadrotorSimpleController::StateCallback() method to check latency (by comparing the timestamp with Gazebo's current SimTime) and if messages get lost (by looking at the seq number).

Note that the GazeboQuadrotorSimpleController plugin has been moved back from package hector_quadrotor_controller to package hector_quadrotor_gazebo_plugins in the SVN trunk recently.