From 91cecd2ea23611d0f6be85a1085aa9ce634efc69 Mon Sep 17 00:00:00 2001 From: werner291 Date: Sat, 28 Aug 2021 23:38:35 +0200 Subject: [PATCH] Fixed possible nullpointer segfault (#95) * Fixed possible nullpointer segfault Fix for #94 * 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. Co-authored-by: v4hn --- src/moveit_visual_tools.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/moveit_visual_tools.cpp b/src/moveit_visual_tools.cpp index 0d62783..4a1476f 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())); @@ -1249,6 +1255,9 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory return false; } + // Ensure that the robot name is available. + loadSharedRobotState(); + // Create the message moveit_msgs::DisplayTrajectory display_trajectory_msg; display_trajectory_msg.model_id = robot_model_->getName(); @@ -1546,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)