#include <PositionController.h>
Classes | |
struct | JointStruct |
Public Member Functions | |
virtual bool | init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) |
Initializes the PositionController by creating joint objects. | |
PositionController () | |
void | referenceCallback (const sensor_msgs::JointState::ConstPtr &msg) |
Updates the goal position of all joints. | |
virtual void | starting () |
Resets all threads of the joints and sets their position. | |
virtual void | stopping () |
Shuts down the subscription and advertisement of the topics. | |
virtual void | update () |
Updates the effort on each joint. Also publishes a JointState message on the measurements topic. | |
virtual | ~PositionController () |
Destructor for PositionController. | |
Private Attributes | |
ros::Time | current_time |
std::map< std::string, JointStruct * > | joint_map_ |
maps the joint names to the structs | |
std::vector< JointStruct * > | joint_structs_ |
ros::Publisher | measurements_pub_ |
publisher of the JointState messages | |
ros::NodeHandle | node_ |
ros::Subscriber | references_sub_ |
subscriber for the messages containing the references | |
pr2_mechanism_model::RobotState * | robot_ |
point to the robot | |
ros::Time | time_of_last_cycle_ |
Definition at line 65 of file PositionController.h.
Definition at line 54 of file PositionController.cpp.
PositionController::~PositionController | ( | ) | [virtual] |
Destructor for PositionController.
Removes all controllers of all joints from memory and then removes the structures that bind them from memory
Definition at line 58 of file PositionController.cpp.
bool PositionController::init | ( | pr2_mechanism_model::RobotState * | robot, |
ros::NodeHandle & | n | ||
) | [virtual] |
Initializes the PositionController by creating joint objects.
Makes a joint_struct for every joint in the robot. If no error occurs subscribes to references and advertises it wants to publish it's measurements.
robot | a pointer to the robot |
n | the handler for this nodes subscriptions and advertisements. |
Implements pr2_controller_interface::Controller.
Definition at line 88 of file PositionController.cpp.
void PositionController::referenceCallback | ( | const sensor_msgs::JointState::ConstPtr & | msg | ) |
Updates the goal position of all joints.
Updates the reference_ values from all joints by taking into account a new JointReference message. It is safe in the regard that messages containing reference values bigger than max_pos_ or smaller than min_pos_ are set to these respective values instead.
msg | a JointReference message |
Definition at line 66 of file PositionController.cpp.
void PositionController::starting | ( | ) | [virtual] |
Resets all threads of the joints and sets their position.
Loops through all joint_structs and resets them.
Reimplemented from pr2_controller_interface::Controller.
Definition at line 173 of file PositionController.cpp.
void PositionController::stopping | ( | ) | [virtual] |
Shuts down the subscription and advertisement of the topics.
Shuts down the subscription on topic references and shuts down the advertisement on topic measurements.
Reimplemented from pr2_controller_interface::Controller.
Definition at line 221 of file PositionController.cpp.
void PositionController::update | ( | void | ) | [virtual] |
Updates the effort on each joint. Also publishes a JointState message on the measurements topic.
Implements pr2_controller_interface::Controller.
Definition at line 182 of file PositionController.cpp.
Definition at line 104 of file PositionController.h.
std::map<std::string, JointStruct*> controller::PositionController::joint_map_ [private] |
maps the joint names to the structs
Definition at line 98 of file PositionController.h.
Definition at line 96 of file PositionController.h.
publisher of the JointState messages
Definition at line 107 of file PositionController.h.
Definition at line 109 of file PositionController.h.
subscriber for the messages containing the references
Definition at line 106 of file PositionController.h.
point to the robot
Definition at line 100 of file PositionController.h.
Definition at line 102 of file PositionController.h.