54 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactData.h> 55 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactCommand.h> 56 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoData.h> 57 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoCommand.h> 58 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoData.h> 59 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoCommand.h> 60 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorData.h> 61 #include <pr2_gripper_sensor_msgs/PR2GripperEventDetectorCommand.h> 62 #include <pr2_gripper_sensor_msgs/PR2GripperSensorRTState.h> 63 #include <pr2_gripper_sensor_msgs/PR2GripperSensorRawData.h> 64 #include <std_srvs/Empty.h> 65 #include <std_msgs/Empty.h> 69 #include <pr2_controllers_msgs/JointControllerState.h> 70 #include <pr2_controllers_msgs/Pr2GripperCommand.h> 71 #include <boost/scoped_ptr.hpp> 72 #include <boost/thread/condition.hpp> 75 #include <std_msgs/Float64.h> 90 bool updateZeros(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp);
91 bool stopMotorOutput(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp);
93 bool reloadParams(std_srvs::Empty::Request& req,std_srvs::Empty::Response& resp);
121 pr2_gripper_sensor_msgs::PR2GripperSensorRawData
raw_data;
151 void positionCB(
const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg);
158 void findContactCB(
const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr& msg);
165 void slipServoCB(
const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr& msg);
172 void forceServoCB(
const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr& msg);
179 void eventDetectorCB(
const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr& msg);
double servo_force
the force we want to servo our controller to
int control_mode
the current/desired control mode we want the controller state machine to be in
std_srvs::Empty::Request empty_req
blank request/responses
bool publish_raw_data
flag to indicate whether we want to publish raw data or not
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperFindContactData > > contact_state_publisher_
find_contact real-time publisher
int placeConditions
storage container for the conditions we want to use in event_detector
bool reloadParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
double fingertip_start_force
the force we want to start seroing to on the fingertips
void reinitializeValues()
double servo_position
the position we want to servo our controller to
int contacts_desired
the contact state we want when we search for contact on the fingers
ros::Subscriber sub_findcontact_command_
ros::Subscriber sub_event_detector_command_
void positionCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr &msg)
bool stopMotorOutput(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSensorRawData > > raw_data_publisher_
raw data real-time publisher
int contactCounter
counter to delay a certain amount of tmie after contact
double slip_trigger
value to use as trigger for slip detection (event_detector)
std_srvs::Empty::Response empty_resp
int findContact_delay
counter to track how long a force is stable for
ros::ServiceServer updateZeros_srv_
our service publishers
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSlipServoData > > slip_state_publisher_
slip_servo real-time publisher
double max_effort
the max effort to allow our controller to use
accelerationObserver * myAccelerationObserver
accelerometer observer object
ros::Time last_time_
counter to track the time
ros::ServiceServer stopMotorOutput_srv_
double fingertip_force_limit
the limit we do not want to exceed when force servoing
void slipServoCB(const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr &msg)
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperEventDetectorData > > event_detector_state_publisher_
place publisher and listener
bool update_zeros
bool to signal whether we are currently updating our zeros or not
virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
double acc_trigger
value to use as trigger for acceleration detection (event_detector)
ros::ServiceServer reloadParams_srv_
bool updateZeros(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
ros::Subscriber sub_forceservo_command_
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperForceServoData > > force_state_publisher_
force_servo real-time publisher
void forceServoCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr &msg)
bool stable_contact
state variable to indicate whether our contact has been held enough
gripperController * myGripperController
gripper action object to control our gripper
pressureObserver * myPressureObserver
pressure observer object to do analysis on the incoming pressure sensor data
int publish_skip
skip count when publishing controller information
ros::NodeHandle nodeHandle
our ROS node handle
void findContactCB(const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr &msg)
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointControllerState > > controller_state_publisher_
position data real-time publisher
pr2_gripper_sensor_msgs::PR2GripperSensorRawData raw_data
an object for raw data publication for debugging
int loop_count_
loop counter
double force_servo_velocity_tolerance
the tolerance on velocity to consider a force stable
bool stable_force
flag to indicate a stable force is achieved
pr2_mechanism_model::RobotState * robotHandle
our robot state handle
bool initializeHandles(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
const pr2_gripper_sensor_msgs::PR2GripperSensorRTState rt_state_def
object that defines states of our controller
ros::Subscriber sub_command_
ros::Subscriber sub_slipservo_command_
double servo_velocity
the velocity we would like our controller to servo to
void eventDetectorCB(const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr &msg)
double deformation_limit
the max allowable deformation for our controller
bool placedState
state flag to indicate when event_detector succceded
bool contact_success
flag to indicate whether the fingers found contact or not