jchengai/gpir

[ICRA'22] Real-Time Trajectory Planning for Autonomous Driving with Gaussian Process and Incremental Refinement

gpir's Issues

terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'

Hi, I try to run the demo code and I meet a crash issue, the detail is below. The scenario is that adc goes straight and the obstacle left turn. I notice the lat_acc is very large, and gtsam library report Thrown when a linear system is ill-posed. The most common cause for this error is having underconstrained variables. I am not familiar with the gtsam, just want to ask how to solve this crash issue.

ICRA 2022 presentation

Hello authors, thanks for open-sourcing your nice work!
Are there any records of your icra2022 presentation available?
(I can only find the related info like: https://events.infovaya.com/presentation?id=79657

cannot find map file

Hello, when run your demo, it complains that could not find file "XXX/hdmap/map/pcd/Town05.pcd". and then cannot receive ego info from carla.

I didn't find this file in the repo. Do I miss something?

The authors should consider citing the related sources

The visitors may spend some time comparing the following references with this repo:

1. Many modules of this module are copied from Apollo. For example, the spline module in this repo is almost identical to that in Apollo(Smooth Spline), but the authors just deleted the copyright notice of apollo and copied the codes from apollo.

2. EPSILON Open Source Code vs The code in this repo
   (also EUDM Paper and SSC(2019) Youtube vs the visualization and video of this repo)

============================ VS =====================================================
============================ VS =====================================================
============================ VS =====================================================

It is ok to refer to other sources when doing research. But it may be inappropriate for the authors of this repo to delete the copyright notice of the related sources and not acknowledge the most related ones, e.g., Apollo and EPSILON.

planing module is not runing

process[planning-1]: started with pid [4933]
process[carla_controller-2]: started with pid [4934]
process[planning_rviz-3]: started with pid [4935]
process[vehicle-4]: started with pid [4940]
double free or corruption (out)
[planning-1] process has died [pid 4933, exit code -6, cmd /home/wen/catkin_ww/devel/lib/planning_core/planning __name:=planning __log:=/home/wen/.ros/log/74d56960-0ce6-11ed-9422-8cc681d13e0e/planning-1.log].
log file: /home/wen/.ros/log/74d56960-0ce6-11ed-9422-8cc681d13e0e/planning-1*.log
[planning_rviz-3] process has finished cleanly
log file: /home/wen/.ros/log/74d56960-0ce6-11ed-9422-8cc681d13e0e/planning_rviz-3*.log

It is still not possible to run planing module alone, and the following error appears. I hope to get your answer.

~/catkin_ww$ rosrun planning_core planning
double free or corruption (out)
Aborted (core dumped)

Process died after roslaunch

Hi,
when i run roslaunch planning_core planning.launch , the process just died.

log:

SUMMARY

PARAMETERS

• /carla_controller/kd: 0.4
• /carla_controller/ki: 0.15
• /carla_controller/kp: 3.5
• /planning/map_path: /home/xfy/catkin_...
• /planning/random_drive_mode: True
• /planning/simulator: Carla
• /planning/town: Town05
• /robot_description: <?xml version="1....
• /rosdistro: noetic
• /rosversion: 1.15.14

NODES
/
carla_controller (planning_core/carla_controller)
planning (planning_core/planning)
planning_rviz (rviz/rviz)
vehicle (robot_state_publisher/robot_state_publisher)

ROS_MASTER_URI=http://localhost:11311

process[planning-1]: started with pid [26740]
process[carla_controller-2]: started with pid [26741]
process[planning_rviz-3]: started with pid [26749]
process[vehicle-4]: started with pid [26750]
[2022-07-24 15:12:30.995] [info] ensureValid(::ad::physics::Distance)>> 1.4224e+10 value out of range
terminate called after throwing an instance of 'std::out_of_range'
what(): Distance value out of range
*** Aborted at 1658646750 (unix time) try "date -d @1658646750" if you are using GNU date ***
PC: @ 0x7f375ad3d00b gsignal
*** SIGABRT (@0x3e800006874) received by PID 26740 (TID 0x7f375546c980) from PID 26740; stack trace: ***
@ 0x7f375ad3d090 (unknown)
@ 0x7f375ad3d00b gsignal
@ 0x7f375ad1c859 abort
@ 0x7f375afa5911 (unknown)
@ 0x7f375afb138c (unknown)
@ 0x7f375afb13f7 std::terminate()
@ 0x7f375afb16a9 __cxa_throw
@ 0x7f375a1691fe ad::physics::Distance::ensureValid()
@ 0x7f375a1a6f6a ad::map::point::findNearestPointOnEdge<>()
@ 0x7f375a1a7c3a ad::map::point::findNearestPointOnSegment<>()
@ 0x7f375a1a8340 ad::map::point::findNearestPointOnEdge<>()
@ 0x7f375a1a8b4f ad::map::point::calculateWidthRange<>()
@ 0x7f375a1a4878 ad::map::lane::updateLaneLengths()
@ 0x7f375a13a3c8 ad::map::access::Factory::set()
@ 0x7f375a1be8d0 ad::map::opendrive::AdMapFactory::addLane()
@ 0x7f375a1c00dc ad::map::opendrive::AdMapFactory::convertToAdMap()
@ 0x7f375a1c0877 ad::map::opendrive::AdMapFactory::createAdMap()
@ 0x7f375a1c0de0 ad::map::opendrive::AdMapFactory::createAdMap()
@ 0x7f375a1284b3 ad::map::access::AdMapAccess::readOpenDriveMap()
@ 0x7f375a129875 ad::map::access::AdMapAccess::initialize()
@ 0x7f375b3ece0c hdmap::HdMapImpl::LoadMap()
@ 0x563a8e5be595 planning::PlanningCore::Init()
@ 0x563a8e5bd4e8 main
@ 0x7f375ad1e083 __libc_start_main
@ 0x563a8e5bd63e _start
[planning-1] process has died

Error occured when roslaunch keyboard.launch

Hi, Thank a lot for open sourcing this great work!
When attempting to apply this work, I encounter some difficulty and hope to get some suggestions.

When I try roslaunch planning_core planning.launch, I get the following:

SUMMARY

PARAMETERS

• /carla_controller/kd: 0.4
• /carla_controller/ki: 0.15
• /carla_controller/kp: 3.5
• /planning/map_path: /home/luoteng/cat...
• /planning/random_drive_mode: True
• /planning/simulator: Carla
• /planning/town: Town05
• /robot_description: <?xml version="1....
• /rosdistro: noetic
• /rosversion: 1.15.14

NODES
/
carla_controller (planning_core/carla_controller)
planning (planning_core/planning)
planning_rviz (rviz/rviz)
vehicle (robot_state_publisher/robot_state_publisher)

ROS_MASTER_URI=http://localhost:11311

process[planning-1]: started with pid [644605]
process[carla_controller-2]: started with pid [644606]
process[planning_rviz-3]: started with pid [644607]
process[vehicle-4]: started with pid [644613]
[pcl::PCDReader::read] Could not find file '/home/luoteng/catkin_gpir/src/gpir/hdmap/maps/pcd/Town05.pcd'.
Fail to load pcd file: /home/luoteng/catkin_gpir/src/gpir/hdmap/maps/pcd/Town05.pcd
W1127 16:04:53.694373 644605 carla_ego_info.cc:87] cannot receive ego info from carla
W1127 16:04:53.694741 644605 carla_adapter.cc:40] fail to get vehicle param from Carla, Retry: 0
I1127 16:04:55.695619 644605 carla_adapter.cc:37]
length: 4.79178
width: 2.16345
height: 1.48832
wheel_base: 3.00465
front_to_front_axle: 0.777641
back_to_rear_axle: 1.00949
rear_axle_to_center: 1.3864
I1127 16:04:55.695652 644605 planning_core.cc:45] Init simulator Carla OK

