search

TempleRAIL / drl_vo_nav

[T-RO 2023] DRL-VO: Learning to Navigate Through Crowded Dynamic Scenes Using Velocity Obstacles
https://doi.org/10.1109/TRO.2023.3257549
GNU General Public License v3.0
140 stars 10 forks source link

Demo bug ? #31

Open yimengweima0 opened 4 days ago

yimengweima0 commented 4 days ago

Hello, thank you very much for your work. When I run your navigation file，Is it normal for the robot to shake that much? just like this https://github.com/user-attachments/assets/13234c0f-9ec3-4c40-a9b0-f74db425e92a

yimengweima0 commented 4 days ago

When I didn't change anything and just ported the code to Melodic, the robot still shake that much.

started roslaunch server http://localhost:37401/

SUMMARY

PARAMETERS

NODES / amcl (amcl/amcl) bumper2pointcloud (nodelet/nodelet) cmd_vel_mux (nodelet/nodelet) cnn_data_pub (drl_vo_nav/cnn_data_pub.py) drl_vo_cmd (drl_vo_nav/drl_vo_inference.py) gazebo (gazebo_ros/gzserver) gazebo_gui (gazebo_ros/gzclient) gazebo_map_broadcaster (tf/static_transform_publisher) goal_visualize (drl_vo_nav/goal_visualize.py) kobuki_safety_controller (nodelet/nodelet) map_server (map_server/map_server) mix_cmd_vel (drl_vo_nav/cmd_vel_pub.py) mobile_base_nodelet_manager (nodelet/nodelet) move_base (move_base/move_base) navigation_velocity_smoother (nodelet/nodelet) pedsim_simulator (pedsim_simulator/pedsim_simulator) pedsim_visualizer (pedsim_visualizer/pedsim_visualizer_node) pure_pursuit (drl_vo_nav/pure_pursuit.py) robot_pose_pub (drl_vo_nav/robot_pose_pub.py) robot_state_publisher (robot_state_publisher/robot_state_publisher) rviz (rviz/rviz) spawn_pedsim_agents (pedsim_gazebo_plugin/spawn_pedsim_agents.py) spawn_turtlebot_model (gazebo_ros/spawn_model) teleop_velocity_smoother (nodelet/nodelet) track_ped_pub (drl_vo_nav/track_ped_pub.py)

auto-starting new master process[master]: started with pid [45690] ROS_MASTER_URI=http://localhost:11311

