From 2f74aa6105b6ffea2de0a75de2661e0d7d7be219 Mon Sep 17 00:00:00 2001 From: werner291 Date: Tue, 20 Jul 2021 11:08:54 +0200 Subject: [PATCH 1/2] Fixed possible nullpointer segfault Fix for #94 --- src/moveit_visual_tools.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/moveit_visual_tools.cpp b/src/moveit_visual_tools.cpp index 445f202..113e0b5 100644 --- a/src/moveit_visual_tools.cpp +++ b/src/moveit_visual_tools.cpp @@ -1248,6 +1248,9 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to publish trajectory path because trajectory has zero points"); return false; } + + // Ensure that the robot name is available. + loadSharedRobotState(); // Create the message moveit_msgs::DisplayTrajectory display_trajectory_msg; From 8fd2ac616487f383b309096c34e1652cf40e38cd Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 11 Aug 2021 20:14:50 +0200 Subject: [PATCH 2/2] followup: safeguard more uses The last one forces a valid RobotState through the method API, so we can get the RobotModel from there without loading anything else. --- src/moveit_visual_tools.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/moveit_visual_tools.cpp b/src/moveit_visual_tools.cpp index 113e0b5..d9c96d8 100644 --- a/src/moveit_visual_tools.cpp +++ b/src/moveit_visual_tools.cpp @@ -1145,6 +1145,9 @@ bool MoveItVisualTools::publishContactPoints(const collision_detection::Collisio bool MoveItVisualTools::publishTrajectoryPoint(const trajectory_msgs::JointTrajectoryPoint& trajectory_pt, const std::string& planning_group, double display_time) { + // Ensure robot_model_ is available + loadSharedRobotState(); + // Get joint state group const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group); @@ -1171,6 +1174,9 @@ bool MoveItVisualTools::publishTrajectoryPoint(const trajectory_msgs::JointTraje bool MoveItVisualTools::publishTrajectoryPath(const std::vector& trajectory, const moveit::core::JointModelGroup* jmg, double speed, bool blocking) { + // Ensure robot_model_ is available + loadSharedRobotState(); + // Copy the vector of RobotStates to a RobotTrajectory robot_trajectory::RobotTrajectoryPtr robot_trajectory( new robot_trajectory::RobotTrajectory(robot_model_, jmg->getName())); @@ -1248,7 +1254,7 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to publish trajectory path because trajectory has zero points"); return false; } - + // Ensure that the robot name is available. loadSharedRobotState(); @@ -1549,7 +1555,7 @@ bool MoveItVisualTools::hideRobot() void MoveItVisualTools::showJointLimits(const moveit::core::RobotStatePtr& robot_state) { - const std::vector& joints = robot_model_->getActiveJointModels(); + const std::vector& joints = robot_state->getRobotModel()->getActiveJointModels(); // Loop through joints for (std::size_t i = 0; i < joints.size(); ++i)