And when I try roslaunch planning_core keyboard.launch, I get the following:

SUMMARY

PARAMETERS

• /rosdistro: noetic
• /rosversion: 1.15.14

NODES
/
keyboard (planning_core/keyboard.py)

ROS_MASTER_URI=http://localhost:11311

process[keyboard-1]: started with pid [645243]
pygame 2.1.2 (SDL 2.0.16, Python 3.7.15)
Hello from the pygame community. https://www.pygame.org/contribute.html
Traceback (most recent call last):
File "/home/luoteng/catkin_gpir/src/gpir/planning_core/keyboard.py", line 321, in
main()
File "/home/luoteng/catkin_gpir/src/gpir/planning_core/keyboard.py", line 307, in main
keyboard_handler = KeyboardHandler(args)
File "/home/luoteng/catkin_gpir/src/gpir/planning_core/keyboard.py", line 170, in init
self.agent_generator = AgentGenerator(args)
File "/home/luoteng/catkin_gpir/src/gpir/planning_core/keyboard.py", line 53, in init
self.ego = actors[0]
IndexError: vector::_M_range_check: __n (which is 0) >= this->size() (which is 0)
[keyboard-1] process has died [pid 645243, exit code 1, cmd /home/luoteng/catkin_gpir/src/gpir/planning_core/keyboard.py --img=/home/luoteng/catkin_gpir/src/gpir/planning_core/keyboard.png __name:=keyboard __log:=/home/luoteng/.ros/log/b1045c60-6a5c-11ed-997a-434582c5cbfd/keyboard-1.log].
log file: /home/luoteng/.ros/log/b1045c60-6a5c-11ed-997a-434582c5cbfd/keyboard-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

