pr2_marker_control: pr2_marker_control: marker

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 // author: Adam Leeper
00031 
00032 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00033 
00034 #include <ros/ros.h>
00035 #include <math.h>
00036 
00037 #include <arm_navigation_msgs/GetStateValidity.h>
00038 
00039 #include <interactive_markers/interactive_marker_server.h>
00040 #include <interactive_markers/menu_handler.h>
00041 
00042 #include <object_manipulator/tools/mechanism_interface.h>
00043 #include <object_manipulator/tools/msg_helpers.h>
00044 #include <object_manipulator/tools/arm_configurations.h>
00045 #include <object_manipulator/tools/vector_tools.h>
00046 #include <object_manipulator/tools/service_action_wrappers.h>
00047 
00048 #include <std_srvs/Empty.h>
00049 
00050 #include <actionlib/client/simple_action_client.h>
00051 #include <actionlib/server/simple_action_server.h>
00052 
00053 #include <boost/thread.hpp>
00054 
00055 #include <tf/transform_listener.h>
00056 
00057 #include <pr2_wrappers/gripper_controller.h>
00058 #include <pr2_wrappers/torso_client.h>
00059 #include <pr2_wrappers/base_client.h>
00060 #include <pr2_wrappers/tuck_arms_client.h>
00061 #include <point_cloud_server/StoreCloudAction.h>
00062 
00063 #include <pr2_object_manipulation_msgs/GetNavPoseAction.h>
00064 #include <pr2_object_manipulation_msgs/GetGripperPoseAction.h>
00065 
00066 #include <interactive_marker_helpers/interactive_marker_helpers.h>
00067 #include "cloud_handler.h"
00068 
00069 
00071 class PR2MarkerControl
00072 {
00073 public:
00074 
00076   PR2MarkerControl();
00077 
00079   ~PR2MarkerControl() { switchToJoint();  }
00080 
00081 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00082 
00084   void fastUpdate();
00085 
00087   void slowUpdate();
00088 
00090   double getJointPosition( std::string name, const arm_navigation_msgs::RobotState& robot_state);
00091 
00093   geometry_msgs::PoseStamped toolToWrist(const geometry_msgs::PoseStamped &ps);
00094 
00096   geometry_msgs::PoseStamped wristToTool(const geometry_msgs::PoseStamped &ps);
00097 
00099   void initAllMarkers(bool apply_immediately = false)
00100   {
00101     initControlMarkers();
00102     initMeshMarkers();
00103 
00104     if(apply_immediately)
00105     {
00106       ROS_INFO_STREAM( "Re-initializing all markers!" );
00107       server_.applyChanges();
00108     }
00109   }
00110 
00112   void initControlMarkers();
00113 
00115   void initMeshMarkers();
00116 
00118   void cancelBaseMovement() {
00119       base_goal_pose_.header.stamp = ros::Time::now();
00120       base_client_.cancelGoals();
00121   }
00122 
00123 
00124 protected:
00125 
00127   void switchToCartesian();
00128 
00130   void switchToJoint();
00131 
00133   bool checkStateValidity(std::string arm_name);
00134 
00136   void gripperToggleModeCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00137 
00138   void snapshotCB() { snapshot_client_.refresh( nh_.resolveName("snapshot_input_topic", true) ); }
00139 
00140   void gripperToggleFixedCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00141 
00142   void gripperToggleControlCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00143 
00144   void gripperResetControlCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00145 
00146   void startDualGrippers( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, bool active );
00147 
00148 
00149   void updateGripper( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id );
00150 
00151   void updateDualGripper( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00152 
00154   void gripperButtonCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int id);
00155 
00156   void upperArmButtonCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int id);
00157 
00158 
00159   void baseButtonCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00160   {
00161     control_state_.base_on_ ^= true;
00162     initControlMarkers();
00163   }
00164 
00165   void refreshPosture( const std::string &arm_name )
00166   {
00167     std::vector<double> arm_angles(7);
00168     for( size_t i = 0; i < arm_angles.size(); i++) arm_angles[i] = 9999;
00169     mechanism_.sendCartesianPostureCommand(arm_name, arm_angles);
00170   }
00171 
00172   void tuckArmsCB( bool tuck_left, bool tuck_right) 
00173   { 
00174     switchToJoint();
00175     tuck_arms_client_.tuckArms( tuck_right, tuck_left, false );  
00176   }
00177 
00178   void updatePosture( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id );
00179 
00180   void projectorMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00181 
00182   void targetPointMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00183 
00184   // Moved to graveyard
00185   //void controllerSelectMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00186 
00187   void updateTorso( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
00188 
00189   void updateBase( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
00190 
00191   void torsoMenuCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
00192 
00193   void gripperClosureCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const float &command);
00194 
00195   void updateHeadGoal( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id );
00196 
00197   void commandGripperPose(const geometry_msgs::PoseStamped &ps, int arm_id, bool use_offset);
00198 
00199   void requestGripperPose(  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, int arm_id);
00200 
00201   void processGripperPoseFeedback(  const pr2_object_manipulation_msgs::GetGripperPoseFeedbackConstPtr &result,
00202                                     const std::string &arm_name);
00203 
00204   void processGripperPoseResult(  const actionlib::SimpleClientGoalState& state,
00205                                   const pr2_object_manipulation_msgs::GetGripperPoseResultConstPtr &result,
00206                                   const std::string &arm_name);
00207 
00208   void moveArm( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
00209                 const std::string &position, bool planner);
00210 
00211   void moveArmThread(std::string arm_name, std::string position, bool collision, bool planner);
00212 
00213   void initMenus();
00214 
00215   void requestNavGoal( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
00216                        const bool & collision_aware );
00217 
00218   void processNavGoal( const actionlib::SimpleClientGoalState& state,
00219                        const pr2_object_manipulation_msgs::GetNavPoseResultConstPtr &result,
00220                        const bool &collision_aware );
00221   
00223   void sendLastNavGoal();
00224   
00225   void clearLocalCostmap( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
00226 
00227 
00228 protected:
00229 
00230   ros::NodeHandle nh_;
00231   ros::NodeHandle pnh_;
00232   ros::Timer spin_timer_;
00233   ros::Timer slow_sync_timer_;
00234   interactive_markers::InteractiveMarkerServer server_;
00235   //ros::Publisher pub_cloud_snapshot_;
00236 
00238   interactive_markers::MenuHandler menu_arms_;
00239   interactive_markers::MenuHandler menu_head_;
00240   interactive_markers::MenuHandler menu_torso_;
00241   interactive_markers::MenuHandler menu_laser_;
00242   interactive_markers::MenuHandler menu_grippers_;
00243   interactive_markers::MenuHandler menu_dual_grippers_;
00244   interactive_markers::MenuHandler menu_gripper_close_;
00245   interactive_markers::MenuHandler menu_base_;
00246 
00248   interactive_markers::MenuHandler::EntryHandle tuck_handle_;
00249   interactive_markers::MenuHandler::EntryHandle joint_handle_;
00250   interactive_markers::MenuHandler::EntryHandle jtranspose_handle_;
00251   interactive_markers::MenuHandler::EntryHandle posture_handle_;
00252   interactive_markers::MenuHandler::EntryHandle projector_handle_;
00253   interactive_markers::MenuHandler::EntryHandle head_target_handle_;
00254   interactive_markers::MenuHandler::EntryHandle gripper_view_facing_handle_;
00255   interactive_markers::MenuHandler::EntryHandle gripper_6dof_handle_;
00256   interactive_markers::MenuHandler::EntryHandle gripper_fixed_control_handle_;
00257   interactive_markers::MenuHandler::EntryHandle gripper_edit_control_handle_;
00258   interactive_markers::MenuHandler::EntryHandle gripper_reset_control_handle_;
00259 
00260   tf::TransformListener tfl_;
00261 
00262   std::vector<tf::Transform> pose_offset_;
00263   geometry_msgs::Pose tool_frame_offset_;
00264   geometry_msgs::PoseStamped head_goal_pose_;
00265 
00266   geometry_msgs::PoseStamped dual_grippers_frame_;
00267   std::vector<tf::Transform> dual_gripper_offsets_;
00268 
00269   geometry_msgs::PoseStamped base_goal_pose_;
00270   
00272   struct GripperState{
00273     GripperState() : on_(false), view_facing_(false), edit_control_(false), torso_frame_(false) {}
00274 
00275     bool on_;
00276     bool view_facing_;
00277     bool edit_control_;
00278     bool torso_frame_;
00279   };
00280 
00282   struct ControlState{
00283     ControlState() : posture_r_(false), posture_l_(false), torso_on_(false), head_on_(false),
00284                      projector_on_(false), init_head_goal_(false), base_on_(false), dual_grippers_(false) {}
00285 
00286     void print()
00287     {
00288       ROS_DEBUG_NAMED("control_state", "gripper: on[%d|%d], edit[%d|%d], torso[%d|%d], dual[%d]",
00289                       l_gripper_.on_, r_gripper_.on_, l_gripper_.edit_control_, r_gripper_.edit_control_, l_gripper_.torso_frame_, r_gripper_.torso_frame_, dual_grippers_ );
00290       ROS_DEBUG_NAMED("control_state", "posture[%d|%d] torso[%d] base[%d] head[%d] projector[%d]",
00291                       posture_l_, posture_r_, torso_on_, base_on_, head_on_, projector_on_ );
00292     }
00293 
00294     bool posture_r_;
00295     bool posture_l_;
00296     bool torso_on_;
00297     bool head_on_;
00298     bool projector_on_;
00299     bool init_head_goal_;
00300     bool base_on_;
00301     bool dual_grippers_;
00302     GripperState r_gripper_;
00303     GripperState l_gripper_;
00304   };
00305 
00306   ControlState control_state_;
00307   pr2_wrappers::GripperController gripper_client_;
00308   pr2_wrappers::TorsoClient torso_client_;
00309   pr2_wrappers::BaseClient base_client_;
00310   pr2_wrappers::TuckArmsClient tuck_arms_client_;
00311 
00312   object_manipulator::MechanismInterface mechanism_;
00313 
00314   bool use_state_validator_;
00315   bool use_right_arm_;
00316   bool use_left_arm_;
00317   double gripper_control_linear_deadband_, gripper_control_angular_deadband_;
00318   double update_period_;
00319   double cartesian_clip_distance_;
00320   double cartesian_clip_angle_;
00321 
00322   double max_direct_nav_radius_;
00323 
00324   bool in_collision_r_, in_collision_l_;
00325 
00326   boost::shared_ptr< boost::thread > sys_thread_;
00327 
00329   object_manipulator::ServiceWrapper<arm_navigation_msgs::GetStateValidity> check_state_validity_client_;
00330   object_manipulator::ServiceWrapper<std_srvs::Empty> collider_node_reset_srv_;
00331   CloudHandler snapshot_client_;
00332 
00334   object_manipulator::ActionWrapper<pr2_object_manipulation_msgs::GetNavPoseAction> base_pose_client_;
00335 
00337   object_manipulator::ActionWrapper<pr2_object_manipulation_msgs::GetGripperPoseAction> gripper_pose_client_;
00338 
00340   int interface_number_;
00342   int task_number_;
00343 
00344   std::string head_pointing_frame_;
00345   bool double_menu_;
00346 
00348   std::string move_base_node_name_;
00349 
00351   bool using_3d_nav_;
00352 };
00353 
00354 
00355
marker_control.h
