怎么利用InteractiveMarkers在rviz上画“曲线”

rviz是大家熟练玩转的界面，它是基于QT开发的一款比较成熟的平台，并留有一些接口给用户进行自定义功能的插入，例如基础的文本框输入、图形界面操作的cmd_vel速度下发插件(rviz plugin tutorials)，但做一件可增删改查的曲线绘制工具目前在教程中还没有普及。

首先要能轻松改变曲线端点位置，若是自己开发QT界面可以开心地使用QDrag制作拖拽图标作为曲线的可拖拽端点，但rviz仅提供了插件接口，因此使用更简洁的轮子InteractiveMarkers是更好的选择，首先看看这个轮子的外部接口：

void insert( const visualization_msgs::InteractiveMarker &int_marker );

void insert( const visualization_msgs::InteractiveMarker &int_marker,
             FeedbackCallback feedback_cb,
             uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
bool setPose( const std::string &name,
              const geometry_msgs::Pose &pose,
              const std_msgs::Header &header=std_msgs::Header() );
bool erase( const std::string &name );
void clear();
bool empty() const;
std::size_t size() const;
bool setCallback( const std::string &name, FeedbackCallback feedback_cb,
                  uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
void applyChanges();
bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const;

上述接口分别提供了插入一个新的marker、指定一个marker的位置、删除一个marker或清空所有、查询是否具有特定名称的marker的功能，（更复杂的内部调用关系略过，可在http://docs.ros.org/jade/api/interactive_markers中查看），对我们的需求非常有用。在官方文档中提供了９种可拖拽的marker，我们选择水平面上自由平移的VIEW_FACING进行操作：

server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) );

tf::Vector3 position;

position = tf::Vector3(0,0,0);

int number;
  cin>>number;
  for(int i=1;i   {
    makeViewFacingMarker(position,i);
  }

对教程代码稍加修改，我们便可得到两个marker，并拖拽至合适位置，作为曲线的端点，接下来在两个端点中插入多个点，充当“曲线”，这个时候需要用到前面展示的接口setPose( )，它需要曲线点与端点互动（即跟着端点拖拽而移动），那么代码如下所示：

float curve_compute(float p0,float p1,float t)
{
      return p0+t*(p1-p0);
 }

它将直线分解成1/t个离散点，假设分解成５０个点，则可以依次计算得到曲线离散点的坐标：
for(j=1;j<50;j++){
    t=j*0.02;
    tPoint.x=curve_compute(vertex1_x,vertex2_x,t);
    tPoint.y=curve_compute(vertex1_y,vertex2_y,t);
    points.push_back(tPoint);  }

保存入数组points中再依次赋值给marker：

for(int i=0;i  {
     float curves_x,curves_y;
     curves_x=points[i].x;
     curves_y=points[i].y;

     geometry_msgs::Pose pose;
     pose.position.x = curves_x;
     pose.position.y = curves_y;
     pose.position.z = 0;
     std::stringstream s;
     s << i;
     server->setPose( s.str(), pose );
  }

这样，只要更新了曲线端点，离散点的坐标便可以再次计算得到，如图：

若是想画出圆滑的复杂曲线，则可以借助贝赛尔曲线，增加一个控制点，计算的代码如下：

float curve_compute(float p0,float p1,float p2,float t)
 {
     float a,b,c;
     float tmin;

     c=p0;
     b=2.0*p1;
     a=p2;

     tmin=1-t;
     return(tmin*tmin*c)+(tmin*t*b)+(t*t*a);
   }

通过与直线类似的方法，可拖拽的点是两个端点与控制点，曲线离散点设置为不可拖拽，如图所示：

这样可以起到类似曲线的作用了。等自己开发QT界面再来画更好看的自定义曲线哈。