setting /run_id to dc3799ee-979e-11ef-9d0b-6c2b59ced16c process[rosout-1]: started with pid [45701] started core service [/rosout] process[gazebo-2]: started with pid [45708] process[gazebo_gui-3]: started with pid [45712] process[spawn_pedsim_agents-4]: started with pid [45715] process[gazebo_map_broadcaster-5]: started with pid [45719] process[spawn_turtlebot_model-6]: started with pid [45720] process[mobile_base_nodelet_manager-7]: started with pid [45726] process[cmd_vel_mux-8]: started with pid [45727] process[bumper2pointcloud-9]: started with pid [45728] process[robot_state_publisher-10]: started with pid [45733] process[pedsim_simulator-11]: started with pid [45739] process[pedsim_visualizer-12]: started with pid [45741] process[map_server-13]: started with pid [45751] process[amcl-14]: started with pid [45821] process[navigation_velocity_smoother-15]: started with pid [45826] process[kobuki_safety_controller-16]: started with pid [45840] process[move_base-17]: started with pid [45848] process[pure_pursuit-18]: started with pid [45850] process[cnn_data_pub-19]: started with pid [45855] process[robot_pose_pub-20]: started with pid [45859] process[track_ped_pub-21]: started with pid [45863] process[goal_visualize-22]: started with pid [45870] process[teleop_velocity_smoother-23]: started with pid [45874] process[drl_vo_cmd-24]: started with pid [45875] process[mix_cmd_vel-25]: started with pid [45878] process[rviz-26]: started with pid [45886] [ INFO] [1730389405.397940821]: Updated sim with live config: Rate=25 incoming rate=25 [ WARN] [1730389405.441671571]: Request for map failed; trying again... [ INFO] [1730389405.461755803]: Using default queue size of 10 for publisher queues... [ INFO] [1730389405.494308871]: Loading scene [/home/lwh/G314/comparison/drl/src/pedsim_ros_with_gazebo/pedsim_simulator/scenarios/eng_hall.xml] for simulation [ INFO] [1730389405.506958719]: node initialized, now running Waiting for gazebo services... [ INFO] [1730389405.975796541]: Finished loading Gazebo ROS API Plugin. [ INFO] [1730389405.978790185]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting... [ INFO] [1730389406.044633624]: Finished loading Gazebo ROS API Plugin. [ INFO] [1730389406.046929053]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting... [WARN] [1730389406.206388, 0.000000]: Could not get robot pose [ INFO] [1730389410.311878218]: waitForService: Service [/gazebo/set_physics_properties] is now available. service: spawn_sdf_model is available .... [ INFO] [1730389410.429057053, 0.038000000]: Physics dynamic reconfigure ready. [WARN] [1730389410.572603, 0.175000]: Could not get robot pose [WARN] [1730389410.575567, 0.178000]: Could not get robot pose [ INFO] [1730389410.959635102, 0.195000000]: Camera Plugin: Using the 'robotNamespace' param: '/' [ INFO] [1730389410.977634731, 0.195000000]: Camera Plugin (ns = /) , set to "" [ INFO] [1730389411.468699952, 0.195000000]: Laser Plugin: Using the 'robotNamespace' param: '/' [ INFO] [1730389411.469191573, 0.195000000]: Starting Laser Plugin (ns = /) [ INFO] [1730389411.472849284, 0.195000000]: Laser Plugin (ns = /) , set to "" [INFO] [1730389411.490945, 0.195000]: Spawning model: actor_id = 1 [ INFO] [1730389411.501619113, 0.195000000]: Starting plugin Kobuki(ns = //) [ WARN] [1730389411.501678525, 0.195000000]: Kobuki(ns = //): missing default is na [ INFO] [1730389411.502623449, 0.195000000]: Kobuki(ns = //): = [ WARN] [1730389411.503093284, 0.195000000]: Kobuki(ns = //): missing default is 0 [ INFO] [1730389411.503242895, 0.195000000]: Will publish tf. [mobile_base] [ INFO] [1730389411.504946851, 0.195000000]: Kobuki(ns = //): Advertise joint_states[joint_states]! [ INFO] [1730389411.507038768, 0.195000000]: Kobuki(ns = //): Advertise Odometry[odom]! [ INFO] [1730389411.513458430, 0.195000000]: Kobuki(ns = //): Try to subscribe to mobile_base/commands/motor_power! [ INFO] [1730389411.521858760, 0.195000000]: Kobuki(ns = //): Try to subscribe to mobile_base/commands/reset_odometry! [ INFO] [1730389411.527048872, 0.195000000]: Kobuki(ns = //): Try to subscribe to mobile_base/commands/velocity! [ INFO] [1730389411.528497529, 0.195000000]: Kobuki(ns = //): Advertise Cliff[mobile_base/events/cliff]! [ INFO] [1730389411.529612108, 0.195000000]: Kobuki(ns = //): Advertise Bumper[mobile_base/events/bumper]! [ INFO] [1730389411.531546044, 0.195000000]: Kobuki(ns = //): Advertise Wheel Drop[mobile_base/events/wheel_drop]! [ INFO] [1730389411.532793264, 0.195000000]: Kobuki(ns = //): Advertise IMU[mobile_base/sensors/imu_data]! [ INFO] [1730389411.535054364, 0.195000000]: Kobuki(ns = //): Advertise Core[mobile_base/sensors/core]! [ INFO] [1730389411.535083079, 0.195000000]: GazeboRosKobuki plugin ready to go! [mobile_base] [WARN] [1730389411.558376, 0.215000]: Could not get robot pose [WARN] [1730389411.601817, 0.245000]: Could not get robot pose Finish loading model. [WARN] [1730389411.664254, 0.283000]: Could not get robot pose [WARN] [1730389411.706955, 0.312000]: Could not get robot pose [INFO] [1730389411.781595, 0.328000]: Spawning model: actor_id = 2 [spawn_turtlebot_model-6] process has finished cleanly log file: /home/lwh/.ros/log/dc3799ee-979e-11ef-9d0b-6c2b59ced16c/spawn_turtlebot_model-6.log [WARN] [1730389412.202959, 0.358000]: Could not get robot pose [WARN] [1730389412.232776, 0.378000]: Could not get robot pose [WARN] [1730389412.274166, 0.412000]: Could not get robot pose [WARN] [1730389412.320379, 0.445000]: Could not get robot pose [INFO] [1730389412.406822, 0.476000]: Spawning model: actor_id = 3 [WARN] [1730389412.473051, 0.494000]: Could not get robot pose [WARN] [1730389412.504045, 0.513000]: Could not get robot pose [WARN] [1730389412.552469, 0.545000]: Could not get robot pose [WARN] [1730389412.623035, 0.578000]: Could not get robot pose [INFO] [1730389412.688301, 0.589000]: Spawning model: actor_id = 4 [WARN] [1730389412.795517, 0.612000]: Could not get robot pose [WARN] [1730389412.840974, 0.645000]: Could not get robot pose [WARN] [1730389412.885783, 0.678000]: Could not get robot pose [INFO] [1730389412.980217, 0.687000]: Spawning model: actor_id = 5 [WARN] [1730389413.097689, 0.712000]: Could not get robot pose [WARN] [1730389413.154767, 0.745000]: Could not get robot pose [WARN] [1730389413.244941, 0.780000]: Could not get robot pose [INFO] [1730389413.274810, 0.780000]: Spawning model: actor_id = 6 [WARN] [1730389413.383611, 0.812000]: Could not get robot pose [WARN] [1730389413.432590, 0.846000]: Could not get robot pose [WARN] [1730389413.481586, 0.878000]: Could not get robot pose [INFO] [1730389413.555389, 0.905000]: Spawning model: actor_id = 7 [WARN] [1730389413.667586, 0.912000]: Could not get robot pose [INFO] [1730389413.834431, 0.929000]: Spawning model: actor_id = 8 [INFO] [1730389414.110235, 0.934000]: Spawning model: actor_id = 9 [INFO] [1730389414.416764, 0.939000]: Spawning model: actor_id = 10 [INFO] [1730389414.722641, 0.944000]: Spawning model: actor_id = 11 [WARN] [1730389414.799671, 0.945000]: Could not get robot pose [INFO] [1730389415.001166, 0.949000]: Spawning model: actor_id = 12 [INFO] [1730389415.281408, 0.953000]: Spawning model: actor_id = 13 [INFO] [1730389415.578950, 0.957000]: Spawning model: actor_id = 14 [INFO] [1730389415.871004, 0.961000]: Spawning model: actor_id = 15 [INFO] [1730389416.237311, 0.965000]: Spawning model: actor_id = 16 [INFO] [1730389416.557613, 0.969000]: Spawning model: actor_id = 17 [INFO] [1730389416.873631, 0.973000]: Spawning model: actor_id = 18 [INFO] [1730389417.182705, 0.977000]: Spawning model: actor_id = 19 [WARN] [1730389417.275303, 0.978000]: Could not get robot pose [INFO] [1730389417.465790, 0.981000]: Spawning model: actor_id = 20 [INFO] [1730389417.759074, 0.989000]: Spawning model: actor_id = 21 [INFO] [1730389418.050841, 0.997000]: Spawning model: actor_id = 22 [INFO] [1730389418.331231, 1.005000]: Spawning model: actor_id = 23 [WARN] [1730389418.525149, 1.012000]: Could not get robot pose [INFO] [1730389418.613298, 1.015000]: Spawning model: actor_id = 24 [INFO] [1730389418.900593, 1.026000]: Spawning model: actor_id = 25 [INFO] [1730389419.171192, 1.035000]: Spawning model: actor_id = 26 [INFO] [1730389419.455594, 1.042000]: Spawning model: actor_id = 27 [WARN] [1730389419.564170, 1.045000]: Could not get robot pose [INFO] [1730389419.727641, 1.050000]: Spawning model: actor_id = 28 [INFO] [1730389420.011492, 1.058000]: Spawning model: actor_id = 29 [INFO] [1730389420.285512, 1.067000]: Spawning model: actor_id = 30 [INFO] [1730389420.564861, 1.076000]: Spawning model: actor_id = 31 [WARN] [1730389420.649187, 1.078000]: Could not get robot pose [INFO] [1730389420.840931, 1.085000]: Spawning model: actor_id = 32 [INFO] [1730389421.111786, 1.094000]: Spawning model: actor_id = 33 [INFO] [1730389421.406721, 1.102000]: Spawning model: actor_id = 34 [WARN] [1730389421.777323, 1.112000]: Could not get robot pose [spawn_pedsim_agents-4] process has finished cleanly log file: /home/lwh/.ros/log/dc3799ee-979e-11ef-9d0b-6c2b59ced16c/spawn_pedsim_agents-4.log [WARN] [1730389422.845267, 1.146000]: Could not get robot pose [WARN] [1730389423.226335, 1.179000]: Could not get robot pose [WARN] [1730389423.581632, 1.212000]: Could not get robot pose [WARN] [1730389423.956079, 1.245000]: Could not get robot pose [WARN] [1730389424.322461, 1.287000]: Could not get robot pose [WARN] [1730389424.363764, 1.317000]: Could not get robot pose QObject::connect: Cannot queue arguments of type 'QVector' (Make sure 'QVector' is registered using qRegisterMetaType().) QObject::connect: Cannot queue arguments of type 'QVector' (Make sure 'QVector' is registered using qRegisterMetaType().) [ WARN] [1730389424.392123418, 1.337000000]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided [ WARN] [1730389424.394863380, 1.337000000]: global_costmap: Pre-Hydro parameter "map_type" unused since "plugins" is provided [ INFO] [1730389424.397197453, 1.347000000]: global_costmap: Using plugin "static_layer" [ INFO] [1730389424.414541111, 1.357000000]: Requesting the map... [ INFO] [1730389424.566460154, 1.467000000]: Resizing costmap to 1024 X 768 at 0.050000 m/pix [ INFO] [1730389424.679770352, 1.567000000]: Received a 1024 X 768 map at 0.050000 m/pix [ INFO] [1730389424.688993853, 1.567000000]: global_costmap: Using plugin "obstacle_layer" [ INFO] [1730389424.698312331, 1.577000000]: Subscribed to Topics: scan bump [ INFO] [1730389424.783875188, 1.647000000]: global_costmap: Using plugin "inflation_layer" [ WARN] [1730389424.891924611, 1.757000000]: local_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided [ WARN] [1730389424.893128168, 1.757000000]: local_costmap: Pre-Hydro parameter "map_type" unused since "plugins" is provided [ INFO] [1730389424.894434561, 1.757000000]: local_costmap: Using plugin "obstacle_layer" [ INFO] [1730389424.902974849, 1.757000000]: Subscribed to Topics: scan bump [ INFO] [1730389424.976042320, 1.827000000]: local_costmap: Using plugin "inflation_layer" [ INFO] [1730389425.041436125, 1.887000000]: Created local_planner dwa_local_planner/DWAPlannerROS [ INFO] [1730389425.048359901, 1.897000000]: Sim period is set to 0.20 [ INFO] [1730389425.241159064, 2.087000000]: Recovery behavior will clear layer 'obstacles' [ INFO] [1730389425.252043990, 2.097000000]: Recovery behavior will clear layer 'obstacles' [ INFO] [1730389425.399268784, 2.247000000]: odom received! [ WARN] [1730389428.297095056, 5.007000000]: Covariance matrix is not positive (semi-)definite in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0) [ WARN] [1730389433.927486600, 10.037000000]: Covariance matrix is not positive (semi-)definite in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0) [ WARN] [1730389439.461948374, 15.037000000]: Covariance matrix is not positive (semi-)definite in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0) [ INFO] [1730389440.482105724, 15.807000000]: Got new plan [ INFO] [1730389441.756424795, 16.807000000]: Got new plan

yimengweima0 commented 3 days ago

Could you please assist me in resolving them? I would really appreciate your guidance. Thank you!" @zzuxzt

zzuxzt commented 3 days ago

Modify lines 133-134 of drl_vo_inference.py as follows:: vz_min = -0.7 vz_max = 0.7

yimengweima0 commented 3 days ago

ok, thank you