00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * \author Joe Romano 00036 *********************************************************************/ 00037 //@author Joe Romano 00038 //@email joeromano@gmail.com 00039 //@brief pr2_gripper_sensor_controller.cpp - controller to read data from 00040 // the sensors on the pr2 gripper (accelerometer in palm, 00041 // pressure arrays on the fingers, and encoder in the hand) 00042 // and publish higher-level useful processed information about 00043 // them, as well as perform low-level control tasks to control 00044 // the gripper based on these signals in real-time. 00045 // 00046 // This controller is inteded to be interacted with through its 00047 // action interface, pr2_gripper_sensor_action. 00048 00049 #include <pr2_controller_interface/controller.h> 00050 #include <pr2_gripper_sensor_controller/gripper_controller.h> 00051 #include <pr2_gripper_sensor_controller/pressure_observer.h> 00052 #include <pr2_gripper_sensor_controller/acceleration_observer.h> 00053 #include <pr2_hardware_interface/hardware_interface.h> 00054 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactData.h> 00055 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactCommand.h> 00056 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoData.h> 00057 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoCommand.h> 00058 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoData.h> 00059 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoCommand.h> 00060 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorData.h> 00061 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorCommand.h> 00062 #include <pr2_gripper_sensor_msgs/PR2GripperSensorRTState.h> 00063 #include <pr2_gripper_sensor_msgs/PR2GripperSensorRawData.h> 00064 #include <std_srvs/Empty.h> 00065 #include <std_msgs/Empty.h> 00066 00067 #include <ros/ros.h> 00068 00069 #include <pr2_controllers_msgs/JointControllerState.h> 00070 #include <pr2_controllers_msgs/Pr2GripperCommand.h> 00071 #include <boost/scoped_ptr.hpp> 00072 #include <boost/thread/condition.hpp> 00073 #include <realtime_tools/realtime_box.h> 00074 #include <realtime_tools/realtime_publisher.h> 00075 #include <std_msgs/Float64.h> 00076 00077 00078 namespace pr2_gripper_sensor_controller{ 00079 00080 class PR2GripperSensorController: public pr2_controller_interface::Controller 00081 { 00082 private: 00083 ros::NodeHandle nodeHandle; 00084 pr2_mechanism_model::RobotState *robotHandle; 00085 00086 00087 // functions --------------- 00088 bool initializeHandles(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00089 bool forceServo(); 00090 bool updateZeros(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp); 00091 bool stopMotorOutput(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp); 00092 void reinitializeValues(); 00093 bool reloadParams(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp); 00094 00095 // variables --------------- 00096 int loop_count_; 00097 int contactCounter; 00098 bool stable_contact; 00099 bool placedState; 00100 bool update_zeros; 00101 int placeConditions; 00102 double acc_trigger; 00103 double slip_trigger; 00104 bool stable_force; 00105 int findContact_delay; 00106 00107 // state variables about our controller 00108 int control_mode; 00109 bool contact_success; 00110 double servo_force; 00111 double servo_position; 00112 double max_effort; 00113 double servo_velocity; 00114 bool contacts_desired; 00115 double deformation_limit; 00116 double fingertip_start_force; 00117 double fingertip_force_limit; 00118 double force_servo_velocity_tolerance; 00119 const pr2_gripper_sensor_msgs::PR2GripperSensorRTState rt_state_def; 00120 pr2_gripper_sensor_msgs::PR2GripperSensorRawData raw_data; 00121 bool publish_raw_data; 00122 00123 ros::Time last_time_; 00124 00125 accelerationObserver *myAccelerationObserver; 00126 gripperController *myGripperController; 00127 pressureObserver *myPressureObserver; 00128 00129 00131 ros::ServiceServer updateZeros_srv_; 00132 ros::ServiceServer reloadParams_srv_; 00133 ros::ServiceServer stopMotorOutput_srv_; 00134 00136 std_srvs::Empty::Request empty_req; 00137 std_srvs::Empty::Response empty_resp; 00138 00139 00141 boost::scoped_ptr< 00142 realtime_tools::RealtimePublisher< 00143 pr2_gripper_sensor_msgs::PR2GripperSensorRawData> > raw_data_publisher_ ; 00144 00146 boost::scoped_ptr< 00147 realtime_tools::RealtimePublisher< 00148 pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ; 00149 ros::Subscriber sub_command_; 00150 void positionCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg); 00151 00153 boost::scoped_ptr< 00154 realtime_tools::RealtimePublisher< 00155 pr2_gripper_sensor_msgs::PR2GripperFindContactData> > contact_state_publisher_ ; 00156 ros::Subscriber sub_findcontact_command_; 00157 void findContactCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr& msg); 00158 00160 boost::scoped_ptr< 00161 realtime_tools::RealtimePublisher< 00162 pr2_gripper_sensor_msgs::PR2GripperSlipServoData> > slip_state_publisher_ ; 00163 ros::Subscriber sub_slipservo_command_; 00164 void slipServoCB(const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr& msg); 00165 00167 boost::scoped_ptr< 00168 realtime_tools::RealtimePublisher< 00169 pr2_gripper_sensor_msgs::PR2GripperForceServoData> > force_state_publisher_ ; 00170 ros::Subscriber sub_forceservo_command_; 00171 void forceServoCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr& msg); 00172 00174 boost::scoped_ptr< 00175 realtime_tools::RealtimePublisher< 00176 pr2_gripper_sensor_msgs::PR2GripperEventDetectorData> > event_detector_state_publisher_ ; 00177 ros::Subscriber sub_event_detector_command_; 00178 void eventDetectorCB(const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr& msg); 00179 00180 public: 00181 virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00182 virtual void update(); 00183 virtual void stopping(); 00184 virtual void starting() {} 00185 00186 }; 00187 }