MAVROS and GAZEBO issues

1

Hello all,
I am trying to MAVROS to fly a simulated quad in GAZEBO (using the base px4 code).

  • Most recent ROS_Indigo version and most recent MAVROS version.
  • Very recent PX4 code
  • Gazebo 7
  • Ubuntu 14.04

There are 2 specific issues i am facing (after OFFBOARD is in control):

  1. The mavros directions are different to the Gazebo and QGroundControl direction (eg, mavros ‘x’ is along the gazebo ‘y’ axis, rotated -90o). Not sure how to fix this, any advice will help.

  2. I can control the position of the quad in Gazebo via MAVROS using:

ros::NodeHandle np;
ros::Publisher local_pos_pub = np.advertise<geometry_msgs::PoseStamped>
(“mavros/setpoint_position/local”, 10);

geometry_msgs::PoseStamped posep;

posep.pose.position.x = 0;
posep.pose.position.y = 0;
posep.pose.position.z = 3;

local_pos_pub.publish(posep);
ros::spinOnce();
ratep.sleep();

However, I cannot control orientation via both methods of:

ros::NodeHandle np;
ros::Publisher local_pos_pub = np.advertise<geometry_msgs::PoseStamped>
(“mavros/setpoint_position/local”, 10);

geometry_msgs::PoseStamped posep;

posep.pose.orientation.z = 0.7071;

local_pos_pub.publish(posep);
ros::spinOnce();
ratep.sleep();

(The rotation via this is binary: when .z set >0.78, it goes to -0.999999 (in quaternion). Otherwise when <0.78, goes to 0)

and

ros::NodeHandle np;
ros::Publisher local_pos_pub = np.advertise<geometry_msgs::PoseStamped>
(“mavros/setpoint_attitude/attitude”, 10);

geometry_msgs::PoseStamped posep;

posep.pose.orientation.z = 0.7071;

local_pos_pub.publish(posep);
ros::spinOnce();
ratep.sleep();

(results in the simulated quad going into freefall).

If it helps, using

/mavros/setpoint_velocity/cmd_vel

works to control velocity, but

/mavros/setpoint_attitude/cmd_vel

goes into freefall.

Any advice for both of these issues?

Thanks,

2

Are you unable to reproduce this tutorial?
http://dev.px4.io/ros-mavros-offboard.html

3

I reproduced that example exactly as it is (except that it rotates on takeoff, due to issue #1 as below).
That is how I got position control to work, and later manipulated it to get velocity control to work (as shown).
I just simplified the code to show you only the active components in the examples shown.

The 2 problems I am facing is that:

  1. The MAVROS and GAZEBO axis of motion are unaligned (rotated -90o along z axis).
  2. I cannot rotate the vehicle to my desired positions (yaw), instead it goes into either freefall OR goes to only 0 or 180 degrees depending on value.
4

Are you only writing the z value of the orientation quaternion, as you show above? That wouldn’t be a valid rotation quaternion because it does not have unit norm. Try sending a valid unit quaternion, e.g.

posep.pose.orientation.z = 0.7071;
posep.pose.orientation.w = 0.7071;
5

I worked out I was doing the quaternion a few days ago wrong, so I have managed to rotate it now.

Still having the mavros and gazebo axis being misaligned though.

6

@CaptainJL, how did you manage to rotate it?
I try the code below. Unfortunately, the drone does not rotate, it just hovers.

	//pose publisher
ros::Publisher local_pos_pub = n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local",10);
geometry_msgs::PoseStamped poseMsg;

//move 1
poseMsg.pose.position.x = 0;
poseMsg.pose.position.y = 0;
poseMsg.pose.position.z = 3;
poseMsg.pose.orientation.z = 0.7071;
poseMsg.pose.orientation.z = 0.7071;
ROS_INFO("move 1");
for(int i = 100; ros::ok() && i > 0; --i){
    	local_pos_pub.publish(poseMsg);
    	ros::spinOnce();
   	 	r.sleep();
	}