Mavros Waypoints Not Publishing

[galadriel123]

I am pretty new to using mavros and I need to fly my ArduCopter in a straight line using ROS. I have written some code to publish one waypoint. Here it is,

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include "mavros_msgs/WaypointList.h"
int main(int argc, char **argv)
{
   ros::init(argc, argv, "Publish_Waypoints");
   ros::NodeHandle n;
   ros::Publisher chatter_pub = n.advertise<mavros_msgs::Waypoint>("/mavros/mission/waypoints",100);
   ros::Rate loop_rate(100);
   ros::spinOnce();
   int count=0;
   mavros_msgs::Waypoint msg;

   while(ros::ok() && count<1)
{
 msg.frame=4;
       msg.command=1;
       msg.is_current=true;
       msg.autocontinue=true;
       msg.param1=1.0;
       msg.param2=1.0;
       msg.param3=1.0;
       msg.param4=1.0;
       chatter_pub.publish(msg);
       ros::spinOnce();
       count++;
       loop_rate.sleep();
   }
 return 0;
}

It runs successfully without any errors, but the table shown by rosrun mavros mavwp show never gets updated. It always shows

+---+------+------+---------+---------------+-----+-----+-----+-----+---------------+---------------+-------+
| # | Curr | Auto |  Frame  |    Command    |  P1 |  P2 |  P3 |  P4 |     X Lat     |     Y Long    | Z Alt |
+---+------+------+---------+---------------+-----+-----+-----+-----+---------------+---------------+-------+
| 0 |  No  | Yes  | GAA (0) | WAYPOINT (16) | 0.0 | 0.0 | 0.0 | 0.0 |      0.0      |      0.0      |  0.0  |
| 1 |  No  | Yes  | GRA (3) |  TAKEOFF (22) | 0.0 | 0.0 | 0.0 | 0.0 | 15.3930940628 | 73.8765869141 |  1.0  |
| 2 |  No  | Yes  | GRA (3) | WAYPOINT (16) | 0.0 | 0.0 | 0.0 | 0.0 | 15.3928804398 | 73.8765335083 |  1.0  |
| 3 |  No  | Yes  | GRA (3) | WAYPOINT (16) | 0.0 | 0.0 | 0.0 | 0.0 | 15.3929195404 | 73.8763961792 |  1.0  |
| 4 |  No  | Yes  | GRA (3) | WAYPOINT (16) | 0.0 | 0.0 | 0.0 | 0.0 | 15.3930692673 | 73.8763122559 |  1.0  |
| 5 |  No  | Yes  | GRA (3) | WAYPOINT (16) | 0.0 | 0.0 | 0.0 | 0.0 | 15.3932113647 | 73.8764266968 |  1.0  |
| 6 |  No  | Yes  | GRA (3) |   LAND (21)   | 0.0 | 0.0 | 0.0 | 0.0 | 15.3933181763 | 73.8765563965 |  1.0  |
+---+------+------+---------+---------------+-----+-----+-----+-----+---------------+---------------+-------+

Why is that? I tried arming it before running this node, but that didn't work either. Should I be publishing Waypointlist? but isn't that just an array of waypoints? I am pretty new to this. Please excuse any fundamental errors.

[vooon]

Mavros is publisher, why you try publish your own waypoint to same topic and with incorrect message type?

[galadriel123]

Ugh. I apologise. I now figured out how to do it using the mavros/mission/push service. Here is the code for that

ros::NodeHandle n;
   ros::ServiceClient client = n.serviceClient<mavros_msgs::WaypointPush>("mavr$
   mavros_msgs::WaypointPush srv2;
   ros::Rate loop_rate(100);
   ros::spinOnce();
   mavros_msgs::Waypoint msg;

       msg.frame=mavros_msgs::Waypoint::FRAME_GLOBAL;
       msg.command=1;
       msg.is_current=true;
       msg.autocontinue=true;
       msg.param1=2.0;
       msg.param2=3.0;
       msg.param3=4.0;
       msg.param4=5.0;
       msg.x_lat=9.0;
       msg.y_long =10.0;
       msg.z_alt=2.0;
 ros::spinOnce();
       srv2.request.waypoints.push_back(msg);
        if(client.call(srv2))
{
ROS_INFO("Success %d", srv2.response.success);
}
else
{
ROS_INFO("Failed");
}

The node runs successfully and gives output

[ INFO] [1496781027.254711560]: Success 0

But the waypoints do not update and the log shows me this error

[ INFO] [1496781662.707030726]: HP: requesting home position
[ INFO] [1496781672.707023923]: HP: requesting home position
[ INFO] [1496781682.707357223]: HP: requesting home position
[ INFO] [1496781692.706846724]: HP: requesting home position
[ INFO] [1496781702.707427939]: HP: requesting home position
[ INFO] [1496781712.706794993]: HP: requesting home position
[ERROR] [1496781717.008203992]: WP: upload failed: error #5
[ INFO] [1496781722.706693763]: HP: requesting home position

Thanks. I hope this is the right approach now?

[vooon]

Yes, but you don't have a connection. So push failed.

[galadriel123]

What do you mean by that? I am running roscore and the launch file.

[vooon]

Look at /diagnostics! Use rqt or rostopic echo -n1 /diagnostics. FCU do not respond on home position request. Also add roslaunch log. Note that roslaunch automatically start roscore.

[TSC21]

@galadriel123 what is the state of this?

[TSC21]

Closing as staled. @galadriel123 if the problem remains, reopen it.