Change home position in Voxl-mapper

[howard]

Hello,
I would like to send the coordinates for PLAN TO POINT in Voxl-Mapper rather than selecting in the map. While going through the codes I found this line where the goal_pose is being updated.
So can I use a python script to send goal_pose and when I click PLAN TO POINT, it plans a path to that point?
For this where should I place the python file? How can I find the voxl-mapper package inside voxl?

[A Former User]

Hey @howard,

The line you linked to is within the control pipe callback function that is setup for exactly this purpose. From within a C/C++ application, you can use the pipe_client_send_control_cmd function to write a command in this format:

plan_to: x_coord y_coord
(where z_coord is taken as your current height, and coords are doubles).

From within a python script or bash on voxl, you can simply echo/write to the control pipe yourself using the same syntax (and the pipe will be /run/mpa/plan_msgs/control).

[howard]

Oh okay, That was helpful ,Thank you

[howard]

Hey,
So today I tried sending x and y coordinates as mentioned above, we could see the path on voxl portal and the trajectory was sent to voxl vison px4 , however when we switched to offboard mode and pressed follow button, the vehicle didn't follow the path instead it yawed by some angle and went a little forward. What could be the issue?
Thanks!!

[A Former User]

@howard what offboard mode do you have set currently? It sounds like you may still be in figure eight mode.

[howard]

@Matthew-Booker No , I have changed it to trajectory

[A Former User]

Try changing the loco_scale_time parameter to false in the /etc/modalai/voxl-mapper.conf

[howard]

@Matthew-Booker sure, will do that. Can you please elaborate a little on loco_scale_time parameter ? what does that parameter do?
Thank you

[A Former User]

The parameter scales the segment times evenly to ensure that the trajectory is feasible given the provided max velocity and max acceleration. It does not change the shape of the trajectory and only increases segment times. I believe that in your version of voxl-mapper we did not expose the parameters for max velocity and max acceleration.

In my recent testing I saw some cases where the trajectory length would be upwards of 3 minutes long despite the distance being extremely small. Changing this parameter prevents that. I have yet to look into the cause of this issue

[howard]

Hey, Thank you , that worked really well.

1. Where can we find the 'voxl_mapper.cc' script in the filesystem of voxl?
2. We are interested to provide a 3d goal point rather than only providing goal x,y coordinates with z fixed. Will point to point navigation work if a 3d goal point is provided (i.e. providing x,y and z coordinates rather than only giving x and y coordinates with a constant z) with the current package?

[A Former User]

1. https://gitlab.com/voxl-public/voxl-sdk/services/voxl-mapper - Here is the link to the publicly available repo. The voxl_mapper.cc file is under the server/voxl-mapper folder. On the voxl the file voxl_mapper.cc doesn't exist since its all packaged up into a single IPK file.

2. The planner sets the z coordinate of the goal to the current height so you would have to change it in this line https://gitlab.com/voxl-public/voxl-sdk/services/voxl-mapper/-/blob/master/server/voxl-mapper/voxl_mapper.cc#L1001

[ahmed-mustahid]

From within a C/C++ application, you can use the pipe_client_send_control_cmd function to write a command in this format:

I have looked for this function in the voxl-mapper gitlab master branch but was not able to find it. Has the name been changed in the latest master branch ?

From within a python script or bash on voxl, you can simply echo/write to the control pipe yourself using the same syntax (and the pipe will be /run/mpa/plan_msgs/control).

I have tried using voxl:~$ echo "-4.53 1.53" >> /run/mpa/plan_msgs/control (as per the plan_to: x_coord y_coord mentioned above) but there was no change in the voxl-portal.
I also tried echo "plan_to: -4.53 1.53" >> /run/mpa/plan_msgs/control but it caused the voxl-mapper to abort. (x: -4.53 y:1.53 worked correctly when used from voxl-portal UI )

voxl:~$ voxl-mapper 
Loading our own config file
Loading extrinsics config file
Trying to init tsdf server
created tsdf server
waiting for server at /run/mpa/vvpx4_body_wrt_fixed/
waiting for server at /run/mpa/tof
Connected to VIO server
Initializing ESDF structs
Connected to depth pipe
ERROR fetching tf from tf ringbuffer
there wasn't sufficient data in the buffer
Client requested plan to location
msg plan_to
goal str  -4.53  1.53

Aborted

Would you let me know the correct method to input the location programmatically (bash, python, C++ etc) ?

BTW, voxl-mapper output when PLAN_TO_POINT in voxl-portal UI is used (voxl-portal UI method works correctly):

voxl:~$ voxl-mapper 
Loading our own config file
Loading extrinsics config file
Trying to init tsdf server
created tsdf server
waiting for server at /run/mpa/vvpx4_body_wrt_fixed/
waiting for server at /run/mpa/tof
Connected to VIO server
Connected to depth pipe
Initializing ESDF structs
ERROR fetching tf from tf ringbuffer
there wasn't sufficient data in the buffer
ERROR fetching tf from tf ringbuffer
there wasn't sufficient data in the buffer
Client requested plan to location
msg plan_to
goal str  0.02459428645670414,-1.4353046417236328,0
Using start pose of: x:  -0.32, y:   0.13, z:  -0.04
Using goal pose of: x:   0.02, y:  -1.44, z:   0.00
Starting RRTConnect planner

^C
received SIGINT Ctrl-C
Closing Pipes
voxl:~$ 

Thank you

[A Former User]

Hey @ahmed-mustahid,

pipe_client_send_control_cmd is a function provided by libmodal-pipe that you could use from another C/C++ application (not within voxl-mapper) to send commands, thats likely why you didn't see it.

As for the syntax of the plan to command, messages are constructed as such:

const msg = "plan_to: " + (pos[0] + plan_pt.position.x) + "," + (pos[1] + plan_pt.position.y) + "," + (pos[2] + plan_pt.position.z);

i.e. message starts with "plan_to: ", and is followed by a comma separated (X,Y,Z) point.
If you can follow this syntax you should be able to send commands using whichever method works best in your workflow.