arm_marker_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, 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 the Willow Garage, 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 
00032 #include <actionlib/client/simple_action_client.h>
00033 #include <simple_arm_server/MoveArmAction.h>
00034 #include <arbotix_msgs/Relax.h>
00035 
00036 #include <interactive_markers/interactive_marker_server.h>
00037 #include <interactive_markers/menu_handler.h>
00038 
00039 #include <geometry_msgs/Twist.h>
00040 #include <geometry_msgs/Pose.h>
00041 #include <std_msgs/Float64.h>
00042 
00043 #include <tf/tf.h>
00044 #include <tf/transform_listener.h>
00045 
00046 #include <string.h>
00047 
00048 #include <boost/foreach.hpp>
00049 
00050 using namespace visualization_msgs;
00051 using namespace interactive_markers;
00052 using namespace std;
00053 
00054 class ArmMarkerServer
00055 {
00056   private:
00057     ros::NodeHandle nh;
00058     actionlib::SimpleActionClient<simple_arm_server::MoveArmAction> client;
00059     interactive_markers::InteractiveMarkerServer server;
00060     tf::TransformListener tf_listener;
00061     
00062     MenuHandler arm_menu_handler;
00063     map<std::string, MenuHandler> joint_menu_handlers;
00064     
00065     bool immediate_commands;
00066     bool in_move;
00067     
00068     ros::Timer arm_timer;
00069     
00070     // Joint and link parameters
00071     vector<string> joints;
00072     vector<string> links;
00073     string tip_link;
00074     string root_link;
00075     
00076     // Gripper parameters
00077     double gripper_marker_offset_x;
00078     double gripper_marker_offset_y;
00079     double gripper_marker_offset_z;
00080     
00081     double gripper_box_offset_x;
00082     double gripper_box_offset_y;
00083     double gripper_box_offset_z;
00084     
00085     // Other parameters
00086     double move_time;
00087     
00088     // Joint command and relax publishers
00089     map<std::string, ros::Publisher> joint_command_publishers;
00090     map<std::string, ros::ServiceClient> joint_relax_clients;
00091     
00092 public:
00093   ArmMarkerServer()
00094     : nh("~"), client("simple_move_arm", true), server("arm_marker_server"), tf_listener(nh), immediate_commands(true), in_move(false)
00095   {
00096     // Get general arm parameters
00097     nh.param<std::string>("root_link", root_link, "torso_link");
00098     nh.param<std::string>("tip_link", tip_link, "gripper_link");
00099     nh.param<double>("move_time", move_time, 2.0);
00100     
00101     // Get the joint list
00102     XmlRpc::XmlRpcValue joint_list;
00103     nh.getParam("joints", joint_list);
00104     ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00105 
00106     for (int32_t i = 0; i < joint_list.size(); ++i) 
00107     {
00108       ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00109       joints.push_back(joint_list[i]);
00110     }
00111     
00112     // Get the corresponding link list
00113     XmlRpc::XmlRpcValue link_list;
00114     nh.getParam("links", link_list);
00115     ROS_ASSERT(link_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00116 
00117     for (int32_t i = 0; i < joint_list.size(); ++i) 
00118     {
00119       ROS_ASSERT(link_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00120       links.push_back(link_list[i]);
00121     }
00122     
00123     // Get gripper offsets
00124     nh.param<double>("gripper/marker_offset/x", gripper_marker_offset_x, 0.02);
00125     nh.param<double>("gripper/marker_offset/y", gripper_marker_offset_y, 0.025);
00126     nh.param<double>("gripper/marker_offset/z", gripper_marker_offset_z, 0.0);
00127     
00128     nh.param<double>("gripper/box_offset/x", gripper_box_offset_x, -0.017);
00129     nh.param<double>("gripper/box_offset/y", gripper_box_offset_y, -0.008);
00130     nh.param<double>("gripper/box_offset/z", gripper_box_offset_z, 0.0);
00131     
00132     createArmMarker();
00133     createGripperMarker();
00134     createArmMenu();
00135     
00136     createJointPublishers();
00137     
00138     resetMarker();
00139     
00140     ROS_INFO("[arm marker server] Initialized.");
00141   }
00142 
00143   void createJointPublishers()
00144   {
00145     BOOST_FOREACH( std::string joint_name, joints )
00146     {
00147       joint_command_publishers[joint_name] = (nh.advertise<std_msgs::Float64>("/" + joint_name + "/command", 1, false));
00148       joint_relax_clients[joint_name] = (nh.serviceClient<arbotix_msgs::Relax>("/" + joint_name + "/relax"));
00149     }
00150   
00151   }
00152 
00153   void processArmFeedback(const InteractiveMarkerFeedbackConstPtr &feedback)
00154   {
00155     if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN && feedback->marker_name == "arm_marker")
00156     {
00157       arm_timer.stop();
00158     }
00159     if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP || feedback->marker_name != "arm_marker")
00160       return;
00161       
00162     if (immediate_commands)
00163     {
00164       sendTrajectoryCommand(feedback);
00165     }
00166   }
00167   
00168   void processJointFeedback(const InteractiveMarkerFeedbackConstPtr &feedback)
00169   {
00170     std_msgs::Float64 command;
00171     command.data = feedback->pose.orientation.y;
00172     joint_command_publishers[feedback->marker_name].publish(command);
00173   }
00174   
00175   void changeMarkerColor(double r, double g, double b, bool set_pose=false, geometry_msgs::Pose pose = geometry_msgs::Pose())
00176   {
00177     InteractiveMarker int_marker;
00178     server.get("arm_marker", int_marker);
00179     
00180     Marker *box_marker = &int_marker.controls[0].markers[0];
00181     
00182     box_marker->color.r = r;
00183     box_marker->color.g = g;
00184     box_marker->color.b = b;
00185     
00186     if (set_pose)
00187       int_marker.pose = pose;
00188     
00189     server.insert(int_marker);
00190     server.applyChanges();
00191   }
00192   
00193   void processCommand(const actionlib::SimpleClientGoalState& state,
00194                const simple_arm_server::MoveArmResultConstPtr& result, 
00195                const InteractiveMarkerFeedbackConstPtr &feedback)
00196   {
00197     ROS_INFO("Finished in state [%s]", state.toString().c_str());
00198   
00199     //changeMarkerColor(0, 0, 1);
00200     if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00201     {
00202       changeMarkerColor(0, 1, 0, true, feedback->pose);
00203       ros::Duration(0.25).sleep();
00204       resetMarker();
00205       //arm_timer.start();
00206     }
00207     else
00208     {
00209       changeMarkerColor(1, 0, 0, true, feedback->pose);
00210     }
00211   }
00212   
00213   bool sendTrajectoryCommand(const InteractiveMarkerFeedbackConstPtr &feedback)
00214   {
00215     simple_arm_server::MoveArmGoal goal;
00216     simple_arm_server::ArmAction action;
00217     
00218     goal.header.frame_id = root_link;
00219     
00220     geometry_msgs::Pose pose;
00221     getTransformedPose(feedback->header.frame_id, feedback->pose, root_link, pose, feedback->header.stamp);
00222     
00223     action.goal.orientation = pose.orientation;
00224     action.goal.position = pose.position;
00225     action.move_time = ros::Duration(move_time);
00226     goal.motions.push_back(action); 
00227     
00228     client.sendGoal(goal, boost::bind(&ArmMarkerServer::processCommand, this, _1, _2, feedback));
00229     changeMarkerColor(0, 0, 1, true, feedback->pose);
00230     
00231     return true;
00232   }
00233   
00234   bool sendGripperCommand(const InteractiveMarkerFeedbackConstPtr &feedback)
00235   {
00236     simple_arm_server::MoveArmGoal goal;
00237     simple_arm_server::ArmAction action;
00238     
00239     goal.header.frame_id = tip_link;
00240     action.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
00241     action.command = feedback->pose.position.y;
00242     goal.motions.push_back(action); 
00243     
00244     client.sendGoal(goal);
00245     //client.waitForResult(ros::Duration(30.0));
00246     //if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00247     //  return true;
00248     //return false;
00249     
00250     // Don't block. Just return.
00251     return true;
00252   }
00253 
00254   void getTransformedPose(const string& source_frame, const geometry_msgs::Pose& source_pose,
00255                           const string& target_frame, geometry_msgs::Pose& target_pose,
00256                           const ros::Time& time)
00257   {
00258     tf::Pose bt_source_pose;
00259     
00260     tf::poseMsgToTF(source_pose, bt_source_pose);
00261     
00262     tf::Stamped<tf::Pose> posein(bt_source_pose, time, source_frame);
00263     tf::Stamped<tf::Pose> poseout;
00264     
00265     try 
00266     {
00267       ros::Duration timeout(10.0);
00268       
00269       // Get base_link transform
00270       tf_listener.waitForTransform(target_frame, source_frame,
00271                                     time, timeout);
00272       tf_listener.transformPose(target_frame, posein, poseout);
00273       
00274       
00275     }
00276     catch (tf::TransformException& ex) {
00277       ROS_WARN("[arm interactive markers] TF exception:\n%s", ex.what());
00278       return;
00279     }
00280       
00281     tf::poseTFToMsg(poseout, target_pose);
00282   }
00283 
00284   void resetMarker()
00285   {
00286     geometry_msgs::Pose arm_pose;
00287     geometry_msgs::Pose blank_pose;
00288     blank_pose.orientation.w = 1;
00289     
00290     getTransformedPose(tip_link, blank_pose, root_link, arm_pose, ros::Time(0));
00291     
00292     server.setPose("arm_marker", arm_pose);
00293     server.applyChanges();
00294   }
00295 
00296   void createArmMarker()
00297   { 
00298     // Create a marker representing the tip of the arm.
00299     InteractiveMarker int_marker;
00300     int_marker.header.frame_id = root_link;
00301     int_marker.name = "arm_marker";
00302     int_marker.scale = 0.1;
00303 
00304     Marker marker;
00305 
00306     marker.type = Marker::CUBE;
00307     marker.scale.x = 0.03;
00308     marker.scale.y = 0.03;
00309     marker.scale.z = 0.03;
00310     marker.color.r = 0.0;
00311     marker.color.g = 1.0;
00312     marker.color.b = 0.0;
00313     marker.color.a = .7;
00314 
00315     InteractiveMarkerControl box_control;
00316     box_control.always_visible = true;
00317     box_control.interaction_mode = InteractiveMarkerControl::BUTTON;
00318     box_control.markers.push_back( marker );
00319     int_marker.controls.push_back( box_control );
00320 
00321     InteractiveMarkerControl control;
00322 
00323     control.orientation.w = 1;
00324     control.orientation.x = 1;
00325     control.orientation.y = 0;
00326     control.orientation.z = 0;
00327     control.name = "rotate_x";
00328     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00329     int_marker.controls.push_back(control);
00330     control.name = "move_x";
00331     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00332     int_marker.controls.push_back(control);
00333 
00334     control.orientation.w = 1;
00335     control.orientation.x = 0;
00336     control.orientation.y = 1;
00337     control.orientation.z = 0;
00338     control.name = "rotate_z";
00339     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00340     int_marker.controls.push_back(control);
00341     control.name = "move_z";
00342     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00343     int_marker.controls.push_back(control);
00344 
00345     control.orientation.w = 1;
00346     control.orientation.x = 0;
00347     control.orientation.y = 0;
00348     control.orientation.z = 1;
00349     control.name = "rotate_y";
00350     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00351     int_marker.controls.push_back(control);
00352     control.name = "move_y";
00353     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00354     int_marker.controls.push_back(control);
00355 
00356     
00357     server.insert(int_marker, boost::bind( &ArmMarkerServer::processArmFeedback, this, _1 ));
00358     
00359     server.applyChanges();
00360   }
00361    
00362   void createGripperMarker()
00363   {
00364     // Create a marker for the gripper
00365     InteractiveMarker int_marker;
00366     int_marker.header.frame_id = tip_link;
00367     int_marker.name = "gripper_marker";
00368     int_marker.scale = 0.075;
00369 
00370     int_marker.pose.position.x = gripper_marker_offset_x;
00371     int_marker.pose.position.y = gripper_marker_offset_y;
00372     int_marker.pose.position.z = gripper_marker_offset_z;
00373 
00374     Marker marker;
00375 
00376     marker.type = Marker::CUBE;
00377     marker.scale.x = 0.05;
00378     marker.scale.y = 0.005;
00379     marker.scale.z = 0.05;
00380     marker.color.r = 0.0;
00381     marker.color.g = 1.0;
00382     marker.color.b = 0.0;
00383     marker.color.a = .7;
00384     
00385     marker.pose.position.x = gripper_box_offset_x;
00386     marker.pose.position.y = gripper_box_offset_y;
00387     marker.pose.position.z = gripper_box_offset_z;
00388 
00389     InteractiveMarkerControl box_control;
00390     box_control.always_visible = true;
00391     box_control.interaction_mode = InteractiveMarkerControl::BUTTON;
00392     box_control.markers.push_back( marker );
00393     int_marker.controls.push_back( box_control );
00394 
00395     InteractiveMarkerControl control;
00396 
00397     control.orientation.w = 1;
00398     control.orientation.x = 0;
00399     control.orientation.y = 0;
00400     control.orientation.z = 1;
00401     control.name = "move_z";
00402     control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00403     int_marker.controls.push_back(control);
00404 
00405     server.insert(int_marker, boost::bind( &ArmMarkerServer::sendGripperCommand, this, _1 ));
00406     
00407     server.applyChanges();
00408   }
00409   
00410   void createJointMarkers()
00411   {
00412     for (unsigned int i = 0; i < joints.size() && i < links.size(); i++)
00413     {
00414       createJointMarker(joints[i], links[i]);
00415     }
00416   }
00417   
00418   void createJointMarker(const string joint_name, const string link_name)
00419   {
00420     InteractiveMarker int_marker;
00421     int_marker.header.frame_id = link_name;
00422     int_marker.name = joint_name;
00423     int_marker.scale = 0.05;
00424 
00425     InteractiveMarkerControl control;
00426 
00427     control.orientation.w = 1;
00428     control.orientation.x = 0;
00429     control.orientation.y = 0;
00430     control.orientation.z = 1;
00431     control.name = "rotate_z";
00432     control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00433     int_marker.controls.push_back(control);
00434 
00435     server.insert(int_marker, boost::bind( &ArmMarkerServer::processJointFeedback, this, _1 ));
00436     
00437     server.applyChanges();
00438   }
00439   
00440   void createArmMenu()
00441   {
00442     arm_menu_handler = MenuHandler();
00443     arm_menu_handler.insert("Send command", boost::bind(&ArmMarkerServer::sendCommandCb, this, _1));
00444     arm_menu_handler.setCheckState(
00445         arm_menu_handler.insert("Send command immediately", boost::bind(&ArmMarkerServer::immediateCb, this, _1)), MenuHandler::CHECKED );
00446     arm_menu_handler.insert("Reset marker", boost::bind(&ArmMarkerServer::resetPoseCb, this, _1));
00447     
00448     MenuHandler::EntryHandle entry = arm_menu_handler.insert("Relax joints");
00449     arm_menu_handler.insert( entry, "All", boost::bind(&ArmMarkerServer::relaxAllCb, this, _1));
00450     BOOST_FOREACH(string joint_name, joints)
00451     {
00452       arm_menu_handler.insert( entry, joint_name, boost::bind(&ArmMarkerServer::relaxCb, this, joint_name, _1) );
00453     }
00454     
00455     arm_menu_handler.insert("Switch to Joint Control", boost::bind(&ArmMarkerServer::switchToJointControlCb, this, _1));
00456     
00457     arm_menu_handler.apply( server, "arm_marker" );
00458     server.applyChanges();
00459   }
00460   
00461   void createJointMenus()
00462   {
00463     BOOST_FOREACH(string joint_name, joints)
00464     {
00465       createJointMenu(joint_name);
00466     }
00467   }
00468   
00469   void createJointMenu(const string joint_name)
00470   {
00471     MenuHandler joint_handler;
00472     joint_handler.insert("Relax all", boost::bind(&ArmMarkerServer::relaxAllCb, this, _1));
00473     joint_handler.insert("Relax this joint", boost::bind(&ArmMarkerServer::relaxCb, this, joint_name, _1));
00474     joint_handler.insert("Switch to Trajectory Control", boost::bind(&ArmMarkerServer::switchToArmControlCb, this, _1));
00475     
00476     joint_menu_handlers[joint_name] = joint_handler;
00477     
00478     joint_menu_handlers[joint_name].apply( server, joint_name );
00479     server.applyChanges();
00480   }
00481 
00482   void sendCommandCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00483   {
00484     sendTrajectoryCommand(feedback);
00485   }
00486   
00487   void relaxAllCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00488   {
00489     BOOST_FOREACH( std::string joint_name, joints )
00490     {
00491       relaxCb(joint_name, feedback);
00492     }
00493   }
00494   
00495   void relaxCb(const std::string joint_name, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00496   {
00497     arbotix_msgs::Relax srv;
00498     joint_relax_clients[joint_name].call(srv);
00499   }
00500   
00501   void immediateCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00502   {
00503     MenuHandler::EntryHandle handle = feedback->menu_entry_id;
00504     MenuHandler::CheckState state;
00505     arm_menu_handler.getCheckState( handle, state );
00506 
00507     if ( state == MenuHandler::CHECKED )
00508     {
00509       arm_menu_handler.setCheckState( handle, MenuHandler::UNCHECKED );
00510       ROS_INFO("Switching to cached commands");
00511       immediate_commands = false;
00512     }
00513     else
00514     {
00515       arm_menu_handler.setCheckState( handle, MenuHandler::CHECKED );
00516       ROS_INFO("Switching to immediate comands");
00517       immediate_commands = true;
00518     }
00519     arm_menu_handler.reApply(server);
00520     
00521     server.applyChanges();
00522   }
00523   
00524   void resetPoseCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00525   {
00526     resetMarker();
00527     arm_timer.start();
00528   }
00529   
00530   void switchToJointControlCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00531   {
00532     server.clear();
00533     server.applyChanges();
00534     
00535     createJointMarkers();
00536     createJointMenus();
00537   }
00538   
00539   void switchToArmControlCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00540   {
00541     server.clear();
00542     server.applyChanges();
00543     
00544     createArmMarker();
00545     createGripperMarker();
00546     //createArmMenu();
00547     
00548     arm_menu_handler.reApply(server);
00549     
00550     resetMarker();
00551   }
00552 };
00553 
00554 int main(int argc, char** argv)
00555 {
00556   ros::init(argc, argv, "turtlebot_arm_marker_server");
00557   ArmMarkerServer turtleserver;
00558   
00559   ros::spin();
00560 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


maxwell_interactive_markers
Author(s): Michael Ferguson
autogenerated on Tue Mar 19 2013 19:51:27