cube.png
源碼
/*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <interactive_markers/interactive_marker_server.h>
#include <math.h>
#include <tf/LinearMath/Vector3.h>
using namespace visualization_msgs;
boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
std::vector< tf::Vector3 > positions;
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
{
//compute difference vector for this cube
tf::Vector3 fb_pos(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
unsigned index = atoi( feedback->marker_name.c_str() );
if ( index > positions.size() )
{
return;
}
tf::Vector3 fb_delta = fb_pos - positions[index];
// move all markers in that direction
for ( unsigned i=0; i<positions.size(); i++ )
{
float d = fb_pos.distance( positions[i] );
float t = 1 / (d*5.0+1.0) - 0.2;
if ( t < 0.0 ) t=0.0;
positions[i] += t * fb_delta;
if ( i == index ) {
ROS_INFO_STREAM( d );
positions[i] = fb_pos;
}
geometry_msgs::Pose pose;
pose.position.x = positions[i].x();
pose.position.y = positions[i].y();
pose.position.z = positions[i].z();
std::stringstream s;
s << i;
server->setPose( s.str(), pose );
}
break;
}
}
server->applyChanges();
}
InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
{
InteractiveMarkerControl control;
control.always_visible = true;
control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
control.independent_marker_orientation = true;
Marker marker;
marker.type = Marker::CUBE;
marker.scale.x = msg.scale;
marker.scale.y = msg.scale;
marker.scale.z = msg.scale;
marker.color.r = 0.65+0.7*msg.pose.position.x;
marker.color.g = 0.65+0.7*msg.pose.position.y;
marker.color.b = 0.65+0.7*msg.pose.position.z;
marker.color.a = 1.0;
control.markers.push_back( marker );
msg.controls.push_back( control );
return msg.controls.back();
}
void makeCube( )
{
int side_length = 10;
float step = 1.0/ (float)side_length;
int count = 0;
positions.reserve( side_length*side_length*side_length );
for ( double x=-0.5; x<0.5; x+=step )
{
for ( double y=-0.5; y<0.5; y+=step )
{
for ( double z=0.0; z<1.0; z+=step )
{
InteractiveMarker int_marker;
int_marker.header.frame_id = "base_link";
int_marker.scale = step;
int_marker.pose.position.x = x;
int_marker.pose.position.y = y;
int_marker.pose.position.z = z;
positions.push_back( tf::Vector3(x,y,z) );
std::stringstream s;
s << count;
int_marker.name = s.str();
makeBoxControl(int_marker);
server->insert( int_marker );
server->setCallback( int_marker.name, &processFeedback );
count++;
}
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "cube");
server.reset( new interactive_markers::InteractiveMarkerServer("cube") );
ros::Duration(0.1).sleep();
ROS_INFO("initializing..");
makeCube();
server->applyChanges();
ROS_INFO("ready.");
ros::spin();
server.reset();
}
源碼分析
#include <ros/ros.h>
#include <interactive_markers/interactive_marker_server.h>
#include <math.h>
#include <tf/LinearMath/Vector3.h>
using namespace visualization_msgs;
首先頭文件包含。
boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
std::vector< tf::Vector3 > positions;
建立了一個服務器和一個三維向量positions.
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
{
//compute difference vector for this cube
tf::Vector3 fb_pos(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
unsigned index = atoi( feedback->marker_name.c_str() );
if ( index > positions.size() )
{
return;
}
tf::Vector3 fb_delta = fb_pos - positions[index];
// move all markers in that direction
for ( unsigned i=0; i<positions.size(); i++ )
{
float d = fb_pos.distance( positions[i] );
float t = 1 / (d*5.0+1.0) - 0.2;
if ( t < 0.0 ) t=0.0;
positions[i] += t * fb_delta;
if ( i == index ) {
ROS_INFO_STREAM( d );
positions[i] = fb_pos;
}
geometry_msgs::Pose pose;
pose.position.x = positions[i].x();
pose.position.y = positions[i].y();
pose.position.z = positions[i].z();
std::stringstream s;
s << i;
server->setPose( s.str(), pose );
}
break;
}
}
server->applyChanges();
}
這個函數很熟悉了，這個是反饋函數。首先當其被服務器調用時，先記錄此時方塊的位置，然后計算它移動的增量。
然后將增量應用到其他塊上。
InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
{
InteractiveMarkerControl control;
control.always_visible = true;
control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
control.independent_marker_orientation = true;
Marker marker;
marker.type = Marker::CUBE;
marker.scale.x = msg.scale;
marker.scale.y = msg.scale;
marker.scale.z = msg.scale;
marker.color.r = 0.65+0.7*msg.pose.position.x;
marker.color.g = 0.65+0.7*msg.pose.position.y;
marker.color.b = 0.65+0.7*msg.pose.position.z;
marker.color.a = 1.0;
control.markers.push_back( marker );
msg.controls.push_back( control );
return msg.controls.back();
}
這個之前說過了，就是建立一個帶著marker的控制，并添加到交互式marker上。
void makeCube( )
{
int side_length = 10;
float step = 1.0/ (float)side_length;
int count = 0;
positions.reserve( side_length*side_length*side_length );
for ( double x=-0.5; x<0.5; x+=step )
{
for ( double y=-0.5; y<0.5; y+=step )
{
for ( double z=0.0; z<1.0; z+=step )
{
InteractiveMarker int_marker;
int_marker.header.frame_id = "base_link";
int_marker.scale = step;
int_marker.pose.position.x = x;
int_marker.pose.position.y = y;
int_marker.pose.position.z = z;
positions.push_back( tf::Vector3(x,y,z) );
std::stringstream s;
s << count;
int_marker.name = s.str();
makeBoxControl(int_marker);
server->insert( int_marker );
server->setCallback( int_marker.name, &processFeedback );
count++;
}
}
}
}
這個就是建立一組cube的程序。
主函數就不說了，跟上次的一樣。
