Setting points after takeoff not working

2

I am not sure if your goal is to fly without gps. But with gps you can push waypoints either when the vehicle is in flight or on the land and have it follow your gps waypoint in gazebo simulation.

This is how I do it. I got these code snippets from other posts and scrounging around documentation and code

 for (int i = 0; i<allUAVWaypoints.size(); i++)
    {
        std::cout<<"Waypoint size = "<<allUAVWaypoints[i].waypoints.size() <<std::endl;
        for(int k = 0; k < allUAVWaypoints[i].waypoints.size(); k++)
        {
            waypointPusher(wayPusher[i], pushClient[i], n,
                           allUAVWaypoints[i].waypoints[k].frame,
                           allUAVWaypoints[i].waypoints[k].command,
                           allUAVWaypoints[i].waypoints[k].isCurrent,
                           allUAVWaypoints[i].waypoints[k].isAutoContinue,
                           allUAVWaypoints[i].waypoints[k].param1,
                           allUAVWaypoints[i].waypoints[k].param2,
                           allUAVWaypoints[i].waypoints[k].param3,
                           allUAVWaypoints[i].waypoints[k].param4,
                           allUAVWaypoints[i].waypoints[k].lat,
                           allUAVWaypoints[i].waypoints[k].lon,
                           allUAVWaypoints[i].waypoints[k].alt,
                           allUAVWaypoints[i].serviceName);
        }
    }

where the waypointpusher is

bool waypointPusher( mavros_msgs::WaypointPush &pusher, ros::ServiceClient client, ros::NodeHandle node,
    int frame, int command, bool isCurrent, bool autoCont,
    float param1, float param2, float param3, float param4,
    float lat, float lon, float alt, std::string serviceName )
{
    client = node.serviceClient<mavros_msgs::WaypointPush>(serviceName);

    mavros_msgs::Waypoint nextWaypoint;

    nextWaypoint.frame = frame;
    nextWaypoint.command = command;
    nextWaypoint.is_current = isCurrent;
    nextWaypoint.autocontinue = autoCont;
    nextWaypoint.param1 = param1;
    nextWaypoint.param2 = param2;
    nextWaypoint.param3 = param3;
    nextWaypoint.param4 = param4;
    nextWaypoint.x_lat = lat;
    nextWaypoint.y_long = lon;
    nextWaypoint.z_alt = alt;

    std::cout<<"Pushing to service "<< serviceName << std::endl;
    pusher.request.waypoints.push_back(nextWaypoint);

    if( client.call( pusher) )
        ROS_INFO_STREAM("PUSHED WAYPOINT");
    else
        ROS_INFO_STREAM("PUSH FAILED");

    return client.call(pusher);
}

I generated the waypoints via QGroundControl and saved them in a file and read them in. The service name to push to is

uav1/mavros/mission/push

Additionally, if you are doing GPS waypoint mission you will need to turn off a couple of parameters that won’t allow missions if there is no RC link or data link. For the exact names of the parameter you can look at my post

Not sure this is helpful to you. But I never got any help from others on this forum. You will at least have a reply now :slight_smile:

1 Like