#include <pr2_gripper_sensor_controller.h>

Inheritance diagram for pr2_gripper_sensor_controller::PR2GripperSensorController:

Public Member Functions
virtual bool	init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)

virtual void	starting ()

virtual void	stopping ()

virtual void	update ()

Public Member Functions inherited from pr2_controller_interface::Controller
	Controller ()

bool	getController (const std::string &name, int sched, ControllerType *&c)

bool	initRequest (ControllerProvider cp, pr2_mechanism_model::RobotState robot, ros::NodeHandle &n)

bool	isRunning ()

void	starting (const ros::Time &time)

bool	startRequest ()

void	stopping (const ros::Time &time)

bool	stopRequest ()

void	update (const ros::Time &time, const ros::Duration &period)

void	updateRequest ()

virtual	~Controller ()

Private Member Functions
void	eventDetectorCB (const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr &msg)

void	findContactCB (const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr &msg)

bool	forceServo ()

void	forceServoCB (const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr &msg)

bool	initializeHandles (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)

void	positionCB (const pr2_controllers_msgs::Pr2GripperCommandConstPtr &msg)

void	reinitializeValues ()

bool	reloadParams (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)

void	slipServoCB (const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr &msg)

bool	stopMotorOutput (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)

bool	updateZeros (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)

Private Attributes
double	acc_trigger
	value to use as trigger for acceleration detection (event_detector) More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperFindContactData > >	contact_state_publisher_
	find_contact real-time publisher More...

bool	contact_success
	flag to indicate whether the fingers found contact or not More...

int	contactCounter
	counter to delay a certain amount of tmie after contact More...

int	contacts_desired
	the contact state we want when we search for contact on the fingers More...

int	control_mode
	the current/desired control mode we want the controller state machine to be in More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointControllerState > >	controller_state_publisher_
	position data real-time publisher More...

double	deformation_limit
	the max allowable deformation for our controller More...

std_srvs::Empty::Request	empty_req
	blank request/responses More...

std_srvs::Empty::Response	empty_resp

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperEventDetectorData > >	event_detector_state_publisher_
	place publisher and listener More...

int	findContact_delay
	counter to track how long a force is stable for More...

double	fingertip_force_limit
	the limit we do not want to exceed when force servoing More...

double	fingertip_start_force
	the force we want to start seroing to on the fingertips More...

double	force_servo_velocity_tolerance
	the tolerance on velocity to consider a force stable More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperForceServoData > >	force_state_publisher_
	force_servo real-time publisher More...

ros::Time	last_time_
	counter to track the time More...

int	loop_count_
	loop counter More...

double	max_effort
	the max effort to allow our controller to use More...

accelerationObserver *	myAccelerationObserver
	accelerometer observer object More...

gripperController *	myGripperController
	gripper action object to control our gripper More...

pressureObserver *	myPressureObserver
	pressure observer object to do analysis on the incoming pressure sensor data More...

ros::NodeHandle	nodeHandle
	our ROS node handle More...

int	placeConditions
	storage container for the conditions we want to use in event_detector More...

bool	placedState
	state flag to indicate when event_detector succceded More...

bool	publish_raw_data
	flag to indicate whether we want to publish raw data or not More...

int	publish_skip
	skip count when publishing controller information More...

pr2_gripper_sensor_msgs::PR2GripperSensorRawData	raw_data
	an object for raw data publication for debugging More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSensorRawData > >	raw_data_publisher_
	raw data real-time publisher More...

ros::ServiceServer	reloadParams_srv_

pr2_mechanism_model::RobotState *	robotHandle
	our robot state handle More...

const pr2_gripper_sensor_msgs::PR2GripperSensorRTState	rt_state_def
	object that defines states of our controller More...

double	servo_force
	the force we want to servo our controller to More...

double	servo_position
	the position we want to servo our controller to More...

double	servo_velocity
	the velocity we would like our controller to servo to More...

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSlipServoData > >	slip_state_publisher_
	slip_servo real-time publisher More...

double	slip_trigger
	value to use as trigger for slip detection (event_detector) More...

bool	stable_contact
	state variable to indicate whether our contact has been held enough More...

bool	stable_force
	flag to indicate a stable force is achieved More...

