Path Planning (Waypoint/Bezier trajectory msg) with companion Computer

[dookei]

Hi there, I am currently performing all the navigation and control in offboard mode with an algorithm to set my waypoints and adapt the route if there are any obstacle on the way. Now this works as expected but I would like to try this method with the waypoints and the Bezier(looks very cool). I know that this is currently being used in the local_planner and global_planner from the avoidance(https://github.com/PX4/avoidance) package. But from what I see in the /mavros/trajectory/desired and generated only one point is being set. Correct? I read already the available docs regarding this path/trajectory planning. Still not 100% clear if this is suppose to do what I aim to do :wink: Mavlink Path Planning(Trajectory) Protocol and Px4 Path Planning Interface

Here is what I tried so far: I made a test case just to try the trajectory in waypoint mode, by setting 3 waypoints as below.

    obst_avoid.point_1.position.x = pose.pose.position.x;
    obst_avoid.point_1.position.y = pose.pose.position.y;
    obst_avoid.point_1.position.z = pose.pose.position.z;
    obst_avoid.point_1.velocity.x = NAN;
    obst_avoid.point_1.velocity.y = NAN;
    obst_avoid.point_1.velocity.z = NAN;
    obst_avoid.point_1.acceleration_or_force.x = NAN;
    obst_avoid.point_1.acceleration_or_force.y = NAN;
    obst_avoid.point_1.acceleration_or_force.z = NAN;
    obst_avoid.point_1.yaw = tf::getYaw(pose.pose.orientation);
    obst_avoid.point_1.yaw_rate = -vel.angular.z;

    obst_avoid.point_valid = {true, true, true, false, false};
    obst_avoid.command = {0, 0, 0, 0, 0}; // this is unclear because only TAKEOFF LOITER LAND are available as in the mavlink_messages.cpp module. and actually 0 is the default. or should it be the Position?

I performed the same for point 2 and 3 just with different coordinates. Just for testing.

I am publishing this trajectory message at 5Hz nh.advertise<mavros_msgs::Trajectory>("/mavros/trajectory/generated", 10);

COM_OBS_AVOID is set to 1.

I also set the /mavros/companion_process/status = 196; /// MAV_COMPONENT_ID_AVOIDANCE

I start the nodes and everything looks good . [ INFO] [1600960481.367579651, 4.132000000]: FCU: Avoidance system connected

I set it to AUTO.TAKEOFF and arm it . after a while I set it to mission. The drone starts moving to Waypoint 1 at (3;3;5), once this is reached it stay in LOITER mode.

From rostopic echo /mavros/trajectory/desired I get the following:

point_1: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: ‘’ coordinate_frame: 0 type_mask: 0 position: x: 1.11012399197 y: 1.25041902065 z: 2.50076293945 … point_valid: [1, 1, 1, 0, 0] command: [17, 17, 65535, 65535, 65535]

from PX4 shell it looks like the waypoints are being correctly published.

pxh> listener vehicle_trajectory_waypoint

TOPIC: vehicle_trajectory_waypoint
vehicle_trajectory_waypoint_s
timestamp: 393668000 (0.000000 seconds ago)
type: 0
px4/trajectory_waypoint[5] waypoints[0] trajectory_waypoint_s

position: [3.0000, 3.0000, -5.0000]
velocity: [nan, nan, nan]
acceleration: [nan, nan, nan]
yaw: 1.5708
yaw_speed: -0.0000
point_valid: True
type: 255
px4/trajectory_waypoint[5] waypoints[1] trajectory_waypoint_s

position: [2.0000, 0.0000, -5.0000]
velocity: [nan, nan, nan]
acceleration: [nan, nan, nan]
yaw: 1.5708
yaw_speed: -0.0000
point_valid: True
type: 255
px4/trajectory_waypoint[5] waypoints[2] trajectory_waypoint_s

position: [-3.0000, 1.0000, -5.0000]
velocity: [nan, nan, nan]
acceleration: [nan, nan, nan]
yaw: 1.5708
yaw_speed: -0.0000
point_valid: True
type: 255
px4/trajectory_waypoint[5] waypoints[3] trajectory_waypoint_s

and from listener trajectory_setpoint

pxh> listener trajectory_setpoint

TOPIC: trajectory_setpoint
vehicle_local_position_setpoint_s
timestamp: 499916000 (0.020000 seconds ago)
x: 3.0000
y: 3.0000
z: -5.0000
yaw: 1.5708
yawspeed: -0.0000
vx: 0.0000
vy: 0.0000
vz: -0.0000
acc_x: 0.0000
acc_y: 0.0000
acc_z: 0.0000
jerk_x: 0.0000
jerk_y: 0.0000
jerk_z: 0.0000
thrust: [nan, nan, nan]

So what am I missing here ? Why it only setpoints the WP1 and does it doesn't fly to the next waypoint?

My questions

1. Can the Trajectory(waypoint) method be used to send 5 waypoints in way that I can dynamically update them. For example I set WP 1 to 5 and along the way I update 2 and 3?
2. Is there a simple functional example to demonstrate this at work?
3. What should be set under mavros_msgs::Trajectory. command () ?
4. Can the same be performed under bezier method ? Send the 5 WP and adapt along the way?
5. How to set WPs MAV_CMD? So that they land, loiter etc when the WP is reached?

If anyone could shed some light on this topic, I believe that there will be more people using this method in the future.

Greetings, Pedro

[dookei]

Hi @mrivi , Could you perhaps give your precious input ? ;-) Many Thanks