controller::Pr2BaseController2Safe Class Reference

#include <pr2_base_controller2_safe.h>

List of all members.

Public Member Functions

geometry_msgs::Twist getCommand ()
 Returns the current position command.
bool init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
 Pr2BaseController2Safe ()
 Default Constructor of the Pr2BaseController2 class.
void setCommand (const geometry_msgs::Twist &cmd_vel)
void setDesiredCasterSteer ()
 set the desired caster steer
void setJointCommands ()
 set the joint commands
void starting ()
void update ()
 (a) Updates commands to caster and wheels. Called every timestep in realtime
 ~Pr2BaseController2Safe ()
 Destructor of the Pr2BaseController2 class.

Public Attributes

BaseKinematics base_kinematics_
 class where the robot's information is computed and stored
pthread_mutex_t pr2_base_controller_lock_
 mutex lock for setting and getting commands

Private Member Functions

void cmdLimitsCB (const safe_base_controller::TwistLimits &cmd_limits)
 callback function which handles pause commands
void commandCallback (const geometry_msgs::TwistConstPtr &msg)
 deal with Twist commands
void computeDesiredCasterSteer (const double &dT)
 computes the desired caster speeds given the desired base speed
void computeDesiredWheelSpeeds (const double &dT)
 computes the desired wheel speeds given the desired base speed
void computeJointCommands (const double &dT)
 computes the desired caster steers and wheel speeds
geometry_msgs::Twist interpolateCommand (const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT)
 interpolates velocities within a given time based on maximum accelerations
void publishState (const ros::Time &current_time)
 Publish the state.
void setDesiredWheelSpeeds ()
 sends the desired wheel speeds to the wheel controllers
void updateJointControllers ()
 tells the wheel and caster controllers to update their speeds

Private Attributes

double alpha_stall_
 low pass filter value used for finding stalls
geometry_msgs::Twist base_vel_msg_
 callback message, used to remember where the base is commanded to go
std::vector< boost::shared_ptr
< JointVelocityController > > 
caster_controller_
 vector that stores the caster controllers
std::vector< control_toolbox::Pid > caster_position_pid_
 The pid controllers for caster position.
filters::MultiChannelTransferFunctionFilter
< double > 
caster_vel_filter_
ros::Time cmd_limits_timestamp_
 timestamp corresponding to when the cmd limits command received by the node
ros::Time cmd_received_timestamp_
 timestamp corresponding to when the command received by the node
ros::Subscriber cmd_sub_
ros::Subscriber cmd_sub_deprecated_
geometry_msgs::Twist cmd_vel_
 speed command vector used internally to represent the current commanded speed
double cmd_vel_rot_eps_
 minimum rotational velocity value allowable
geometry_msgs::Twist cmd_vel_t_
 Input speed command vector represents the desired speed requested by the node. Note that this may differ from the current commanded speed due to acceleration limits imposed by the controller.
double cmd_vel_trans_eps_
 minimum tranlational velocity value allowable
geometry_msgs::Twist desired_vel_
 Input speed command vector represents the desired speed requested by the node.
double eps_
 generic epsilon value that is used as a minimum or maximum allowable input value
std::vector< double > filtered_velocity_
std::vector< double > filtered_wheel_velocity_
double kp_caster_steer_
 local gain used for speed control of the caster (to achieve resultant position control)
ros::Time last_publish_time_
 Time interval between state publishing.
ros::Time last_time_
 time corresponding to when update was last called
geometry_msgs::Twist max_accel_
 acceleration limits specified externally
double max_rotational_velocity_
 maximum rotational velocity magnitude allowable
double max_translational_velocity_
 maximum translational velocity magnitude allowable
geometry_msgs::Twist max_vel_
 velocity limits specified externally
bool new_cmd_available_
 true when new command received by node
ros::NodeHandle node_
bool paused_
 true when controller is paused -> base should stop/not move.
bool publish_state_
ros::NodeHandle root_handle_
double safe_max_x_
 max x velocity limit recevied from vmd_limits message
double safe_max_y_
 max y velocity limit recevied from vmd_limits message
double safe_max_z_
 max z velocity limit recevied from vmd_limits message
double safe_min_x_
 min x velocity limit recevied from vmd_limits message
double safe_min_y_
 min y velocity limit recevied from vmd_limits message
double safe_min_z_
 min z velocity limit recevied from vmd_limits message
double state_publish_rate_
double state_publish_time_
 Time interval between state publishing.
boost::scoped_ptr
< realtime_tools::RealtimePublisher
< pr2_mechanism_controllers::BaseControllerState2 > > 
state_publisher_
 publishes information about the caster and wheel controllers
ros::Subscriber sub_cmd_limits
 subscribes to ~/pause topic
double timeout_
 timeout specifying time that the controller waits before setting the current velocity command to zero
double timeout_vel_limits_
 timeout specifying time that the controller waits for velocity limits before setting the current velocity command to zero
std::vector< boost::shared_ptr
< JointVelocityController > > 
wheel_controller_
 vector that stores the wheel controllers
std::vector< control_toolbox::Pid > wheel_pid_controllers_
 The pid controllers for caster position.
filters::MultiChannelTransferFunctionFilter
< double > 
wheel_vel_filter_

Detailed Description

Definition at line 57 of file pr2_base_controller2_safe.h.


Constructor & Destructor Documentation

controller::Pr2BaseController2Safe::Pr2BaseController2Safe (  ) 

Default Constructor of the Pr2BaseController2 class.

Definition at line 46 of file pr2_base_controller2_safe.cpp.

controller::Pr2BaseController2Safe::~Pr2BaseController2Safe (  ) 

Destructor of the Pr2BaseController2 class.

Definition at line 74 of file pr2_base_controller2_safe.cpp.


Member Function Documentation

void controller::Pr2BaseController2Safe::cmdLimitsCB ( const safe_base_controller::TwistLimits cmd_limits  )  [private]

callback function which handles pause commands

Definition at line 516 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::commandCallback ( const geometry_msgs::TwistConstPtr &  msg  )  [private]

deal with Twist commands

Definition at line 531 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::computeDesiredCasterSteer ( const double &  dT  )  [private]

computes the desired caster speeds given the desired base speed

Definition at line 409 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::computeDesiredWheelSpeeds ( const double &  dT  )  [private]

computes the desired wheel speeds given the desired base speed

Definition at line 462 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::computeJointCommands ( const double &  dT  )  [private]

computes the desired caster steers and wheel speeds

Definition at line 393 of file pr2_base_controller2_safe.cpp.

geometry_msgs::Twist controller::Pr2BaseController2Safe::getCommand (  ) 

Returns the current position command.

Returns:
Current velocity command

Definition at line 273 of file pr2_base_controller2_safe.cpp.

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

Definition at line 81 of file pr2_base_controller2_safe.cpp.

geometry_msgs::Twist controller::Pr2BaseController2Safe::interpolateCommand ( const geometry_msgs::Twist &  start,
const geometry_msgs::Twist &  end,
const geometry_msgs::Twist &  max_rate,
const double &  dT 
) [private]

interpolates velocities within a given time based on maximum accelerations

Definition at line 234 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::publishState ( const ros::Time &  current_time  )  [private]

Publish the state.

Definition at line 353 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::setCommand ( const geometry_msgs::Twist &  cmd_vel  ) 

Definition at line 201 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::setDesiredCasterSteer (  ) 

set the desired caster steer

Definition at line 454 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::setDesiredWheelSpeeds (  )  [private]

sends the desired wheel speeds to the wheel controllers

Definition at line 500 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::setJointCommands (  ) 

set the joint commands

Definition at line 402 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::starting (  ) 

Definition at line 284 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::update (  ) 

(a) Updates commands to caster and wheels. Called every timestep in realtime

Definition at line 298 of file pr2_base_controller2_safe.cpp.

void controller::Pr2BaseController2Safe::updateJointControllers (  )  [private]

tells the wheel and caster controllers to update their speeds

Definition at line 508 of file pr2_base_controller2_safe.cpp.


Member Data Documentation

low pass filter value used for finding stalls

Definition at line 209 of file pr2_base_controller2_safe.h.

class where the robot's information is computed and stored

Returns:
BaseKinematic instance that is being used by this controller

Definition at line 97 of file pr2_base_controller2_safe.h.

geometry_msgs::Twist controller::Pr2BaseController2Safe::base_vel_msg_ [private]

callback message, used to remember where the base is commanded to go

Definition at line 304 of file pr2_base_controller2_safe.h.

std::vector<boost::shared_ptr<JointVelocityController> > controller::Pr2BaseController2Safe::caster_controller_ [private]

vector that stores the caster controllers

Definition at line 219 of file pr2_base_controller2_safe.h.

std::vector<control_toolbox::Pid> controller::Pr2BaseController2Safe::caster_position_pid_ [private]

The pid controllers for caster position.

Definition at line 342 of file pr2_base_controller2_safe.h.

filters::MultiChannelTransferFunctionFilter<double> controller::Pr2BaseController2Safe::caster_vel_filter_ [private]

Definition at line 344 of file pr2_base_controller2_safe.h.

timestamp corresponding to when the cmd limits command received by the node

Definition at line 299 of file pr2_base_controller2_safe.h.

timestamp corresponding to when the command received by the node

Definition at line 163 of file pr2_base_controller2_safe.h.

Definition at line 126 of file pr2_base_controller2_safe.h.

Definition at line 128 of file pr2_base_controller2_safe.h.

geometry_msgs::Twist controller::Pr2BaseController2Safe::cmd_vel_ [private]

speed command vector used internally to represent the current commanded speed

Definition at line 174 of file pr2_base_controller2_safe.h.

minimum rotational velocity value allowable

Definition at line 314 of file pr2_base_controller2_safe.h.

geometry_msgs::Twist controller::Pr2BaseController2Safe::cmd_vel_t_ [private]

Input speed command vector represents the desired speed requested by the node. Note that this may differ from the current commanded speed due to acceleration limits imposed by the controller.

Definition at line 169 of file pr2_base_controller2_safe.h.

minimum tranlational velocity value allowable

Definition at line 329 of file pr2_base_controller2_safe.h.

geometry_msgs::Twist controller::Pr2BaseController2Safe::desired_vel_ [private]

Input speed command vector represents the desired speed requested by the node.

Definition at line 179 of file pr2_base_controller2_safe.h.

generic epsilon value that is used as a minimum or maximum allowable input value

Definition at line 309 of file pr2_base_controller2_safe.h.

Definition at line 346 of file pr2_base_controller2_safe.h.

Definition at line 350 of file pr2_base_controller2_safe.h.

local gain used for speed control of the caster (to achieve resultant position control)

Definition at line 204 of file pr2_base_controller2_safe.h.

Time interval between state publishing.

Definition at line 324 of file pr2_base_controller2_safe.h.

time corresponding to when update was last called

Definition at line 158 of file pr2_base_controller2_safe.h.

geometry_msgs::Twist controller::Pr2BaseController2Safe::max_accel_ [private]

acceleration limits specified externally

Definition at line 189 of file pr2_base_controller2_safe.h.

maximum rotational velocity magnitude allowable

Definition at line 199 of file pr2_base_controller2_safe.h.

maximum translational velocity magnitude allowable

Definition at line 194 of file pr2_base_controller2_safe.h.

geometry_msgs::Twist controller::Pr2BaseController2Safe::max_vel_ [private]

velocity limits specified externally

Definition at line 184 of file pr2_base_controller2_safe.h.

true when new command received by node

Definition at line 153 of file pr2_base_controller2_safe.h.

ros::NodeHandle controller::Pr2BaseController2Safe::node_ [private]

Definition at line 122 of file pr2_base_controller2_safe.h.

true when controller is paused -> base should stop/not move.

Definition at line 138 of file pr2_base_controller2_safe.h.

mutex lock for setting and getting commands

Definition at line 102 of file pr2_base_controller2_safe.h.

Definition at line 337 of file pr2_base_controller2_safe.h.

Definition at line 124 of file pr2_base_controller2_safe.h.

max x velocity limit recevied from vmd_limits message

Definition at line 269 of file pr2_base_controller2_safe.h.

max y velocity limit recevied from vmd_limits message

Definition at line 279 of file pr2_base_controller2_safe.h.

max z velocity limit recevied from vmd_limits message

Definition at line 289 of file pr2_base_controller2_safe.h.

min x velocity limit recevied from vmd_limits message

Definition at line 274 of file pr2_base_controller2_safe.h.

min y velocity limit recevied from vmd_limits message

Definition at line 284 of file pr2_base_controller2_safe.h.

min z velocity limit recevied from vmd_limits message

Definition at line 294 of file pr2_base_controller2_safe.h.

Definition at line 319 of file pr2_base_controller2_safe.h.

Time interval between state publishing.

Definition at line 319 of file pr2_base_controller2_safe.h.

boost::scoped_ptr<realtime_tools::RealtimePublisher<pr2_mechanism_controllers::BaseControllerState2> > controller::Pr2BaseController2Safe::state_publisher_ [private]

publishes information about the caster and wheel controllers

Definition at line 224 of file pr2_base_controller2_safe.h.

subscribes to ~/pause topic

Definition at line 133 of file pr2_base_controller2_safe.h.

timeout specifying time that the controller waits before setting the current velocity command to zero

Definition at line 143 of file pr2_base_controller2_safe.h.

timeout specifying time that the controller waits for velocity limits before setting the current velocity command to zero

Definition at line 148 of file pr2_base_controller2_safe.h.

std::vector<boost::shared_ptr<JointVelocityController> > controller::Pr2BaseController2Safe::wheel_controller_ [private]

vector that stores the wheel controllers

Definition at line 214 of file pr2_base_controller2_safe.h.

std::vector<control_toolbox::Pid> controller::Pr2BaseController2Safe::wheel_pid_controllers_ [private]

The pid controllers for caster position.

Definition at line 355 of file pr2_base_controller2_safe.h.

filters::MultiChannelTransferFunctionFilter<double> controller::Pr2BaseController2Safe::wheel_vel_filter_ [private]

Definition at line 348 of file pr2_base_controller2_safe.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs


safe_base_controller
Author(s): Sebastian Haug, Sachin Chita, John Hsu, Melonee Wise
autogenerated on Fri Jan 11 10:03:26 2013