ros::ServiceServer	stopMotorOutput_srv_

ros::Subscriber	sub_command_

ros::Subscriber	sub_event_detector_command_

ros::Subscriber	sub_findcontact_command_

ros::Subscriber	sub_forceservo_command_

ros::Subscriber	sub_slipservo_command_

bool	update_zeros
	bool to signal whether we are currently updating our zeros or not More...

ros::ServiceServer	updateZeros_srv_
	our service publishers More...

Additional Inherited Members
Public Attributes inherited from pr2_controller_interface::Controller
std::vector< std::string >	after_list_

	AFTER_ME

std::vector< std::string >	before_list_

	BEFORE_ME

	CONSTRUCTED

	INITIALIZED

	RUNNING

enum pr2_controller_interface::Controller:: { ... }	state_

Detailed Description

Definition at line 80 of file pr2_gripper_sensor_controller.h.

Member Function Documentation

void PR2GripperSensorController::eventDetectorCB ( const pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr & msg )

private

Function to take in a event_detector request goal request and set the event_detector parameters to trigger on this goal

Parameters

&msg	A pr2_gripper_sensor_msgs::PR2GripperEventDetectorCommandConstPtr that indicates the conditions of the event trigger

Definition at line 691 of file pr2_gripper_sensor_controller.cpp.

void PR2GripperSensorController::findContactCB ( const pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr & msg )

private

Function to take in a find_contact request goal request and place the controller into contact finding control mode.

Parameters

&msg	A pr2_gripper_sensor_msgs::PR2GripperFindContactCommandConstPtr that indicates what the find_contact goal is

Definition at line 659 of file pr2_gripper_sensor_controller.cpp.

bool pr2_gripper_sensor_controller::PR2GripperSensorController::forceServo ( )

private

void PR2GripperSensorController::forceServoCB ( const pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr & msg )

private

This function forces the real-time controller into force servo mode, sending motor control commands to the gripper to attempt to hold a specified force as seen by the finger pressure sensors.

Parameters

&msg	A pr2_gripper_sensor_msgs::PR2GripperForceServoCommandConstPtr that defines the force servo target force

Definition at line 741 of file pr2_gripper_sensor_controller.cpp.

bool PR2GripperSensorController::init	(	pr2_mechanism_model::RobotState *	robot,
		ros::NodeHandle &	n
	)

virtual

ROS real-time controller initialization routine. Establishes action/message/service I/O for real-time controller and inits runtime variables.

Parameters

*robot	A pointer to our robot state instance
&n	The node handle instance of the calling node.

Returns: true if all hardware handles were initialized successfully.

Implements pr2_controller_interface::Controller.

Definition at line 221 of file pr2_gripper_sensor_controller.cpp.

bool PR2GripperSensorController::initializeHandles	(	pr2_mechanism_model::RobotState *	robot,
		ros::NodeHandle &	n
	)

private

In this function we grab a handle to the palm accelerometer, each finger pressure array, and the motor/encoder of the gripper.

Parameters

*robot	A pointer to our robot state instance
&n	The node handle instance of the calling node.

Returns: true if all handles were initialized successfully.

Definition at line 69 of file pr2_gripper_sensor_controller.cpp.

void PR2GripperSensorController::positionCB ( const pr2_controllers_msgs::Pr2GripperCommandConstPtr & msg )

private

Function to take in a position request goal request and place the controller into position control mode.

Parameters

&msg	A pr2_controllers_msgs::Pr2GripperCommandConstPtr that indicates what the position request goal is

Definition at line 630 of file pr2_gripper_sensor_controller.cpp.

void PR2GripperSensorController::reinitializeValues ( )

private

Helper function called during many action commands to reset key stat variables that are used to determine what is happening inside the controller

Definition at line 593 of file pr2_gripper_sensor_controller.cpp.

bool PR2GripperSensorController::reloadParams	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	resp
	)

private

A helper function to reload parameters from the param server

Parameters

&req	An std_srvs::Empty::Request
&resp	An std_srvs::Empty::Response

Returns: returns true when complete

Definition at line 158 of file pr2_gripper_sensor_controller.cpp.

void PR2GripperSensorController::slipServoCB ( const pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr & msg )

