Action class for the EECartImped controller. More...

#include <ee_cart_imped_action.hh>

Public Member Functions
	EECartImpedExecuter (ros::NodeHandle &n)
	Constructs the executer.
	~EECartImpedExecuter ()
	Deconstructs the executor. Shuts down the controller and stops EECartImpedExecuter::watchdog_timer_.
Private Types
typedef actionlib::ActionServer < ee_cart_imped_msgs::EECartImpedAction >	EECIAS
	The action server type.
typedef EECIAS::GoalHandle	GoalHandle
	The goal handle type.
Private Member Functions
void	cancelCB (GoalHandle gh)
	Callback function when a goal is cancelled.
bool	checkConstraints (const ee_cart_imped_msgs::EECartImpedFeedbackConstPtr &msg, geometry_msgs::Pose pose_constraints, double effort_constraint)
	Helper function to check if the actual point is within the constraints of the desired point.
void	controllerStateCB (const ee_cart_imped_msgs::EECartImpedFeedbackConstPtr &msg)
	Callback function when a message is received from the controller via EECartImpedExecuter::sub_controller_state_.
void	goalCB (GoalHandle gh)
	Callback function when a new goal is received from EECartImpedExecuter::action_server_.
void	watchdog (const ros::TimerEvent &e)
	Triggered every second by EECartImpedExecuter::watchdog_timer_ to make sure the controller is still alive.
Private Attributes
EECIAS	action_server_
	The action server.
GoalHandle	active_goal_
	The handle to the current active goal.
bool	check_header_
	True if the header should be checked when receiving a new goal.
std::string	controller_frame_id_
	The frame in which the controller expects the trajectory.
ee_cart_imped_msgs::EECartImpedGoal	current_traj_
	The current trajectory the robot is following.
geometry_msgs::Pose	goal_constraints_
	Constraints on the goal position.
double	goal_effort_constraint_
	Goal constraint on the squared error of the difference between the requested joint torques and the actual ones.
double	goal_time_constraint_
	If the goal takes more goal_time_constraint_ over the time it was supposed to end, it is aborted.
bool	has_active_goal_
	True if the controller is actively pursuing a goal.
ee_cart_imped_msgs::EECartImpedFeedbackConstPtr	last_controller_state_
	The state of the controller at the last message received via EECartImpedExecuter::sub_controller_state_.
ros::NodeHandle	node_
	ROS node containing namespace information, etc.
ros::Publisher	pub_controller_command_
	Publishes commands to the controller.
ee_cart_imped_msgs::EECartImpedResult	result_
	The result of the last goal.
ros::Subscriber	sub_controller_state_
	Subscribes to the controller state.
tf::TransformListener	tf_listener
	Transformer for transforming goals into the correct frame ids.
geometry_msgs::Pose	trajectory_constraints_
	Constraints on the trajectory position.
double	trajectory_effort_constraint_
	Trajectory constraint on the squared error of the difference between the requested joint torques and the actual ones.
ros::Timer	watchdog_timer_
	Each second this timer triggers and calls EECartImpedExecuter::watchdog()

Detailed Description

Action class for the EECartImped controller.

Implements the action interface to the controller. Accepts goals, monitors the trajectory to make sure it's inside constraints, publishes feedback, and reports when goals have been reached.

Definition at line 21 of file ee_cart_imped_action.hh.

Member Typedef Documentation

typedef actionlib::ActionServer<ee_cart_imped_msgs::EECartImpedAction> EECartImpedExecuter::EECIAS [private]

The action server type.

See the actionlib documentation for more details.

Definition at line 31 of file ee_cart_imped_action.hh.

typedef EECIAS::GoalHandle EECartImpedExecuter::GoalHandle [private]

The goal handle type.

See the actionlib documentation for more details.

Definition at line 40 of file ee_cart_imped_action.hh.

Constructor & Destructor Documentation

EECartImpedExecuter::EECartImpedExecuter ( ros::NodeHandle & n )

