mavros set offboard mode
Hi,
I'm trying to control a pixhawk via mavros. I have written a test node which set the mode to offboard, arm the drone and send some mavros/setpoint_velocity/cmd_vel
messages to change the z velocity.
When I run the node the arming works (motors start spinning) but that's it, nothing more. I can't find the problem, This is my first time using mavros, so it's all new for me.
EDIT
While running my code I did rostopic echo /mavros/state
which results in:
---
header:
seq: 6144
stamp:
secs: 1512042153
nsecs: 557924943
frame_id: ''
connected: True
armed: True
guided: False
mode: "STABILIZE"
system_status: 3
---
So I believe that setting offboard mode isn't working.
My code:
#include "classheader_dronecontrol.hpp"
DroneControl::DroneControl(ros::NodeHandle n)
{
initRos(n);
}
void DroneControl::initRos(ros::NodeHandle n)
{
cout << "Start Initialising" << endl;
this->state_sub = n.subscribe<mavros_msgs::State>("mavros/state", 10, &DroneControl::stateCallback, this);
this->arming_client = n.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
this->set_mode_client = n.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
this->KFarucomarkerpos_sub = n.subscribe("droneposition", 1, &DroneControl::droneControlCallback, this);
this->move_pub = n.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel",10);
cout << "Initialising done!" << endl;
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}
this->move_msg.twist.linear.z = 1.0;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
this->move_pub.publish(this->move_msg);
ros::spinOnce();
rate.sleep();
}
this->offb_set_mode.request.custom_mode = "OFFBOARD";
this->arm_cmd.request.value = true;
last_request = ros::Time::now();
this->set_mode_client.call(this->offb_set_mode);
this->offb_set_mode.response.mode_sent;
ROS_INFO("Offboard enabled");
this->move_pub.publish(this->move_msg);
ros::spinOnce();
this->arming_client.call(this->arm_cmd);
ROS_INFO("Vehicle armed");
this->move_pub.publish(this->move_msg);
for(int i = 100; ros::ok() && i > 0; --i){
this->move_pub.publish(this->move_msg);
ros::spinOnce();
rate.sleep();
}
ros::spinOnce();
}
void DroneControl::stateCallback(const mavros_msgs::State::ConstPtr& msg)
{
this->current_state = *msg;
}
EDIT2
I used the provided starting point by 0xd1ma and used his answer for following code. It's a small program where I used roll = 30°, pitch = 30°, yaw = 0° and throttle 0.9. The messages will be published 500 times. The drone (in gazebo) armed perfectly and moved for 5 seconds with a velocity of 0.25 in z-axes (just some test code to get him of the ground), after this the testDroneControl() function gets called, but the drone starts moving randomly and for sure not in the chosen direction.
void DroneControl::testDroneControl()
{
ros::Rate rate(50.0);
this->cmd_att.pose.position.x = 0.0;
this->cmd_att.pose.position.y = 0.0;
this->cmd_att.pose.position.z = 0.0;
tf::Quaternion mav_orientation = tf::createQuaternionFromRPY(0.523598775, 0.523598775, 0 );
this->cmd_att.pose.orientation.x = mav_orientation.x();
this->cmd_att.pose.orientation.y = mav_orientation.y();
this->cmd_att.pose.orientation.z = mav_orientation.z();
this->cmd_att.pose.orientation.w = mav_orientation.w();
this->cmd_thr.data = 0.9;
for(int i=0; i<500; i++){
cout << this->cmd_att.pose ...