I am tring to solve the encountered problem, and I may wonder if there is something I missed that leads to the situation, or someone have met the similar situation. Thanks again for the sharing of the excellent work.

Unable to launch roslaunch planning_core planning.launch successfully

Hi, Many Thanks for open sourcing great work. I have couple of indicators which I am unable to solve after a lot of time spent. could you please help if you have any ideas.

1. When I do catkin_make -DCMAKE_BUILD_TYPE=RELEASE -j4 then I am getting following error. I am wondering why when the opencv-python is successfully installed.
   fatal error: opencv2/core/core.hpp: No such file or directory
   15 | #include <opencv2/core/core.hpp>
2. Though I have the rosbridge setup, but when I do roslaunch planning_core planning.launch I dont see any lane lines or other cars except ego vehicle and that too stationary. Could you please help me with some hints.

for your reference below output from roslaunch planning_core planning.launch:
carla-ros-bridge/catkin_ws$ roslaunch planning_core planning.launch
... logging to /home/manasvi/.ros/log/b47813ce-4265-11ed-b005-5fb570d486ce/roslaunch-manasvi-PC-3628.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.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
started roslaunch server http://manasvi-PC:35483/

SUMMARY

PARAMETERS

• /carla_controller/kd: 0.4
• /carla_controller/ki: 0.15
• /carla_controller/kp: 3.5
• /planning/map_path: /home/manasvi/car...
• /planning/random_drive_mode: True
• /planning/simulator: Carla
• /planning/town: Town05
• /robot_description: <?xml version="1....
• /rosdistro: noetic
• /rosversion: 1.15.14

NODES
/
carla_controller (planning_core/carla_controller)
planning (planning_core/planning)
planning_rviz (rviz/rviz)
vehicle (robot_state_publisher/robot_state_publisher)

ROS_MASTER_URI=http://localhost:11311

ERROR: cannot launch node of type [planning_core/planning]: Cannot locate node of type [planning] in package [planning_core]. Make sure file exists in package path and permission is set to executable (chmod +x)
process[carla_controller-2]: started with pid [3651]
process[planning_rviz-3]: started with pid [3652]
process[vehicle-4]: started with pid [3654]

output from roslaunch planning_core carla_setup.launch
... logging to /home/manasvi/.ros/log/b47813ce-4265-11ed-b005-5fb570d486ce/roslaunch-manasvi-PC-3317.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://manasvi-PC:36051/

SUMMARY

PARAMETERS

