Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed possible nullpointer segfault #95

Merged
merged 2 commits into from
Aug 28, 2021
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 10 additions & 1 deletion src/moveit_visual_tools.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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()));
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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)
Expand Down