PR changes

photoneo · Jul 7, 2021 · 40d913d · 40d913d
1 parent aa07491
commit 40d913d
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 9 deletions.
diff --git a/stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h b/stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h
@@ -15,10 +15,7 @@ typedef boost::variate_generator< RGNType, boost::uniform_real<> > RandomGenerat
 
 /**
  * @class stomp_moveit::noise_generators::CartesianConstraintsSampling
- * @brief This class generates noisy trajectories to an under-constrained cartesian goal pose.
- *
- * @par Examples:
- * All examples are located here @ref stomp_plugins_examples
+ * @brief This class generates noisy trajectories to an under-constrained cartesian waypoints.
  *
  */
 class CartesianConstraintsSampling: public StompNoiseGenerator

diff --git a/stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp b/stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp
@@ -96,6 +96,14 @@ bool CartesianConstraintsSampling::configure(const XmlRpc::XmlRpcValue& config)
 
     // noise generation parameters
     XmlRpcValue stddev_param = params["stddev"];
+    tool_link_ = static_cast<std::string>(params["link_id"]);
+
+    // Check link_id
+    if (tool_link_.empty())
+    {
+      ROS_ERROR("%s the 'link_id' parameter is empty",getName().c_str());
+      return false;
+    }
 
     // check  stddev
     if(stddev_param.size() != stddev_.size())
@@ -124,9 +132,8 @@ bool CartesianConstraintsSampling::setMotionPlanRequest(const planning_scene::Pl
                  const stomp_core::StompConfiguration &config,
                  moveit_msgs::MoveItErrorCodes& error_code)
 {
-  bool succeed = setupNoiseGeneration(planning_scene,req,config,error_code) &&
+  return setupNoiseGeneration(planning_scene,req,config,error_code) &&
           setupRobotState(planning_scene,req,config,error_code);
-  return succeed;
 }
 
 bool CartesianConstraintsSampling::setupNoiseGeneration(const planning_scene::PlanningSceneConstPtr& planning_scene,
@@ -176,8 +183,6 @@ bool CartesianConstraintsSampling::setupRobotState(const planning_scene::Plannin
   using namespace utils::kinematics;
 
   // robot state
-  const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_);
-  tool_link_ = joint_group->getLinkModelNames().back();
   state_.reset(new RobotState(robot_model_));
   robotStateMsgToRobotState(req.start_state,*state_);
 
@@ -241,7 +246,7 @@ Eigen::Affine3d CartesianConstraintsSampling::applyCartesianNoise(const Eigen::V
   state_->setJointGroupPositions(joint_group,reference_joint_pose);
   Eigen::Affine3d tool_pose = state_->getFrameTransform(tool_link_);
 
-  auto& n = raw_noise_;
+  const auto& n = raw_noise_;
   return tool_pose * Translation3d(Vector3d(n(0),n(1),n(2)))*AngleAxisd(n(3),Vector3d::UnitX())*AngleAxisd(n(4),Vector3d::UnitY())*AngleAxisd(n(5),Vector3d::UnitZ());
 }