• /carla_manual_control_ego_vehicle/role_name: ego_vehicle
• /carla_ros_bridge/ego_vehicle_role_name: ["hero", "ego_veh...
• /carla_ros_bridge/fixed_delta_seconds: 0.02
• /carla_ros_bridge/host: localhost
• /carla_ros_bridge/passive:
• /carla_ros_bridge/port: 2000
• /carla_ros_bridge/register_all_sensors: True
• /carla_ros_bridge/synchronous_mode: True
• /carla_ros_bridge/synchronous_mode_wait_for_vehicle_control_command: False
• /carla_ros_bridge/timeout: 10
• /carla_ros_bridge/town: /home/manasvi/car...
• /carla_spawn_objects_manasvi_PC_3317_8527585904025881905/objects_definition_file: /home/manasvi/car...
• /carla_spawn_objects_manasvi_PC_3317_8527585904025881905/spawn_point_ego_vehicle: 73.44,-205.43,1,0...
• /carla_spawn_objects_manasvi_PC_3317_8527585904025881905/spawn_sensors_only: False
• /rosdistro: noetic
• /rosversion: 1.15.14
• /set_initial_pose_ego_vehicle/control_id: control
• /set_initial_pose_ego_vehicle/role_name: ego_vehicle
• /use_sim_time: True

NODES
/
carla_manual_control_ego_vehicle (carla_manual_control/carla_manual_control.py)
carla_ros_bridge (carla_ros_bridge/bridge.py)
carla_spawn_objects_manasvi_PC_3317_8527585904025881905 (carla_spawn_objects/carla_spawn_objects.py)
carla_tf (planning_core/carla_tf)
set_initial_pose_ego_vehicle (carla_spawn_objects/set_initial_pose.py)

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

setting /run_id to b47813ce-4265-11ed-b005-5fb570d486ce
process[rosout-1]: started with pid [3349]
started core service [/rosout]
process[carla_ros_bridge-2]: started with pid [3357]
process[carla_spawn_objects_manasvi_PC_3317_8527585904025881905-3]: started with pid [3358]
process[set_initial_pose_ego_vehicle-4]: started with pid [3359]
process[carla_manual_control_ego_vehicle-5]: started with pid [3360]
process[carla_tf-6]: started with pid [3361]
pygame 2.1.2 (SDL 2.0.16, Python 3.7.13)
Hello from the pygame community. https://www.pygame.org/contribute.html
[INFO] [1664724013.756730, 0.000000]: Trying to connect to localhost:2000
[INFO] [1664724013.761823, 0.000000]: Loading opendrive world from file '/home/manasvi/carla-ros-bridge/catkin_ws/src/gpir/hdmap/maps/opendrive/Town05.xodr'
[INFO] [1664724016.554498, 0.000000]: synchronous_mode: True
[INFO] [1664724016.556426, 0.000000]: fixed_delta_seconds: 0.02
[INFO] [1664724016.626248, 0.000000]: Parameters:
[INFO] [1664724016.629431, 0.000000]: host: localhost
[INFO] [1664724016.631720, 0.000000]: port: 2000
[INFO] [1664724016.633436, 0.000000]: timeout: 10
[INFO] [1664724016.635184, 0.000000]: passive:
[INFO] [1664724016.638046, 0.000000]: synchronous_mode: True
[INFO] [1664724016.642200, 0.000000]: synchronous_mode_wait_for_vehicle_control_command: False
[INFO] [1664724016.644830, 0.000000]: fixed_delta_seconds: 0.02
[INFO] [1664724016.648138, 0.000000]: register_all_sensors: True
[INFO] [1664724016.651010, 0.000000]: town: /home/manasvi/carla-ros-bridge/catkin_ws/src/gpir/hdmap/maps/opendrive/Town05.xodr
[INFO] [1664724016.654036, 0.000000]: ego_vehicle: {'role_name': '["hero", "ego_vehicle", "hero0", "hero1", "hero2", "hero3", "hero4", "hero5", "hero6", "hero7", "hero8", "hero9"]'}
[INFO] [1664724017.238757, 0.000000]: Created Spectator(id=150)
[
.....
[INFO] [1664724017.366481, 0.000000]: Object (type='sensor.pseudo.traffic_lights', id='traffic_lights') spawned successfully as 10000.
[INFO] [1664724017.367463, 0.000000]: Created TrafficLight(id=184)
[INFO] [1664724017.368878, 0.000000]: Created TrafficLight(id=185)
[INFO] [1664724017.370593, 0.000000]: Object (type='sensor.pseudo.objects', id='objects') spawned successfully as 10001.
[INFO] [1664724017.370785, 0.000000]: Created TrafficLight(id=186)
[INFO] [1664724017.372992, 0.000000]: Created TrafficLight(id=187)
[INFO] [1664724017.375771, 0.000000]: Created TrafficLight(id=188)
[INFO] [1664724017.376672, 0.000000]: Object (type='sensor.pseudo.actor_list', id='actor_list') spawned successfully as 10002.
[INFO] [1664724017.378023, 0.000000]: Created TrafficLight(id=189)
[INFO] [1664724017.380032, 0.000000]: Created TrafficLight(id=190)
[INFO] [1664724017.381251, 0.000000]: Object (type='sensor.pseudo.markers', id='markers') spawned successfully as 10003.
[INFO] [1664724017.384470, 0.000000]: Spawn point from ros parameters
[INFO] [1664724017.390499, 0.000000]: Created TrafficLight(id=191)
[INFO] [1664724017.393760, 0.000000]: Created TrafficLight(id=192)
[INFO] [1664724017.395912, 0.000000]: Created TrafficLight(id=193)
[INFO] [1664724017.398227, 0.000000]: Created TrafficLight(id=194)
[INFO] [1664724017.399329, 0.000000]: Object (type='vehicle.tesla.model3', id='ego_vehicle') spawned successfully as 210.
[INFO] [1664724017.401632, 0.000000]: Created TrafficLight(id=195)
[INFO] [1664724017.403616, 0.000000]: Created TrafficLight(id=196)
[INFO] [1664724017.405358, 0.000000]: Created TrafficLight(id=197)
[INFO] [1664724017.407894, 0.000000]: Created TrafficLight(id=198)
[INFO] [1664724017.410006, 0.000000]: Created TrafficLight(id=199)
[INFO] [1664724017.411203, 0.000000]: Object (type='sensor.camera.rgb', id='rgb_view') spawned successfully as 211.
[INFO] [1664724017.435630, 0.000000]: Created TrafficLight(id=209)
[INFO] [1664724017.440400, 0.000000]: Object (type='sensor.other.lane_invasion', id='lane_invasion') spawned successfully as 213.
[INFO] [1664724017.442550, 0.000000]: Created TrafficLightsSensor(id=10000)
[INFO] [1664724017.445724, 0.000000]: Created ObjectSensor(id=10001)
[INFO] [1664724017.448721, 0.000000]: Created ActorListSensor(id=10002)
[INFO] [1664724017.475168, 0.000000]: Created MarkerSensor(id=10003)
[INFO] [1664724017.489951, 0.000000]: Created EgoVehicle(id=210)
[INFO] [1664724017.512054, 0.000000]: Created RgbCamera(id=211)
[INFO] [1664724017.526733, 0.000000]: Created ActorControl(id=10004)
[INFO] [1664724017.557877, 0.000000]: Created CollisionSensor(id=212)
[INFO] [1664724017.836328, 0.000000]: Created LaneInvasionSensor(id=213)
[INFO] [1664724017.838608, 0.000000]: Object (type='sensor.pseudo.tf', id='tf') spawned successfully as 10005.
[INFO] [1664724017.843218, 0.000000]: Object (type='sensor.pseudo.odom', id='odometry') spawned successfully as 10006.
[INFO] [1664724017.845052, 0.000000]: All objects spawned.
[INFO] [1664724017.950256, 2.287129]: Created TFSensor(id=10005)
[INFO] [1664724017.957978, 2.287129]: Created OdometrySensor(id=10006)