mit-acl / mader Goto Github PK
View Code? Open in Web Editor NEWTrajectory Planner in Multi-Agent and Dynamic Environments
License: BSD 3-Clause "New" or "Revised" License
Trajectory Planner in Multi-Agent and Dynamic Environments
License: BSD 3-Clause "New" or "Revised" License
I encountered this issue when trying to run mader in ROS Noetic and NLopt (I have not tested any other version). This issue can be reproduced in docker, as follows:
Create an empty directory containing the dockerfile Dockerfile
with this content:
FROM ros:noetic
RUN apt update
RUN apt install git -y
WORKDIR /home/src
RUN git clone https://github.com/mit-acl/mader.git --recursive
WORKDIR /home
RUN export DEBIAN_FRONTEND=noninteractive && rosdep install --from-paths src/mader --ignore-src -r -y --rosdistro noetic
RUN apt install python3-catkin-tools libgmp3-dev libmpfr-dev wget -y
#CGAL
RUN wget https://github.com/CGAL/cgal/releases/download/releases%2FCGAL-4.14.2/CGAL-4.14.2.tar.xz
RUN tar -xf CGAL-4.14.2.tar.xz
WORKDIR /home/CGAL-4.14.2
RUN cmake . -DCMAKE_BUILD_TYPE=Release
RUN make install
WORKDIR /home
RUN rm -rf CGAL-4.14.2 CGAL-4.14.2.tar.xz
#NLOPT
RUN wget https://github.com/stevengj/nlopt/archive/v2.6.2.tar.gz
RUN tar -xvzf v2.6.2.tar.gz
WORKDIR /home/nlopt-2.6.2
RUN cmake . && make install
WORKDIR /home
RUN rm -rf nlopt-2.6.2 v2.6.2.tar.gz
RUN . /opt/ros/noetic/setup.sh && catkin config -DCMAKE_BUILD_TYPE=Release && catkin build -DUSE_GUROBI=OFF
RUN apt install python-is-python3 ros-noetic-gazebo-msgs -y
In this directory, run docker build -t ros_mader .
and afterwards run docker run -it ros_mader bash -c '. devel/setup.bash && roslaunch mader single_agent_simulation.launch'
. This is the full logging output:
... logging to /root/.ros/log/f3df482a-6291-11ec-8558-0242ac110002/roslaunch-90861c4a2d98-1.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://90861c4a2d98:34169/
SUMMARY
========
PARAMETERS
* /SQ01s/mader/Ra: 4.0
* /SQ01s/mader/a_max: [20.0, 20.0, 9.6]
* /SQ01s/mader/a_star_bias: 1.0
* /SQ01s/mader/a_star_fraction_voxel_size: 0.5
* /SQ01s/mader/a_star_samp_x: 5
* /SQ01s/mader/a_star_samp_y: 5
* /SQ01s/mader/a_star_samp_z: 5
* /SQ01s/mader/allow_infeasible_guess: True
* /SQ01s/mader/alpha: 0.0
* /SQ01s/mader/alpha_filter_dyaw: 0
* /SQ01s/mader/alpha_shrink: 0.95
* /SQ01s/mader/basis: MINVO
* /SQ01s/mader/beta: 0.0
* /SQ01s/mader/color_type: vel
* /SQ01s/mader/dc: 0.01
* /SQ01s/mader/deg_pol: 3
* /SQ01s/mader/dist_factor_alloc_close: 2.0
* /SQ01s/mader/drone_radius: 0.05
* /SQ01s/mader/epsilon_tol_constraints: 1e-06
* /SQ01s/mader/factor_alloc: 1.0
* /SQ01s/mader/factor_alloc_close: 1.5
* /SQ01s/mader/factor_alpha: 1.5
* /SQ01s/mader/fov_depth: 4.5
* /SQ01s/mader/fov_horiz_deg: 100
* /SQ01s/mader/fov_vert_deg: 100
* /SQ01s/mader/ftol_rel: 1e-10
* /SQ01s/mader/gamma: 0.1
* /SQ01s/mader/goal_radius: 0.15
* /SQ01s/mader/j_max: [30.0, 30.0, 30.0]
* /SQ01s/mader/kappa: 0.4
* /SQ01s/mader/lower_bound_runtime_snlopt: 0.05
* /SQ01s/mader/mu: 0.4
* /SQ01s/mader/n_agents: 16
* /SQ01s/mader/num_pol: 4
* /SQ01s/mader/res_plot_traj: 15.0
* /SQ01s/mader/solver: LD_MMA
* /SQ01s/mader/upper_bound_runtime_snlopt: 0.35
* /SQ01s/mader/use_ff: True
* /SQ01s/mader/v_max: [3.5, 3.5, 3.5]
* /SQ01s/mader/visual: True
* /SQ01s/mader/w_max: 2.5
* /SQ01s/mader/weight: 1000.0
* /SQ01s/mader/x_max: 10000000000.0
* /SQ01s/mader/x_min: -1000000000.0
* /SQ01s/mader/xtol_rel: 1e-13
* /SQ01s/mader/y_max: 1000000000.0
* /SQ01s/mader/y_min: -1000000000.0
* /SQ01s/mader/z_max: 1000000000.0
* /SQ01s/mader/z_min: -1000000000.0
* /SQ01s/perfect_tracker/x: 0
* /SQ01s/perfect_tracker/y: 0
* /SQ01s/perfect_tracker/yaw: 0.0
* /SQ01s/perfect_tracker/z: 1.0
* /SQ01s/rosconsole_overlay_text/nodes: /SQ01s/mader
* /SQ01s/rosconsole_overlay_text/reverse_lines: False
* /rosdistro: noetic
* /rosversion: 1.15.13
NODES
/
behavior_selector (behavior_selector/behavior_selector_node.py)
dynamic_corridor (mader/dynamic_corridor.py)
rviz (rviz/rviz)
/SQ01s/
mader (mader/mader_node)
mader_commands (mader/mader_commands.py)
perfect_tracker (mader/perfect_controller.py)
rosconsole_overlay_text (jsk_rviz_plugins/rosconsole_overlay_text.py)
vicon2world (tf/static_transform_publisher)
auto-starting new master
process[master]: started with pid [50]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to f3df482a-6291-11ec-8558-0242ac110002
process[rosout-1]: started with pid [60]
started core service [/rosout]
process[SQ01s/mader_commands-2]: started with pid [67]
process[SQ01s/vicon2world-3]: started with pid [68]
process[SQ01s/perfect_tracker-4]: started with pid [69]
process[behavior_selector-5]: started with pid [70]
process[rviz-6]: started with pid [76]
process[dynamic_corridor-7]: started with pid [77]
process[SQ01s/mader-8]: started with pid [78]
process[SQ01s/rosconsole_overlay_text-9]: started with pid [79]
qt.qpa.xcb: could not connect to display
qt.qpa.plugin: Could not load the Qt platform plugin "xcb" in "" even though it was found.
This application failed to start because no Qt platform plugin could be initialized. Reinstalling the application may fix this problem.
Available platform plugins are: eglfs, linuxfb, minimal, minimalegl, offscreen, vnc, xcb.
Parameters obtained
Parameters obtained, checking them...
Parameters checked
In the SolverNlopt Constructor
initial_state=
Pos, Vel, Accel, Jerk= -4 1 -2 -1.3 0.6 1.5 0 0 0 0 0 0
final_state=
Pos, Vel, Accel, Jerk= 4 2 3 0 0 0 0 0 0 0 0 0
[NL] Running A* from-4 1 -2 to 4 2 3, allowing time = 0 ms
[A*] Running...
[A*] Max Runtime was reached
[A*] choosing closest complete path as solution
complete_closest_dist_so_far_= 1.27
velocity constraint for vi is not satisfied, i=0
N_=10
qa= -4 1 -2
qb= -4.17 1.08 -1.8
qc= -4.51 1.24 -1.41
qd= 4.35 1.47 2.31
Vbs=
-1.3 -1.3 22.4
0.6 0.6 0.6
1.5 1.5 9.41
V_newbasis=
-1.3 -1.3 22.4
0.6 0.6 0.6
1.5 1.5 9.41
v_max_=
10
10
10
Using matrix
1 0 0
0 1 0
0 0 1
velocity constraint for vi is not satisfied, i=1
N_=10
qa= -4.17 1.08 -1.8
qb= -4.51 1.24 -1.41
qc= 4.35 1.47 2.31
qd= 3.83 1.71 2.9
Vbs=
-1.3 22.4 -1.3
0.6 0.6 0.6
1.5 9.41 1.5
V_newbasis=
-1.3 22.4 -1.3
0.6 0.6 0.6
1.5 9.41 1.5
v_max_=
10
10
10
Using matrix
1 0 0
0 1 0
0 0 1
velocity constraint for vi is not satisfied, i=2
N_=10
qa= -4.51 1.24 -1.41
qb= 4.35 1.47 2.31
qc= 3.83 1.71 2.9
qd= 3.32 1.95 3.5
Vbs=
22.4 -1.3 -1.3
0.6 0.6 0.6
9.41 1.5 1.5
V_newbasis=
22.4 -1.3 -1.3
0.6 0.6 0.6
9.41 1.5 1.5
v_max_=
10
10
10
Using matrix
1 0 0
0 1 0
0 0 1
This should never happen: All complete paths are guaranteed to be feasible
=====================================================
[rviz-6] process has died [pid 76, exit code -6, cmd /opt/ros/noetic/lib/rviz/rviz -d /home/src/mader/mader/rviz_cfgs/mader.rviz __name:=rviz __log:=/root/.ros/log/f3df482a-6291-11ec-8558-0242ac110002/rviz-6.log].
log file: /root/.ros/log/f3df482a-6291-11ec-8558-0242ac110002/rviz-6*.log
********************************
['/home/src/mader/mader/scripts/dynamic_corridor.py', '80', '__name:=dynamic_corridor', '__log:=/root/.ros/log/f3df482a-6291-11ec-8558-0242ac110002/dynamic_corridor-7.log']
Starting perfect tracker node for: /SQ01s/
Starting behavior selector
80
[SQ01s/mader-8] process has died [pid 78, exit code -6, cmd /home/devel/lib/mader/mader_node ~state:=state ~who_plans:=who_plans ~term_goal:=term_goal ~traj:=traj ~goal:=goal ~setpoint:=setpoint __name:=mader __log:=/root/.ros/log/f3df482a-6291-11ec-8558-0242ac110002/SQ01s-mader-8.log].
log file: /root/.ros/log/f3df482a-6291-11ec-8558-0242ac110002/SQ01s-mader-8*.log
As shown in the figure, I have modified the dimensions of the static obstacles and the agents stuck in avoiding them. It seems the agents don't have the perception capabilities and straight goes towards the obstacles rather avoiding them. Can you please let me know if I am making any mistakes or if I need to change any parameters?
P.S. The dimension of the static obstacle is modified only in the class MovingForest
initialization of the dynamic_forest.py
script.
Thanks
Hi,
I was curious as to why you didn't add jerk bounds in the optimization formulation to further guarantee feasability. Is it possible with the current formulation? Am I missing something? Is the cost function minimizing the jerk sufficient?
Cheers,
CT
[SQ01s/perfect_tracker-4] process has died [pid 312, exit code 1, cmd /home/gsk/Dynamic_multi_agents_ws/src/mader/mader/scripts/perfect_controller.py __name:=perfect_tracker __log:=/home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/SQ01s-perfect_tracker-4.log].
log file: /home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/SQ01s-perfect_tracker-4*.log
process[SQ01s/mader-8]: started with pid [325]
process[SQ01s/rosconsole_overlay_text-9]: started with pid [327]
Traceback (most recent call last):
File "/home/gsk/Dynamic_multi_agents_ws/src/mader/mader/scripts/dynamic_corridor.py", line 14, in
import roslib
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/init.py", line 50, in
from roslib.launcher import load_manifest # noqa: F401
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 42, in
import rospkg
ModuleNotFoundError: No module named 'rospkg'
Parameters obtained
Parameters obtained, checking them...
Parameters checked
Changing DroneStatus from TRAVELING to GOAL_REACHED
Set parameter Username
Academic license - for non-commercial use only - expires 2025-06-05
[dynamic_corridor-7] process has died [pid 321, exit code 1, cmd /home/gsk/Dynamic_multi_agents_ws/src/mader/mader/scripts/dynamic_corridor.py 80 __name:=dynamic_corridor __log:=/home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/dynamic_corridor-7.log].
log file: /home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/dynamic_corridor-7*.log
Traceback (most recent call last):
File "/home/gsk/Dynamic_multi_agents_ws/src/mader/mader/scripts/mader_commands.py", line 11, in
import rospy
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/init.py", line 49, in
from .client import spin, myargv, init_node,
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/client.py", line 52, in
import roslib
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/init.py", line 50, in
from roslib.launcher import load_manifest # noqa: F401
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 42, in
import rospkg
ModuleNotFoundError: No module named 'rospkg'
Traceback (most recent call last):
File "/home/gsk/Dynamic_multi_agents_ws/src/mader/submodules/behavior_selector/scripts/behavior_selector_node.py", line 2, in
import rospy
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/init.py", line 49, in
from .client import spin, myargv, init_node,
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/client.py", line 52, in
import roslib
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/init.py", line 50, in
from roslib.launcher import load_manifest # noqa: F401
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 42, in
import rospkg
ModuleNotFoundError: No module named 'rospkg'
[SQ01s/mader_commands-2] process has died [pid 310, exit code 1, cmd /home/gsk/Dynamic_multi_agents_ws/src/mader/mader/scripts/mader_commands.py __name:=mader_commands __log:=/home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/SQ01s-mader_commands-2.log].
log file: /home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/SQ01s-mader_commands-2*.log
Traceback (most recent call last):
File "/opt/ros/melodic/lib/jsk_rviz_plugins/rosconsole_overlay_text.py", line 7, in
import rospy
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/init.py", line 49, in
from .client import spin, myargv, init_node,
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/client.py", line 52, in
import roslib
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/init.py", line 50, in
from roslib.launcher import load_manifest # noqa: F401
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 42, in
import rospkg
ModuleNotFoundError: No module named 'rospkg'
[behavior_selector-5] process has died [pid 318, exit code 1, cmd /home/gsk/Dynamic_multi_agents_ws/src/mader/submodules/behavior_selector/scripts/behavior_selector_node.py __name:=behavior_selector __log:=/home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/behavior_selector-5.log].
log file: /home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/behavior_selector-5*.log
[SQ01s/rosconsole_overlay_text-9] process has died [pid 327, exit code 1, cmd /opt/ros/melodic/lib/jsk_rviz_plugins/rosconsole_overlay_text.py __name:=rosconsole_overlay_text __log:=/home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/SQ01s-rosconsole_overlay_text-9.log].
log file: /home/gsk/.ros/log/6d3751cc-234c-11ef-8c52-70b5e83420b1/SQ01s-rosconsole_overlay_text-9*.log
[ INFO] [1717599663.933602689]: Planner initialized
Hi,
Just wondering why the trajectories generated in octopus search does not actually reach the final end point?
Thanks!
Hi there,
I checked https://github.com/mit-acl/minvo but could not find how these basis transformations were obtained
BSPLINE to MINVO POSITION
mader/mader/include/mader_types.hpp
Line 192 in 5e9fdf1
BSPLINE to BEZIER POSITION
mader/mader/include/mader_types.hpp
Line 192 in 5e9fdf1
if you can provide any hint, it would be great.
Thanks,
Hi,
I'm interested in running Mader with the NLOPT optimizer. Could you advise me on the most suitable commit version to use for this purpose? Your assistance would be greatly appreciated.
Best, Hadar.
Hi,
I was wondering if this is repo is tested in the real world? or does it have a gazebo simulation example? @jtorde
thanks
Hi,
Which parameters need to be modified to ensure that the algorithm consistently calculates a path that reaches the final goal?
( I'm using nlopt if it matters)
Thanks!
hello I have great interest in mader
so i want to know the input and output of mader in real world,such as depth image or lidar point cloud?
I followed the setup steps in the description, however, an error occured when running 'bash src/mader/install_and_compile.sh'.
The error is :
'Could not find a package configuration file provided by "snapstack_msgs" with any of the following names:
snapstack_msgsConfig.cmake
snastack_msgs-config.cmake '
And this directly led to building failure of the package named "decomp_test_node"
Could anyone please help me with this one?
The code has just been released at https://github.com/mit-acl/mader
Hi,
Firstly, thanks for this wonderful package. Upon looking into the codes, I realize that the Gurobi version does not implement the original QCQP problem introduced in the paper, but instead it uses the initial guesses of the plane parameters as constants. I would like to ask, is this purely for efficiency reasons, or it is because Gurobi is not capable of solving this type of problem? From Gurobi documentation it seems capable of solving QCQP, but it requires the problem to be convex.
Thank you!
Best Regards,
Mq
I can successfully make the package of mader, but when I run the follow cmd, I encountered a fetal problem.
roslaunch mader single_agent_simulation.launch
[SQ01s/mader-8] process has died [pid 30973, exit code -6, cmd /home/dzp/mader_ws/devel/lib/mader/mader_node ~state:=state ~who_plans:=who_plans ~term_goal:=term_goal ~traj:=traj ~goal:=goal ~setpoint:=setpoint __name:=mader __log:=/home/dzp/.ros/log/497f92e2-8a1f-11ee-bb0b-e89c257f2e5e/SQ01s-mader-8.log].
log file: /home/dzp/.ros/log/497f92e2-8a1f-11ee-bb0b-e89c257f2e5e/SQ01s-mader-8*.log
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.