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();
}
}
