Rviz published new points, and then local_planner died. #443
Description
My environment is Ubuntu 24.04 + ROS Jazzy. I tested the following code according to the wiki instructions:
ros2 launch quad_utils quad_gazebo.py
ros2 topic pub /robot_1/control/mode std_msgs/UInt8 "data: 1"
ros2 launch quad_utils quad_plan.py
It can start the Rviz, load GO2 and let it stand up and go to the default goal.
However, after completing the above steps, I encountered a bug when trying to publish points to the next target location using the RViz toolbar. After clicking on a location on the map, a red global planning route appeared, but the local_planner node crashed.
I tried adding the following code to local_footstep_planner.cpp, which is called by local_planner.cpp.
// Compute the midair index
int mid_air = std::round(i_liftoff + swing_duration / 2);
// DEBUGGING
RCLCPP_INFO(node_->get_logger(),
"Checking mid_air: mid_air = %d, i_liftoff = %d, swing_duration = %f, body_plan.rows = %ld",
mid_air, i_liftoff, swing_duration, body_plan.rows());
// Initialize variables
Eigen::VectorXd body_plan_mid_air;
double swing_apex;
if (mid_air < 0) {
// If the mid_air is something in previous, take it from the memory
swing_apex = most_recent_foothold_msg.velocity.z;
} else {
// Otherwise, update it using the latest plan
body_plan_mid_air = body_plan.row(mid_air); // **mid_air>>rows**
// Compute the swing apex
swing_apex = computeSwingApex(
j, body_plan_mid_air, foot_position_prev_nominal, foot_position_next);
// Update the memory, we borrow the past_footholds_msg since its velocity
// is unused
past_footholds_msg.feet[j].velocity.z = swing_apex;
}
The output of the terminal is
[local_planner_node-2] [INFO] [1762252391.506381814] [robot_1.local_planner]: Checking mid_air: mid_air = 171, i_liftoff = 330, swing_duration = -318.000000, body_plan.rows = 26
[local_planner_node-2] local_planner_node: /usr/include/eigen3/Eigen/src/Core/Block.h:120: Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Block(XprType&, Eigen::Index) [with XprType = const Eigen::Matrix<double, -1, -1>; int BlockRows = 1; int BlockCols = -1; bool InnerPanel = false; Eigen::Index = long int]: Assertion `(i>=0) && ( ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i<xpr.rows()) ||((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && i<xpr.cols()))' failed.
[global_body_planner_node-1] [INFO] [1762252391.508079731] [robot_1.global_body_planner]: GBP in refine mode
[body_force_estimator_node-3] [ERROR] [1762252392.214834022] [robot_1.body_force_estimator]: ID node couldn't find the correct ref state!
[ERROR] [local_planner_node-2]: process has died [pid 258951, exit code -6, cmd '/home/zhanghd/ros2_ws/install/local_planner/lib/local_planner/local_planner_node --ros-args -r __node:=local_planner -r __ns:=/robot_1 --params-file /home/zhanghd/ros2_ws/install/local_planner/share/local_planner/config/local_planner.yaml --params-file /home/zhanghd/ros2_ws/install/nmpc_controller/share/nmpc_controller/config/nmpc_controller.yaml --params-file /home/zhanghd/ros2_ws/install/local_planner/share/local_planner/config/local_planner_topics.yaml --params-file /home/zhanghd/ros2_ws/install/quad_utils/share/quad_utils/config/go2.yaml --params-file /tmp/launch_params_5wtbhzh4'].
[global_body_planner_node-1] [INFO] [1762252393.577377757] [robot_1.global_body_planner]: GBP in refine mode
[ERROR] [body_force_estimator_node-3]: process has died [pid 258952, exit code -11, cmd '/home/zhanghd/ros2_ws/install/body_force_estimator/lib/body_force_estimator/body_force_estimator_node --ros-args -r __node:=body_force_estimator -r __ns:=/robot_1 --params-file /home/zhanghd/ros2_ws/install/body_force_estimator/share/body_force_estimator/config/body_force_estimator.yaml --params-file /home/zhanghd/ros2_ws/install/body_force_estimator/share/body_force_estimator/config/body_force_estimator_topics.yaml --params-file /tmp/launch_params_rndflfjk'].
Note that, if mid_air>0, it maybe great than rows and lead to this bug. How to fix it?
Thank you!