moveit_core kinematics_constraints - addressing some issues

AcutronicRobotics · May 24, 2019 · beb3bca · beb3bca
1 parent d70f888
commit beb3bca
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 4 deletions.
diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h
@@ -200,6 +200,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
  *
  * @return was the construction successful?
  */
+ // TODO rework this function
 // bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints);
 }
 

diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -851,15 +851,14 @@ shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotSt
 }
 
 void VisibilityConstraint::getMarkers(const robot_state::RobotState& state,
-                                      visualization_msgs::msg::MarkerArray& markers)
+                                      visualization_msgs::msg::MarkerArray& markers) const
 {
   shapes::Mesh* m = getVisibilityCone(state);
   visualization_msgs::msg::Marker mk;
   shapes::constructMarkerFromShape(m, mk);
-  rclcpp::Time stamp = clock_ros_.now();
   delete m;
   mk.header.frame_id = robot_model_->getModelFrame();
-  mk.header.stamp = stamp;
+  mk.header.stamp = rclcpp::Clock().now();
   mk.ns = "constraints";
   mk.id = 1;
   mk.action = visualization_msgs::msg::Marker::ADD;

diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp
@@ -37,7 +37,6 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <geometric_shapes/solid_primitive_dims.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-//#include <moveit/utils/xmlrpc_casts.h>
 
 using namespace moveit::core;
 
@@ -276,6 +275,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
 
   return goal;
 }
+//TODO ROS 2: Rework these functions
+
 // TODO: Is not getting called from anywhere, check if is necessary
 // static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::msg::PoseStamped& pose)
 // {
-Original file line number
+Diff line change
@@ Expand Up @@
      *
      * @return was the construction successful?
      */
+     // TODO rework this function
     // bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints);
     }
@@ Expand Down @@