search

mit-acl / mader

Trajectory Planner in Multi-Agent and Dynamic Environments
BSD 3-Clause "New" or "Revised" License
496 stars 77 forks source link

[ROS Noetic/NLopt] This should never happen: All complete paths are guaranteed to be feasible #9

Closed janblumenkamp closed 2 years ago

janblumenkamp commented 2 years ago

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
janblumenkamp commented 2 years ago

Tried running it with Gurobi and get no error messages, but the mader node still crashes with the same exit code:

[SQ01s/mader-8] process has died [pid 83, 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:=/home/jan/.ros/log/453c5c7e-6316-11ec-bae3-0242ac110002/SQ01s-mader-8.log].

The updated Dockerfile and a script to run the container that allows X11 forwarding through docker can be found here: runfiles.zip This requires the Gurobi license file to be placed in the same folder as the docker container.

jtorde commented 2 years ago

The error "This should never happen: All complete paths are guaranteed to be feasible" should be fixed in the master branch, let me know if you still find this issue

wangmenghetrue commented 2 years ago

I encountered the same problem. How did you solve this problem?