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 <[email protected]>

Author: werner291

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<moveit::core::RobotStatePtr>& 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<const moveit::core::JointModel*>& joints = robot_model_->getActiveJointModels();
+  const std::vector<const moveit::core::JointModel*>& joints = robot_state->getRobotModel()->getActiveJointModels();
 
   // Loop through joints
   for (std::size_t i = 0; i < joints.size(); ++i)