twist_marker.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (CC BY-NC-SA 4.0 License)
 *
 *  Copyright (c) 2014, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  This work is licensed under the Creative Commons
 *  Attribution-NonCommercial-ShareAlike 4.0 International License.
 *
 *  To view a copy of this license, visit
 *  http://creativecommons.org/licenses/by-nc-sa/4.0/
 *  or send a letter to
 *  Creative Commons, 444 Castro Street, Suite 900,
 *  Mountain View, California, 94041, USA.
 *********************************************************************/
 
/*
 * @author Enrique Fernandez
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
 
#include <string>
 
class TwistMarker
{
public:
 
  TwistMarker(double scale = 1.0, double z = 0.0, const std::string& frame_id = "base_footprint")
    : frame_id_(frame_id)
    , scale_(scale)
    , z_(z)
  {
    // ID and type:
    marker_.id = 0;
    marker_.type = visualization_msgs::Marker::ARROW;
 
    // Frame ID:
    marker_.header.frame_id = frame_id_;
 
    // Pre-allocate points for setting the arrow with the twist:
    marker_.points.resize(2);
 
    // Vertical position:
    marker_.pose.position.z = z_;
 
    // Scale:
    marker_.scale.x = 0.05 * scale_;
    marker_.scale.y = 2 * marker_.scale.x;
 
    // Color:
    marker_.color.a = 1.0;
    marker_.color.r = 0.0;
    marker_.color.g = 1.0;
    marker_.color.b = 0.0;
  }
 
  void update(const geometry_msgs::Twist& twist)
  {
    marker_.points[1].x = twist.linear.x;
 
    if (fabs(twist.linear.y) > fabs(twist.angular.z))
    {
      marker_.points[1].y = twist.linear.y;
    }
    else
    {
      marker_.points[1].y = twist.angular.z;
    }
  }
 
  const visualization_msgs::Marker& getMarker()
  {
    return marker_;
  }
 
private:
  visualization_msgs::Marker marker_;
 
  std::string frame_id_;
  double scale_;
  double z_;
};
 
class TwistMarkerPublisher
{
public:
 
  TwistMarkerPublisher(double scale = 1.0, double z = 0.0)
    : marker_(scale, z)
  {
    ros::NodeHandle nh;
 
    pub_ = nh.advertise<visualization_msgs::Marker>("marker", 1, true);
    sub_ = nh.subscribe("twist", 1, &TwistMarkerPublisher::callback, this);
  }
 
  void callback(const geometry_msgs::TwistConstPtr& twist)
  {
    marker_.update(*twist);
 
    pub_.publish(marker_.getMarker());
  }
 
private:
  ros::Subscriber sub_;
  ros::Publisher  pub_;
 
  TwistMarker marker_;
};
 
int
main(int argc, char *argv[])
{
  ros::init(argc, argv, "twist_marker");
 
  TwistMarkerPublisher t(1.0, 2.0);
 
  while (ros::ok())
  {
    ros::spin();
  }
 
  return EXIT_SUCCESS;
}