recordreplay_planner died when following trajectory
Description
When trying trajectory following, the recordreplay_planner
died with the following messages.
[INFO] [planning.recordreplay_planner]: Replaying recorded ego postion as trajectory
recordreplay_planner_node_exe: /tmp/binarydeb/ros-dashing-cyclonedds-0.5.1/src/core/ddsc/src/dds_whc.c:341: remove_whcn_from_hash: Assertion `0' failed.
Thread 1 "recordreplay_pl" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
51 ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
My gdb debugging shows below.
(gdb) bt
#0 __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1 0x00007fe2da3418b1 in __GI_abort () at abort.c:79
#2 0x00007fe2da33142a in __assert_fail_base (fmt=0x7fe2da4b8a38 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", assertion=assertion@entry=0x7fe2d6fcd1de "0", file=file@entry=0x7fe2d6fcdff8 "/tmp/binarydeb/ros-dashing-cyclonedds-0.5.1/src/core/ddsc/src/dds_whc.c", line=line@entry=341,
function=function@entry=0x7fe2d6fcef50 "remove_whcn_from_hash") at assert.c:92
#3 0x00007fe2da3314a2 in __GI___assert_fail (assertion=0x7fe2d6fcd1de "0", file=0x7fe2d6fcdff8 "/tmp/binarydeb/ros-dashing-cyclonedds-0.5.1/src/core/ddsc/src/dds_whc.c", line=341, function=0x7fe2d6fcef50 "remove_whcn_from_hash") at assert.c:101
#4 0x00007fe2d6fa6d43 in ?? () from /opt/ros/dashing/lib/libddsc.so.0
#5 0x00007fe2d6fa70c4 in ?? () from /opt/ros/dashing/lib/libddsc.so.0
#6 0x00007fe2d6fa7af4 in ?? () from /opt/ros/dashing/lib/libddsc.so.0
#7 0x00007fe2d6fa91d4 in ?? () from /opt/ros/dashing/lib/libddsc.so.0
#8 0x00007fe2d6f88bc9 in ?? () from /opt/ros/dashing/lib/libddsc.so.0
#9 0x00007fe2d6fa61f7 in dds_write_impl () from /opt/ros/dashing/lib/libddsc.so.0
#10 0x00007fe2d6fa638b in dds_write () from /opt/ros/dashing/lib/libddsc.so.0
#11 0x00007fe2d7212035 in rmw_publish () from /opt/ros/dashing/lib/librmw_cyclonedds_cpp.so
#12 0x00007fe2d9d405ef in rcl_publish () from /opt/ros/dashing/lib/librcl.so
#13 0x00007fe2d51fae70 in rcl_action_publish_feedback () from /opt/ros/dashing/lib/librcl_action.so
#14 0x00007fe2d54132c1 in rclcpp_action::ServerBase::publish_feedback(std::shared_ptr<void>) () from /opt/ros/dashing/lib/librclcpp_action.so
#15 0x00007fe2d60f2ab5 in rclcpp_action::Server<recordreplay_planner_actions::action::ReplayTrajectory>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_t>, std::array<unsigned char, 16ul>, std::shared_ptr<void>)::{lambda(std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >)#3}::operator()(std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >) const (
feedback_msg=std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> >> (empty) = {...}, __closure=<optimized out>) at /opt/ros/dashing/include/rclcpp_action/server.hpp:416
#16 std::_Function_handler<void (std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >), rclcpp_action::Server<recordreplay_planner_actions::action::ReplayTrajectory>::call_goal_accepted_callback(std::shared_ptr<rcl_action_goal_handle_t>, std::array<unsigned char, 16ul>, std::shared_ptr<void>)::{lambda(std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >)#3}>::_M_invoke(std::_Any_data const&, std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >&&) (__functor=..., __args#0=...) at /usr/include/c++/7/bits/std_function.h:316
#17 0x00007fe2d60f5466 in std::function<void (std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >)>::operator()(std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> > >) const (
this=<optimized out>, __args#0=std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_FeedbackMessage_<std::allocator<void> >> (empty) = {...}) at /usr/include/c++/7/bits/std_function.h:706
#18 0x00007fe2d60fb02d in rclcpp_action::ServerGoalHandle<recordreplay_planner_actions::action::ReplayTrajectory>::publish_feedback (this=0x5631eb702870,
feedback_msg=std::shared_ptr<recordreplay_planner_actions::action::ReplayTrajectory_Feedback_<std::allocator<void> >> (use count 2, weak count 0) = {...}) at /opt/ros/dashing/include/rclcpp_action/server_goal_handle.hpp:154
#19 0x00007fe2d60e8ebc in motion::planning::recordreplay_planner_node::RecordReplayPlannerNode::on_ego (this=0x5631eb5f8b10, msg=std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >> (use count 4, weak count 0) = {...})
at /home/horibe/AutowareAuto/src/motion/planning/recordreplay_planner_node/src/recordreplay_planner_node/recordreplay_planner_node.cpp:161
#20 0x00007fe2d60e90be in motion::planning::recordreplay_planner_node::RecordReplayPlannerNode::<lambda(autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >::SharedPtr)>::operator() (
msg=std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >> (empty) = {...}, __closure=<optimized out>) at /home/horibe/AutowareAuto/src/motion/planning/recordreplay_planner_node/src/recordreplay_planner_node/recordreplay_planner_node.cpp:101
#21 std::_Function_handler<void(std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> > >), motion::planning::recordreplay_planner_node::RecordReplayPlannerNode::init(const string&, const string&, const string&, const motion::motion_common::VehicleConfig&, autoware::common::types::float64_t, autoware::common::types::float64_t)::<lambda(autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >::SharedPtr)> >::_M_invoke(const std::_Any_data &, std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> > > &&) (__functor=...,
__args#0=...) at /usr/include/c++/7/bits/std_function.h:316
#22 0x00007fe2d60ff552 in std::function<void (std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> > >)>::operator()(std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> > >) const (this=this@entry=0x5631eb660750,
__args#0=std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >> (empty) = {...}) at /usr/include/c++/7/bits/std_function.h:706
#23 0x00007fe2d60ffcb9 in rclcpp::AnySubscriptionCallback<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >, std::allocator<void> >::dispatch (message_info=..., message=std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >> (empty) = {...},
this=0x5631eb660750) at /opt/ros/dashing/include/rclcpp/any_subscription_callback.hpp:159
#24 rclcpp::Subscription<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >, std::allocator<void> >::handle_message (this=0x5631eb6606c0, message=std::shared_ptr<void> (use count 4, weak count 0) = {...}, message_info=...) at /opt/ros/dashing/include/rclcpp/subscription.hpp:146
#25 0x00007fe2daf2d33f in rclcpp::executor::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/dashing/lib/librclcpp.so
#26 0x00007fe2daf2e385 in rclcpp::executor::Executor::execute_any_executable(rclcpp::executor::AnyExecutable&) () from /opt/ros/dashing/lib/librclcpp.so
#27 0x00007fe2daf3535f in rclcpp::executors::SingleThreadedExecutor::spin() () from /opt/ros/dashing/lib/librclcpp.so
(gdb) frame 19
#19 0x00007fe2d60e8ebc in motion::planning::recordreplay_planner_node::RecordReplayPlannerNode::on_ego (this=0x5631eb5f8b10,
msg=std::shared_ptr<autoware_auto_msgs::msg::VehicleKinematicState_<std::allocator<void> >> (use count 4, weak count 0) = {...})
at /home/horibe/AutowareAuto/src/motion/planning/recordreplay_planner_node/src/recordreplay_planner_node/recordreplay_planner_node.cpp:161
161 m_replaygoalhandle->publish_feedback(feedback_msg);
(gdb) p *feedback_msg
$1 = {remaining_length = 100}
(gdb) p m_replaygoalhandle
$8 = std::shared_ptr<rclcpp_action::ServerGoalHandle<recordreplay_planner_actions::action::ReplayTrajectory>> (use count 1, weak count 1) = {get() = 0x5631eb702870}
How to Reproduce
Launch the LGSVL with
$ source /opt/AutowareAuto/setup.bash
$ /opt/lgsvl/simulator
Run the ms2.launch
, or trajectory_recording.launch
following here, then send action for recording and replaying to the recordreplay_planner
.