// Add a new block
void addBlock( const geometry_msgs::Pose pose, int n, bool active, std::string link)
{
InteractiveMarker marker;
marker.header.frame_id = link;
marker.pose = pose;
marker.scale = block_size;
std::stringstream conv;
conv << n;
conv.str();
marker.name = "block" + conv.str();
InteractiveMarkerControl control;
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
if (active)
marker.controls.push_back( control );
control.markers.push_back( makeBox(marker, .5, .5, .5) );
control.always_visible = true;
marker.controls.push_back( control );
interactive_m_server_.insert( marker );
interactive_m_server_.setCallback( marker.name, boost::bind( &BlockLogicServer::feedbackCb, this, _1 ));
}
void TurtlebotMarkerServer::createInteractiveMarkers()
{
// create an interactive marker for our server
InteractiveMarker int_marker;
int_marker.header.frame_id = link_name;
int_marker.name = "turtlebot_marker";
//int_marker.description = "Move the turtlebot";
InteractiveMarkerControl control;
control.orientation_mode = InteractiveMarkerControl::FIXED;
control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
control.name = "move_x";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
control.name = "rotate_z";
control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
//control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
// Commented out for non-holonomic turtlebot. If holonomic, can move in y.
/*control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
control.name = "move_y";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);*/
server.insert(int_marker, boost::bind( &TurtlebotMarkerServer::processFeedback, this, _1 ));
server.applyChanges();
}
void InteractiveTF::addTFMarker()
{
ros::NodeHandle nh;
visualization_msgs::InteractiveMarker tf_marker;
tf_marker.header.frame_id = parent_frame_;
tf_marker.name = "tf_marker";
tf_marker.scale = 0.2;
visualization_msgs::InteractiveMarkerControl tf_control;
tf_control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
// x
tf_control.orientation.x = 1; tf_control.orientation.y = 0;
tf_control.orientation.z = 0; tf_control.orientation.w = 1;
tf_control.name = "rotate_x";
tf_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
tf_marker.controls.push_back(tf_control);
tf_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
tf_marker.controls.push_back(tf_control);
// y
tf_control.orientation.x = 0; tf_control.orientation.y = 1;
tf_control.orientation.z = 0; tf_control.orientation.w = 1;
tf_control.name = "rotate_y";
tf_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
tf_marker.controls.push_back(tf_control);
tf_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
tf_marker.controls.push_back(tf_control);
// z
tf_control.orientation.x = 0; tf_control.orientation.y = 0;
tf_control.orientation.z = 1; tf_control.orientation.w = 1;
tf_control.name = "rotate_z";
tf_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
tf_marker.controls.push_back(tf_control);
tf_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
tf_marker.controls.push_back(tf_control);
im_server_.insert(tf_marker, boost::bind(&InteractiveTF::processTFControl, this, _1));
im_server_.applyChanges();
tf_timer_ = nh.createTimer(ros::Duration(1.0 / rate_), &InteractiveTF::publishTF, this);
}
