48 #include <boost/lexical_cast.hpp>
49 #include <boost/math/constants/constants.hpp>
53 #include <Eigen/Geometry>
59 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
60 : LockedRobotState(initial_robot_state)
61 , name_(fixName(
name))
65 , display_meshes_(true)
66 , display_controls_(true)
71 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
73 , name_(fixName(
name))
77 , display_meshes_(true)
78 , display_controls_(true)
84 std::replace(
name.begin(),
name.end(),
'_',
'-');
157 std::map<std::string, geometry_msgs::PoseStamped>::iterator it =
pose_map_.find(vj.
joint_name);
203 const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
220 const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
222 if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
225 geometry_msgs::PoseStamped tpose;
226 geometry_msgs::Pose offset;
228 offset.orientation.w = 1;
243 updateStateEndEffector(*state, eef, pose, callback);
252 const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
254 if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
257 geometry_msgs::PoseStamped tpose;
258 geometry_msgs::Pose offset;
260 offset.orientation.w = 1;
275 updateStateJoint(*state, vj, pose, callback);
285 const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback,
292 cb(handler, error_state_changed);
298 const geometry_msgs::Pose& pose, StateChangeCallbackFn& callback)
308 cb(handler, error_state_changed);
314 const geometry_msgs::Pose& feedback_pose, StateChangeCallbackFn& callback)
316 Eigen::Isometry3d pose;
356 if (new_error_state == old_error_state)
374 const geometry_msgs::Pose& offset, geometry_msgs::PoseStamped& tpose)
376 tpose.header = feedback->header;
377 tpose.pose = feedback->pose;
383 geometry_msgs::PoseStamped spose(tpose);
394 ROS_ERROR(
"Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(),
400 ROS_ERROR(
"Cannot transform from frame '%s' to frame '%s' (no TF instance provided)",