husky_marker_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Clearpath Robotics, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Clearpath Robotics, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <interactive_markers/interactive_marker_server.h>
00032 #include <string.h>
00033 #include <geometry_msgs/Twist.h>
00034 #include <geometry_msgs/Pose.h>
00035 #include <tf/tf.h>
00036 
00037 using namespace visualization_msgs;
00038 
00039 class HuskyMarkerServer
00040 {
00041   public:
00042     HuskyMarkerServer()
00043       : nh("~"), server("husky_marker_server")
00044     {
00045       std::string cmd_vel_topic;
00046 
00047       nh.param<std::string>("link_name", link_name, "/base_link");
00048       nh.param<double>("linear_scale", linear_scale, 1.0);
00049       nh.param<double>("angular_scale", angular_scale, 2.2);
00050 
00051       vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
00052       createInteractiveMarkers();
00053 
00054       ROS_INFO("[husky_marker_server] Initialized.");
00055     }
00056 
00057     void processFeedback(
00058         const InteractiveMarkerFeedbackConstPtr &feedback );
00059   
00060   private:
00061     void createInteractiveMarkers();
00062   
00063     ros::NodeHandle nh;
00064     ros::Publisher vel_pub;
00065     interactive_markers::InteractiveMarkerServer server;
00066     
00067     double linear_scale;
00068     double angular_scale;
00069     
00070     std::string link_name;
00071 };
00072 
00073 void HuskyMarkerServer::processFeedback(
00074     const InteractiveMarkerFeedbackConstPtr &feedback )
00075 {
00076   // Handle angular change (yaw is the only direction in which you can rotate)
00077   double yaw = tf::getYaw(feedback->pose.orientation);
00078   
00079   geometry_msgs::Twist vel;
00080   vel.angular.z = angular_scale*yaw;
00081   vel.linear.x = linear_scale*feedback->pose.position.x;
00082 
00083   vel_pub.publish(vel);    
00084   
00085   // Make the marker snap back to Husky
00086   server.setPose("husky_marker", geometry_msgs::Pose());
00087   
00088   server.applyChanges();
00089 }
00090 
00091 void HuskyMarkerServer::createInteractiveMarkers()
00092 { 
00093   // create an interactive marker for our server
00094   InteractiveMarker int_marker;
00095   int_marker.header.frame_id = link_name;
00096   int_marker.name = "husky_marker";
00097   //int_marker.description = "Move the Husky"; // This doesn't really look good in rviz
00098   
00099   InteractiveMarkerControl control;
00100 
00101   control.orientation_mode = InteractiveMarkerControl::FIXED;
00102   control.orientation.w = 1;
00103   control.orientation.x = 1;
00104   control.orientation.y = 0;
00105   control.orientation.z = 0;
00106   control.name = "move_x";
00107   control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00108   int_marker.controls.push_back(control);
00109 
00110   control.orientation.w = 1;
00111   control.orientation.x = 0;
00112   control.orientation.y = 1;
00113   control.orientation.z = 0;
00114   control.name = "rotate_z";
00115   
00116   control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
00117   int_marker.controls.push_back(control);
00118 
00119   server.insert(int_marker, boost::bind( &HuskyMarkerServer::processFeedback, this, _1 ));
00120   
00121   server.applyChanges();
00122 }
00123 
00124 
00125 int main(int argc, char** argv)
00126 {
00127   ros::init(argc, argv, "husky_marker_server");
00128   HuskyMarkerServer huskyserver;
00129   
00130   ros::spin();
00131 }


husky_interactive_markers
Author(s): Ryan Gariepy
autogenerated on Sun Oct 5 2014 22:52:49