Sending WP to pixhawk

[JJERPEB]

Hello!

I'm new to ROS and MAVlink and have been trying to control the pixhawk from a drone in a project i'm working on withouht succes...

I want to create a node that changes the drone mode to auto, then arms the drone and send a WP to the autopilot...

this is what i came up with...

`

include

include <ros/ros.h>

include <mavros_msgs/CommandBool.h>

include <mavros_msgs/CommandCode.h>

include <mavros_msgs/CommandTOL.h>

include <mavros_msgs/SetMode.h>

include <mavros_msgs/Mavlink.h>

include <mavros_msgs/Waypoint.h>

include <mavros_msgs/WaypointList.h>

include <mavros_msgs/WaypointSetCurrent.h>

include <mavros_msgs/WaypointClear.h>

include <mavros_msgs/WaypointPull.h>

include <mavros_msgs/WaypointPush.h>

int main(int argc, char **argv) { int rate = 10;

ros::init(argc, argv, "mavlink_comm");
ros::NodeHandle n;

ros::Rate r(rate);

//Setting Autopilot mode to AUTO_DISARMED

ros::ServiceClient cl = n.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
mavros_msgs::SetMode srv_setMode;
srv_setMode.request.base_mode =  92; //"MAV_MODE_AUTO_DISARMED";
//srv_setMode.request.custom_mode = 10;
if(cl.call(srv_setMode)){
    ROS_INFO("setmode send ok %d value:", srv_setMode.response.success);
}else{
    ROS_ERROR("Failed SetMode");
    return -1;
}

// Setting Autopilot to ARMED

ros::ServiceClient arming_cl = n.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
mavros_msgs::CommandBool srv_arming;
srv_arming.request.value = true;
if(arming_cl.call(srv_arming)){
    ROS_INFO("ARM send ok %d value", srv_arming.response.success);
}else{
    ROS_ERROR("Failed arming or disarming");
}

//Pushing a WP

ros::ServiceClient wp_cl = n.serviceClient<mavros_msgs::WaypointPush>("mavros/mission/push");

mavros_msgs::WaypointPush srv_wpPush;
mavros_msgs::Waypoint wp;

//Filling the WP 
wp.frame = mavros_msgs::Waypoint::FRAME_GLOBAL; //GLOBAL_FRAME
wp.command = mavros_msgs::CommandCode::NAV_WAYPOINT; //NAV_WAYPOINT
wp.is_current = false;
wp.autocontinue = false;
wp.param1 = 0;
wp.param2 = 2;
wp.param3 = 0;
wp.param4 = 0;
wp.x_lat = 48.359343;
wp.y_long = -4.573233;
wp.z_alt = 0;

srv_wpPush.request.waypoints.push_back(wp);

if (wp_cl.call(srv_wpPush)){
ROS_INFO("WP send ok %d value", srv_wpPush.response.success);
}else{
ROS_ERROR("Failed WP Send");
return -1;
}

while (n.ok())
{
  ros::spinOnce();
  r.sleep();
}

return 0;

} ` code in txt file node.txt

When I run the node I get the following

[ INFO] [1469719022.983060738]: setmode send ok 1 value: [ INFO] [1469719027.984290461]: ARM send ok 0 value [ INFO] [1469719031.988864722]: WP send ok 0 value

and mavros gives me these lines...

[ WARN] [1469719155.319478523]: WP: timeout, retries left 2 [ WARN] [1469719156.319690261]: WP: timeout, retries left 1 [ WARN] [1469719157.320182784]: WP: timeout, retries left 0 [ERROR] [1469719158.320509131]: WP: timed out.

I do not have a GPS fix (testing indoors) so that might have an impact on the WP

I've also read that there is a topic mavlink/to but I don't understand how to publish a wp or a mavlink message to it...

Any help will be greatly appreciated.

[vooon]

Did you got "connected" message? What rosrun mavros checkid says (run mavros first)?

[JJERPEB]

First on a terminal a ran roslaunch mavros apm.launch and I get this

roslaunch mavros.txt

In another terminal I ran rosrun mavros checkid and this is what I get

rosrun mavros checkid.txt

[vooon]

ERROR. I got 3 addresses, but not your target 1:1

---
Received 794 messages, from 3 addresses
sys:comp   list of messages
 13:1     0, 1, 2, 35, 36, 165, 193, 33, 74, 241, 163, 116, 62, 150, 24, 152, 27, 125, 30, 42
 51:68    109, 166
 12:1     0, 33, 2, 35, 36, 165, 1, 42, 125, 152, 163, 116, 30, 150, 24, 27, 74, 62

So you somehow has several devices. 51:68 is 3DR Radio, and i see it's messages RADIO_STATUS and RADIO. Set target system by adding tgt_system:=13 to launch.

[JJERPEB]

This worked, thank you for your help!!