void PickAndPlace::set_attached_object(bool attach, const geometry_msgs::Pose &pose,moveit_msgs::RobotState &robot_state) { // get robot state robot_state::RobotStatePtr current_state= move_group_ptr->getCurrentState(); if(attach) { // constructing shape std::vector<shapes::ShapeConstPtr> shapes_array; shapes::ShapeConstPtr shape( shapes::constructShapeFromMsg( cfg.ATTACHED_OBJECT.primitives[0])); shapes_array.push_back(shape); // constructing pose tf::Transform attached_tf; tf::poseMsgToTF(cfg.ATTACHED_OBJECT.primitive_poses[0],attached_tf); EigenSTL::vector_Affine3d pose_array(1); tf::transformTFToEigen(attached_tf,pose_array[0]); // attaching current_state->attachBody(cfg.ATTACHED_OBJECT_LINK_NAME,shapes_array,pose_array,cfg.TOUCH_LINKS,cfg.TCP_LINK_NAME); // update box marker cfg.MARKER_MESSAGE.header.frame_id = cfg.TCP_LINK_NAME; cfg.MARKER_MESSAGE.pose = cfg.TCP_TO_BOX_POSE; } else { // detaching if(current_state->hasAttachedBody(cfg.ATTACHED_OBJECT_LINK_NAME)) current_state->clearAttachedBody(cfg.ATTACHED_OBJECT_LINK_NAME); } // save robot state data robot_state::robotStateToRobotStateMsg(*current_state,robot_state); }
void InteractiveMarkerInitializer::process_feedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { std::lock_guard<std::mutex> lock(mutex_); // update marker pose int i = std::stoi(feedback->marker_name); ROS_INFO("Marker Frame %s", feedback->header.frame_id.c_str()); // if the feedback from the marker is in a different than the desired frame // convert it using a transform listener if (feedback->header.frame_id.compare(camera_frame_id_) != 0) { geometry_msgs::PoseStamped pose_in; pose_in.header = feedback->header; pose_in.pose = feedback->pose; geometry_msgs::PoseStamped pose_out; try { listener_.transformPose(camera_frame_id_, pose_in, pose_out); } catch (tf::TransformException ex) { ROS_ERROR("%s", ex.what()); ros::Duration(1.0).sleep(); } poses_[i] = pose_out.pose; } else { poses_[i] = feedback->pose; } visualization_msgs::InteractiveMarker int_marker; server_.get(feedback->marker_name, int_marker); if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK) { ROS_INFO("Feedback frame %s and Marker Frame %s", feedback->header.frame_id.c_str(), int_marker.header.frame_id.c_str()); // cache this pose cache_pose(feedback->marker_name, poses_[i]); // toggle active state active_[i] = !active_[i]; switch_marker(int_marker, active_[i]); if (are_all_object_poses_set() && poses_update_callback_) { poses_update_callback_(pose_array()); } // update marker int_marker.header.frame_id = feedback->header.frame_id; server_.insert(int_marker); server_.applyChanges(); } }