// Move the real block!
void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
{
if (!action_server_.isActive())
{
ROS_INFO("[block logic] Got feedback but not active!");
return;
}
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
ROS_INFO_STREAM("[block logic] Staging " << feedback->marker_name);
old_pose_ = feedback->pose;
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
ROS_INFO_STREAM("[block logic] Now moving " << feedback->marker_name);
moveBlock(old_pose_, feedback->pose);
break;
}
interactive_m_server_.applyChanges();
}
// 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::processFeedback(
const InteractiveMarkerFeedbackConstPtr &feedback )
{
// Handle angular change (yaw is the only direction in which you can rotate)
double yaw = tf::getYaw(feedback->pose.orientation);
geometry_msgs::Twist vel;
vel.angular.z = angular_scale*yaw;
vel.linear.x = linear_scale*feedback->pose.position.x;
vel_pub.publish(vel);
// Make the marker snap back to turtlebot
server.setPose("turtlebot_marker", geometry_msgs::Pose());
server.applyChanges();
}
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 addBlocks(const geometry_msgs::PoseArrayConstPtr& msg)
{
interactive_m_server_.clear();
interactive_m_server_.applyChanges();
ROS_INFO("[block logic] Got block detection callback. Adding blocks.");
geometry_msgs::Pose block;
bool active = action_server_.isActive();
for (unsigned int i=0; i < msg->poses.size(); i++)
{
block = msg->poses[i];
addBlock(block, i, active, msg->header.frame_id);
}
ROS_INFO("[block logic] Added %d blocks to Rviz", int(msg->poses.size()));
interactive_m_server_.applyChanges();
msg_ = msg;
initialized_ = true;
}
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);
}