private

Function to take in a slip_servo request goal request and set the controller into slip servo mode.

Parameters

&msg	A pr2_gripper_sensor_msgs::PR2GripperSlipServoCommandConstPtr

Definition at line 709 of file pr2_gripper_sensor_controller.cpp.

virtual void pr2_gripper_sensor_controller::PR2GripperSensorController::starting ( )

inlinevirtual

Reimplemented from pr2_controller_interface::Controller.

Definition at line 185 of file pr2_gripper_sensor_controller.h.

bool PR2GripperSensorController::stopMotorOutput	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	resp
	)

private

This function forces the real-time controller into a mode where no information is sent to the motor. Useful for stopping and interrupting other control modes

Parameters

&req	An std_srvs::Empty::Request
&resp	An std_srvs::Empty::Response

Returns: returns true when complete

Definition at line 580 of file pr2_gripper_sensor_controller.cpp.

void PR2GripperSensorController::stopping ( )

virtual

real-time callback to stop controller

Sends a zero-effort command to the motor before quitting

Reimplemented from pr2_controller_interface::Controller.

Definition at line 535 of file pr2_gripper_sensor_controller.cpp.

void PR2GripperSensorController::update ( void )

virtual

real-time thread update callback, should be run at 1khz ros rt rate.

Listens to various command messages and publishes data back on response messages. Upon receiving a command message the controller moves into various different real-time control_modes for for the gripper.

Implements pr2_controller_interface::Controller.

Definition at line 304 of file pr2_gripper_sensor_controller.cpp.

bool PR2GripperSensorController::updateZeros	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	resp
	)

private

This function updates the pressure sensor zero offest value. It will run for 0.25 seconds and average the sensor readings, after which this average is used to offest the senor readings.

Parameters

&req	An std_srvs::Empty::Request
&resp	An std_srvs::Empty::Response

Returns: returns true when complete

Definition at line 556 of file pr2_gripper_sensor_controller.cpp.

Member Data Documentation

double pr2_gripper_sensor_controller::PR2GripperSensorController::acc_trigger

private

value to use as trigger for acceleration detection (event_detector)

Definition at line 102 of file pr2_gripper_sensor_controller.h.

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperFindContactData> > pr2_gripper_sensor_controller::PR2GripperSensorController::contact_state_publisher_

private

find_contact real-time publisher

Definition at line 156 of file pr2_gripper_sensor_controller.h.

bool pr2_gripper_sensor_controller::PR2GripperSensorController::contact_success

private

flag to indicate whether the fingers found contact or not

Definition at line 110 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::contactCounter

private

counter to delay a certain amount of tmie after contact

Definition at line 97 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::contacts_desired

private

the contact state we want when we search for contact on the fingers

Definition at line 115 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::control_mode

private

the current/desired control mode we want the controller state machine to be in

Definition at line 109 of file pr2_gripper_sensor_controller.h.

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointControllerState> > pr2_gripper_sensor_controller::PR2GripperSensorController::controller_state_publisher_

private

position data real-time publisher

Definition at line 149 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::deformation_limit

private

the max allowable deformation for our controller

Definition at line 116 of file pr2_gripper_sensor_controller.h.

std_srvs::Empty::Request pr2_gripper_sensor_controller::PR2GripperSensorController::empty_req

private

blank request/responses

Definition at line 137 of file pr2_gripper_sensor_controller.h.

std_srvs::Empty::Response pr2_gripper_sensor_controller::PR2GripperSensorController::empty_resp

private

Definition at line 138 of file pr2_gripper_sensor_controller.h.

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperEventDetectorData> > pr2_gripper_sensor_controller::PR2GripperSensorController::event_detector_state_publisher_

private

place publisher and listener

Definition at line 177 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::findContact_delay

private

counter to track how long a force is stable for

Definition at line 105 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::fingertip_force_limit

private

the limit we do not want to exceed when force servoing

Definition at line 118 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::fingertip_start_force

private

the force we want to start seroing to on the fingertips

Definition at line 117 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::force_servo_velocity_tolerance

private

the tolerance on velocity to consider a force stable