Constructs the executer.

Constructs the "ee_cart_imped_action" action server and binds EECartImpedExecuter::goalCB() and EECartImpedExecuter::cancelCB to it. It then waits for the controller to come online and, once it has, starts the action server. It also advertises "command", subscribes to "state" (within the namespace of the passed in NodeHandle), reads the constraints off the parameter server, and initializes EECartImpedExecuter::watchdog_timer_ to trigger each second.

Parameters:

n	The NodeHandle for constructing this instance of the executer.

Definition at line 6 of file ee_cart_imped_action.cpp.

EECartImpedExecuter::~EECartImpedExecuter ( )

Deconstructs the executor. Shuts down the controller and stops EECartImpedExecuter::watchdog_timer_.

Definition at line 76 of file ee_cart_imped_action.cpp.

Member Function Documentation

void EECartImpedExecuter::cancelCB ( GoalHandle gh ) [private]

Callback function when a goal is cancelled.

Parameters:

gh	The goal to cancel.

Definition at line 190 of file ee_cart_imped_action.cpp.

bool EECartImpedExecuter::checkConstraints	(	const ee_cart_imped_msgs::EECartImpedFeedbackConstPtr &	msg,
		geometry_msgs::Pose	pose_constraints,
		double	effort_constraint
	)		`[private]`

Helper function to check if the actual point is within the constraints of the desired point.

Parameters:

msg	A pointer to a EECartImpedFeedback message that is the current state of the controller, including actual position and desired position and current effort error
pose_constraints	The position constraints that must be respected
effort_constraint	The maximum allowable error in the squared difference between the desired effort and the actual effort

Returns:: False if msg->actual_pose is not within constraints of msg->desired.pose in any Cartesian direction or, msg->desired.isForce/Torque is True in some Cartesian direction and msg->effort_sq_error > effort_constraint. True otherwise.

Definition at line 263 of file ee_cart_imped_action.cpp.

void EECartImpedExecuter::controllerStateCB ( const ee_cart_imped_msgs::EECartImpedFeedbackConstPtr & msg ) [private]

Callback function when a message is received from the controller via EECartImpedExecuter::sub_controller_state_.

This function is responsible for making sure the trajectory stays within EECartImpedExecuter::trajectory_constraints_ and EECartImpedExecuter::trajecotry_effort_constraint_. If the current point is not within the trajectory constraints, the goal is cancelled and the trajectory is stopped.

This function is also responsible for checking if the goal has been reached. Once the current time is greater than the time it should have taken to accomplish the whole trajectory, it checks whether the current point is within EECartImpedExecuter::goal_constraints_ and EECartImpedExecuter::goal_effort_constraint_ of the goal point. If so, it marks the goal as succeeded. Otherwise, it checks whether the current time is greater than the time the trajectory should have been finished plus EECartImpedExecuter::goal_time_constraint_. If it is and the current point is not within the goal constraints, it aborts the goal.

Parameters:

msg	A pointer to a EECartImpedFeedback message that is the current controller state as published on state.

Definition at line 209 of file ee_cart_imped_action.cpp.

void EECartImpedExecuter::goalCB ( GoalHandle gh ) [private]

Callback function when a new goal is received from EECartImpedExecuter::action_server_.

Cancels the currently active goal if there is one and sends the new goal to the controller via EECartImpedExecuter::pub_controller_command_. Transforms each goal point to be in the frame of the root link for the controller.

Parameters:

gh	A handle to the new goal

Definition at line 117 of file ee_cart_imped_action.cpp.

void EECartImpedExecuter::watchdog ( const ros::TimerEvent & e ) [private]

Triggered every second by EECartImpedExecuter::watchdog_timer_ to make sure the controller is still alive.

Checks when the last time we heard from the controller was. If it was more than 5 seconds ago or we have never heard from it, the current goal is aborted.

This function does not check the trajectory and goal constraints; those are managed in EECartImpedExecuter::controllerStateCB().

Parameters: