marker_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  *           (c) 2013, Mike Purvis
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <interactive_markers/interactive_marker_server.h>
00032 #include <geometry_msgs/Twist.h>
00033 #include <geometry_msgs/Pose.h>
00034 #include <geometry_msgs/Point.h>
00035 #include <ros/ros.h>
00036 #include <tf/tf.h>
00037 
00038 #include <algorithm>
00039 #include <string>
00040 #include <map>
00041 
00042 using visualization_msgs::InteractiveMarker;
00043 using visualization_msgs::InteractiveMarkerControl;
00044 using visualization_msgs::InteractiveMarkerFeedback;
00045 
00046 class MarkerServer
00047 {
00048   public:
00049     MarkerServer()
00050       : nh("~"), server("twist_marker_server")
00051     {
00052       std::string cmd_vel_topic;
00053 
00054       nh.param<std::string>("link_name", link_name, "/base_link");
00055       nh.param<std::string>("robot_name", robot_name, "robot");
00056       nh.param<double>("link_offset/x", link_offset.x, 0);
00057       nh.param<double>("link_offset/y", link_offset.y, 0);
00058       nh.param<double>("link_offset/z", link_offset.z, 0);
00059 
00060       if (nh.getParam("linear_scale", linear_drive_scale_map))
00061       {
00062         nh.getParam("linear_scale", linear_drive_scale_map);
00063         nh.getParam("max_positive_linear_velocity", max_positive_linear_velocity_map);
00064         nh.getParam("max_negative_linear_velocity", max_negative_linear_velocity_map);
00065       }
00066       else
00067       {
00068         nh.param<double>("linear_scale", linear_drive_scale_map["x"], 1.0);
00069         nh.param<double>("max_positive_linear_velocity", max_positive_linear_velocity_map["x"],  1.0);
00070         nh.param<double>("max_negative_linear_velocity", max_negative_linear_velocity_map["x"], -1.0);
00071       }
00072 
00073       nh.param<double>("angular_scale", angular_drive_scale, 2.2);
00074       nh.param<double>("max_angular_velocity", max_angular_velocity, 2.2);
00075       nh.param<double>("marker_size_scale", marker_size_scale, 1.0);
00076 
00077       vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00078       createInteractiveMarkers();
00079 
00080       ROS_INFO("[twist_marker_server] Initialized.");
00081     }
00082 
00083     void processFeedback(
00084         const InteractiveMarkerFeedback::ConstPtr &feedback);
00085 
00086   private:
00087     void createInteractiveMarkers();
00088 
00089     ros::NodeHandle nh;
00090     ros::Publisher vel_pub;
00091     interactive_markers::InteractiveMarkerServer server;
00092 
00093     std::map<std::string, double> linear_drive_scale_map;
00094     std::map<std::string, double> max_positive_linear_velocity_map;
00095     std::map<std::string, double> max_negative_linear_velocity_map;
00096 
00097     double angular_drive_scale;
00098     double max_angular_velocity;
00099     double marker_size_scale;
00100 
00101     geometry_msgs::Point link_offset;
00102 
00103     std::string link_name;
00104     std::string robot_name;
00105 };
00106 
00107 void MarkerServer::processFeedback(
00108     const InteractiveMarkerFeedback::ConstPtr &feedback )
00109 {
00110   geometry_msgs::Twist vel;
00111 
00112   // Handle angular change (yaw is the only direction in which you can rotate)
00113   double yaw = tf::getYaw(feedback->pose.orientation);
00114   vel.angular.z = angular_drive_scale * yaw;
00115   vel.angular.z = std::min(vel.angular.z,  max_angular_velocity);
00116   vel.angular.z = std::max(vel.angular.z, -max_angular_velocity);
00117 
00118   if (linear_drive_scale_map.find("x") != linear_drive_scale_map.end())
00119   {
00120     vel.linear.x = linear_drive_scale_map["x"] * feedback->pose.position.x;
00121     vel.linear.x = std::min(vel.linear.x, max_positive_linear_velocity_map["x"]);
00122     vel.linear.x = std::max(vel.linear.x, max_negative_linear_velocity_map["x"]);
00123   }
00124   if (linear_drive_scale_map.find("y") != linear_drive_scale_map.end())
00125   {
00126     vel.linear.y = linear_drive_scale_map["y"] * feedback->pose.position.y;
00127     vel.linear.y = std::min(vel.linear.y, max_positive_linear_velocity_map["y"]);
00128     vel.linear.y = std::max(vel.linear.y, max_negative_linear_velocity_map["y"]);
00129   }
00130   if (linear_drive_scale_map.find("z") != linear_drive_scale_map.end())
00131   {
00132     vel.linear.z = linear_drive_scale_map["z"] * feedback->pose.position.z;
00133     vel.linear.z = std::min(vel.linear.z, max_positive_linear_velocity_map["z"]);
00134     vel.linear.z = std::max(vel.linear.z, max_negative_linear_velocity_map["z"]);
00135   }
00136 
00137   vel_pub.publish(vel);
00138 
00139   // Make the marker snap back to robot
00140   geometry_msgs::Pose updated_pose;
00141   updated_pose.position = link_offset;
00142   server.setPose(robot_name + "_twist_marker", updated_pose);
00143 
00144   server.applyChanges();
00145 }
00146 
00147 void MarkerServer::createInteractiveMarkers()
00148 {
00149   // create an interactive marker for our server
00150   InteractiveMarker int_marker;
00151   int_marker.header.frame_id = link_name;
00152   int_marker.name = robot_name + "_twist_marker";
00153   int_marker.description = "twist controller for " + robot_name;
00154   int_marker.scale = marker_size_scale;
00155 
00156   // Add any offsets to the marker position.
00157   int_marker.pose.position = link_offset;
00158 
00159   InteractiveMarkerControl control;
00160 
00161   control.orientation_mode = InteractiveMarkerControl::FIXED;
00162 
00163   if (linear_drive_scale_map.find("x") != linear_drive_scale_map.end())
00164   {
00165     control.orientation.w = 1;
00166     control.orientation.x = 1;
00167     control.orientation.y = 0;
00168     control.orientation.z = 0;
00169     control.name = "move_x";
00170     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00171     int_marker.controls.push_back(control);
00172   }
00173   if (linear_drive_scale_map.find("y") != linear_drive_scale_map.end())
00174   {
00175     control.orientation.w = 1;
00176     control.orientation.x = 0;
00177     control.orientation.y = 0;
00178     control.orientation.z = 1;
00179     control.name = "move_y";
00180     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00181     int_marker.controls.push_back(control);
00182   }
00183   if (linear_drive_scale_map.find("z") != linear_drive_scale_map.end())
00184   {
00185     control.orientation.w = 1;
00186     control.orientation.x = 0;
00187     control.orientation.y = 1;
00188     control.orientation.z = 0;
00189     control.name = "move_z";
00190     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00191     int_marker.controls.push_back(control);
00192   }
00193 
00194   control.orientation.w = 1;
00195   control.orientation.x = 0;
00196   control.orientation.y = 1;
00197   control.orientation.z = 0;
00198   control.name = "rotate_z";
00199   control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00200   int_marker.controls.push_back(control);
00201 
00202   server.insert(int_marker, boost::bind(&MarkerServer::processFeedback, this, _1));
00203 
00204   server.applyChanges();
00205 }
00206 
00207 
00208 int main(int argc, char** argv)
00209 {
00210   ros::init(argc, argv, "marker_server");
00211   MarkerServer server;
00212 
00213   ros::spin();
00214 }


interactive_marker_twist_server
Author(s):
autogenerated on Sat Jun 8 2019 10:28:07