51 #include <std_srvs/Empty.h> 74 if(!hardwareInterface)
76 ROS_ERROR(
"Perhaps Something wrong with the hardware interface pointer!!!!");
80 std::string accelerometer_name;
81 if(!n.
getParam(
"accelerometer_name", accelerometer_name))
88 if(!accelerometerHandle)
90 ROS_ERROR(
"PR2GripperSensorController could not find sensor named '%s'", accelerometer_name.c_str());
96 std::string leftFinger_pressureSensor_name;
97 if(!n.
getParam(
"left_pressure_sensor_name", leftFinger_pressureSensor_name))
103 if(!leftFinger_pressureSensorHandle)
105 ROS_ERROR(
"PR2GripperSensorController could not find sensor named '%s'", leftFinger_pressureSensor_name.c_str());
110 std::string rightFinger_pressureSensor_name;
111 if(!n.
getParam(
"right_pressure_sensor_name", rightFinger_pressureSensor_name))
117 if(!rightFinger_pressureSensorHandle)
119 ROS_ERROR(
"PR2GripperSensorController could not find sensor named '%s'", rightFinger_pressureSensor_name.c_str());
125 std::string joint_name;
126 if (!n.
getParam(
"joint_name", joint_name))
134 ROS_ERROR(
"PR2GripperSensorController could not find joint named '%s'", joint_name.c_str());
165 ROS_ERROR(
"Incorrect sign on close_speed (negative speed is impossible), setting to 0!");
283 std_srvs::Empty::Request req;
284 std_srvs::Empty::Response rsp;
394 ROS_ERROR(
"PR2_GRIPPER_SENSOR_CONTROLLER ENTERED A BAD STATE MACHINE STATE");
562 ROS_INFO(
".... zeros finished updating");
582 ROS_INFO(
"Stopping gripper command output.");
663 ROS_ERROR(
"REFUSING TO FIND CONTACT - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!");
713 ROS_ERROR(
"REFUSING TO SLIP SERVO - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!");
745 ROS_ERROR(
"REFUSING TO FORCE SERVO - PRESSURE SENSOR HAS ZERO READING AND MAY BE MALFUNCTIONING!");
751 if(msg->fingertip_force >= 0)
double servo_force
the force we want to servo our controller to
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
double fingertip_force_limit
int control_mode
the current/desired control mode we want the controller state machine to be in
Accelerometer * getAccelerometer(const std::string &name) const
bool publish_raw_data
flag to indicate whether we want to publish raw data or not
double force_servo_force_tolerance
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 gripper_state_now_velocity
bool deformation_limit_flag
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
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
double padForce_left_cur_nonbiased
ros::Subscriber sub_findcontact_command_
ros::Subscriber sub_event_detector_command_
bool positionServo(double desiredPos, double desiredVel)
void positionCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr &msg)
bool stopMotorOutput(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
pr2_hardware_interface::HardwareInterface * hw_
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSensorRawData > > raw_data_publisher_
raw data real-time publisher
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_cur_bias
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)
bool grabObject(double close_speed, int contactsDesired)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
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
pr2_mechanism_model::JointState * jointState
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
double padForce_right_cur
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)
double gripper_state_now_measured_effort
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)
bool place(int placeConditions, double acc_trigger, double slip_trigger)
bool forceServo2(double desired_Force)
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)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::Subscriber sub_forceservo_command_
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperForceServoData > > force_state_publisher_
force_servo real-time publisher
const std::string & getNamespace() const
double positionMarker_limit
PressureSensor * getPressureSensor(const std::string &name) const
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
pr2_gripper_sensor_msgs::PR2GripperPressureData pressure_current_zerod
JointState * getJointState(const std::string &name)
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
bool getParam(const std::string &key, std::string &s) const
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
int loop_count_
loop counter
double force_servo_velocity_tolerance
the tolerance on velocity to consider a force stable
double gripper_state_now_position
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
double vel_integral_vcontrol
ros::Subscriber sub_command_
ros::Subscriber sub_slipservo_command_
double padForce_right_cur_nonbiased
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
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