Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00086 server.setPose("husky_marker", geometry_msgs::Pose());
00087
00088 server.applyChanges();
00089 }
00090
00091 void HuskyMarkerServer::createInteractiveMarkers()
00092 {
00093
00094 InteractiveMarker int_marker;
00095 int_marker.header.frame_id = link_name;
00096 int_marker.name = "husky_marker";
00097
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
00118 int_marker.controls.push_back(control);
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129 server.insert(int_marker, boost::bind( &HuskyMarkerServer::processFeedback, this, _1 ));
00130
00131 server.applyChanges();
00132 }
00133
00134
00135 int main(int argc, char** argv)
00136 {
00137 ros::init(argc, argv, "husky_marker_server");
00138 HuskyMarkerServer huskyserver;
00139
00140 ros::spin();
00141 }