Setting points after takeoff not working

[danividanivi]

I have been following this tutorial with success. I have a gazebo simulation that takes off in the desired GPS position and after that lands safely.

However, I want my simulated drone to fly to a different position, so I tried to modifying the part of the code “Do stuff” with this. Instead of:

////////////////////////////////////////////
/////////////////DO STUFF///////////////////
////////////////////////////////////////////
sleep(10);

I have now:

        ros::Rate rate(20.0);
        ros::Publisher local_pos_pub = n.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
        geometry_msgs::PoseStamped pose;
        pose.pose.position.x = 20;
        pose.pose.position.y = 20;
        pose.pose.position.z = 40;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    local_pos_pub.publish(pose);

But it is not working, drone takes off and lands but it doesn’t go to the position I want.
What am I doing wrong?

[kiranarcot]

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

How to to provide different missions to multiple UAVS in SITL

Hello All, I have gone through the “[multi vehicle simulation”] (Multi-Vehicle Simulation · PX4 Developer Guide) in Gazebo documentation and was able to add one more IRIS model to multi_uav_mavros_sitl.launch and it works great. There are 3 IRISes in Gazebo and 3 Vehicles in QGC. I attempted to provide 3 different missions to the 3 different vehicles via QGC. For example, Vehicle 1: Takeoff@5m, WP1 at 5 meter and hover for 30 seconds, WP at 5 meters hover for 30 seconds, Land Vehicle 2: Take…

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

[danividanivi]

This was really helpful, thank you very much.