twist_marker.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (CC BY-NC-SA 4.0 License)
00003  *
00004  *  Copyright (c) 2014, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  This work is licensed under the Creative Commons
00008  *  Attribution-NonCommercial-ShareAlike 4.0 International License.
00009  *
00010  *  To view a copy of this license, visit
00011  *  http://creativecommons.org/licenses/by-nc-sa/4.0/
00012  *  or send a letter to
00013  *  Creative Commons, 444 Castro Street, Suite 900,
00014  *  Mountain View, California, 94041, USA.
00015  *********************************************************************/
00016 
00017 /*
00018  * @author Enrique Fernandez
00019  */
00020 
00021 #include <ros/ros.h>
00022 #include <geometry_msgs/Twist.h>
00023 #include <visualization_msgs/Marker.h>
00024 #include <visualization_msgs/MarkerArray.h>
00025 
00026 #include <string>
00027 
00028 class TwistMarker
00029 {
00030 public:
00031 
00032   TwistMarker(double scale = 1.0, double z = 0.0, const std::string& frame_id = "base_footprint")
00033     : frame_id_(frame_id)
00034     , scale_(scale)
00035     , z_(z)
00036   {
00037     // ID and type:
00038     marker_.id = 0;
00039     marker_.type = visualization_msgs::Marker::ARROW;
00040 
00041     // Frame ID:
00042     marker_.header.frame_id = frame_id_;
00043 
00044     // Pre-allocate points for setting the arrow with the twist:
00045     marker_.points.resize(2);
00046 
00047     // Vertical position:
00048     marker_.pose.position.z = z_;
00049 
00050     // Scale:
00051     marker_.scale.x = 0.05 * scale_;
00052     marker_.scale.y = 2 * marker_.scale.x;
00053 
00054     // Color:
00055     marker_.color.a = 1.0;
00056     marker_.color.r = 0.0;
00057     marker_.color.g = 1.0;
00058     marker_.color.b = 0.0;
00059   }
00060 
00061   void update(const geometry_msgs::Twist& twist)
00062   {
00063     marker_.points[1].x = twist.linear.x;
00064 
00065     if (fabs(twist.linear.y) > fabs(twist.angular.z))
00066     {
00067       marker_.points[1].y = twist.linear.y;
00068     }
00069     else
00070     {
00071       marker_.points[1].y = twist.angular.z;
00072     }
00073   }
00074 
00075   const visualization_msgs::Marker& getMarker()
00076   {
00077     return marker_;
00078   }
00079 
00080 private:
00081   visualization_msgs::Marker marker_;
00082 
00083   std::string frame_id_;
00084   double scale_;
00085   double z_;
00086 };
00087 
00088 class TwistMarkerPublisher
00089 {
00090 public:
00091 
00092   TwistMarkerPublisher(double scale = 1.0, double z = 0.0)
00093     : marker_(scale, z)
00094   {
00095     ros::NodeHandle nh;
00096 
00097     pub_ = nh.advertise<visualization_msgs::Marker>("marker", 1, true);
00098     sub_ = nh.subscribe("twist", 1, &TwistMarkerPublisher::callback, this);
00099   }
00100 
00101   void callback(const geometry_msgs::TwistConstPtr& twist)
00102   {
00103     marker_.update(*twist);
00104 
00105     pub_.publish(marker_.getMarker());
00106   }
00107 
00108 private:
00109   ros::Subscriber sub_;
00110   ros::Publisher  pub_;
00111 
00112   TwistMarker marker_;
00113 };
00114 
00115 int
00116 main(int argc, char *argv[])
00117 {
00118   ros::init(argc, argv, "twist_marker");
00119 
00120   TwistMarkerPublisher t(1.0, 2.0);
00121 
00122   while (ros::ok())
00123   {
00124     ros::spin();
00125   }
00126 
00127   return EXIT_SUCCESS;
00128 }
00129 


twist_mux
Author(s): Enrique Fernandez , Siegfried-A. Gevatter Pujals
autogenerated on Fri Aug 28 2015 13:25:02