Definition at line 119 of file pr2_gripper_sensor_controller.h.

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperForceServoData> > pr2_gripper_sensor_controller::PR2GripperSensorController::force_state_publisher_

private

force_servo real-time publisher

Definition at line 170 of file pr2_gripper_sensor_controller.h.

ros::Time pr2_gripper_sensor_controller::PR2GripperSensorController::last_time_

private

counter to track the time

Definition at line 124 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::loop_count_

private

loop counter

Definition at line 96 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::max_effort

private

the max effort to allow our controller to use

Definition at line 113 of file pr2_gripper_sensor_controller.h.

accelerationObserver* pr2_gripper_sensor_controller::PR2GripperSensorController::myAccelerationObserver

private

accelerometer observer object

Definition at line 126 of file pr2_gripper_sensor_controller.h.

gripperController* pr2_gripper_sensor_controller::PR2GripperSensorController::myGripperController

private

gripper action object to control our gripper

Definition at line 127 of file pr2_gripper_sensor_controller.h.

pressureObserver* pr2_gripper_sensor_controller::PR2GripperSensorController::myPressureObserver

private

pressure observer object to do analysis on the incoming pressure sensor data

Definition at line 128 of file pr2_gripper_sensor_controller.h.

ros::NodeHandle pr2_gripper_sensor_controller::PR2GripperSensorController::nodeHandle

private

our ROS node handle

Definition at line 83 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::placeConditions

private

storage container for the conditions we want to use in event_detector

Definition at line 101 of file pr2_gripper_sensor_controller.h.

bool pr2_gripper_sensor_controller::PR2GripperSensorController::placedState

private

state flag to indicate when event_detector succceded

Definition at line 99 of file pr2_gripper_sensor_controller.h.

bool pr2_gripper_sensor_controller::PR2GripperSensorController::publish_raw_data

private

flag to indicate whether we want to publish raw data or not

Definition at line 122 of file pr2_gripper_sensor_controller.h.

int pr2_gripper_sensor_controller::PR2GripperSensorController::publish_skip

private

skip count when publishing controller information

Definition at line 106 of file pr2_gripper_sensor_controller.h.

pr2_gripper_sensor_msgs::PR2GripperSensorRawData pr2_gripper_sensor_controller::PR2GripperSensorController::raw_data

private

an object for raw data publication for debugging

Definition at line 121 of file pr2_gripper_sensor_controller.h.

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSensorRawData> > pr2_gripper_sensor_controller::PR2GripperSensorController::raw_data_publisher_

private

raw data real-time publisher

Definition at line 144 of file pr2_gripper_sensor_controller.h.

ros::ServiceServer pr2_gripper_sensor_controller::PR2GripperSensorController::reloadParams_srv_

private

Definition at line 133 of file pr2_gripper_sensor_controller.h.

pr2_mechanism_model::RobotState* pr2_gripper_sensor_controller::PR2GripperSensorController::robotHandle

private

our robot state handle

Definition at line 84 of file pr2_gripper_sensor_controller.h.

const pr2_gripper_sensor_msgs::PR2GripperSensorRTState pr2_gripper_sensor_controller::PR2GripperSensorController::rt_state_def

private

object that defines states of our controller

Definition at line 120 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::servo_force

private

the force we want to servo our controller to

Definition at line 111 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::servo_position

private

the position we want to servo our controller to

Definition at line 112 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::servo_velocity

private

the velocity we would like our controller to servo to

Definition at line 114 of file pr2_gripper_sensor_controller.h.

boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_gripper_sensor_msgs::PR2GripperSlipServoData> > pr2_gripper_sensor_controller::PR2GripperSensorController::slip_state_publisher_

private

slip_servo real-time publisher

Definition at line 163 of file pr2_gripper_sensor_controller.h.

double pr2_gripper_sensor_controller::PR2GripperSensorController::slip_trigger

private

value to use as trigger for slip detection (event_detector)

Definition at line 103 of file pr2_gripper_sensor_controller.h.

bool pr2_gripper_sensor_controller::PR2GripperSensorController::stable_contact

private

state variable to indicate whether our contact has been held enough

Definition at line 98 of file pr2_gripper_sensor_controller.h.

bool pr2_gripper_sensor_controller::PR2GripperSensorController::stable_force