Manages moving joints to a new pose, determining when they are settled, and returning current joint_states. More...
#include <chain_manager.h>
Classes | |
struct | ChainController |
Public Member Functions | |
ChainManager (ros::NodeHandle &nh, double wait_time=15.0) | |
Constructor, sets up chains from ros parameters. | |
std::vector< std::string > | getChainJointNames (const std::string &chain_name) |
Get the joint names associated with a chain. Mainly for testing. | |
std::vector< std::string > | getChains () |
Get the names of chains. Mainly for testing. | |
std::string | getPlanningGroupName (const std::string &chain_name) |
bool | getState (sensor_msgs::JointState *state) |
Get the current JointState message. | |
bool | moveToState (const sensor_msgs::JointState &state) |
Send commands to all managed joints. The ChainManager automatically figures out which controller to send these to. | |
bool | waitToSettle () |
Wait for joints to settle. | |
Private Types | |
typedef actionlib::SimpleActionClient < moveit_msgs::MoveGroupAction > | MoveGroupClient |
typedef boost::shared_ptr < MoveGroupClient > | MoveGroupClientPtr |
typedef boost::shared_ptr < const moveit_msgs::MoveGroupResult > | MoveGroupResultPtr |
typedef actionlib::SimpleActionClient < control_msgs::FollowJointTrajectoryAction > | TrajectoryClient |
Private Member Functions | |
trajectory_msgs::JointTrajectoryPoint | makePoint (const sensor_msgs::JointState &state, const std::vector< std::string > &joints) |
void | stateCallback (const sensor_msgs::JointStateConstPtr &msg) |
Private Attributes | |
std::vector< boost::shared_ptr < ChainController > > | controllers_ |
double | duration_ |
MoveGroupClientPtr | move_group_ |
sensor_msgs::JointState | state_ |
boost::mutex | state_mutex_ |
ros::Subscriber | subscriber_ |
double | velocity_factor_ |
Manages moving joints to a new pose, determining when they are settled, and returning current joint_states.
Definition at line 37 of file chain_manager.h.
typedef actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> robot_calibration::ChainManager::MoveGroupClient [private] |
Definition at line 40 of file chain_manager.h.
typedef boost::shared_ptr<MoveGroupClient> robot_calibration::ChainManager::MoveGroupClientPtr [private] |
Definition at line 41 of file chain_manager.h.
typedef boost::shared_ptr<const moveit_msgs::MoveGroupResult> robot_calibration::ChainManager::MoveGroupResultPtr [private] |
Definition at line 42 of file chain_manager.h.
typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> robot_calibration::ChainManager::TrajectoryClient [private] |
Definition at line 39 of file chain_manager.h.
robot_calibration::ChainManager::ChainManager | ( | ros::NodeHandle & | nh, |
double | wait_time = 15.0 |
||
) |
Constructor, sets up chains from ros parameters.
nh | The node handle, sets namespace for parameters. |
wait_time | The time to wait for each action to come up. |
Definition at line 25 of file chain_manager.cpp.
std::vector< std::string > robot_calibration::ChainManager::getChainJointNames | ( | const std::string & | chain_name | ) |
Get the joint names associated with a chain. Mainly for testing.
Definition at line 285 of file chain_manager.cpp.
std::vector< std::string > robot_calibration::ChainManager::getChains | ( | ) |
Get the names of chains. Mainly for testing.
Definition at line 275 of file chain_manager.cpp.
std::string robot_calibration::ChainManager::getPlanningGroupName | ( | const std::string & | chain_name | ) |
Definition at line 297 of file chain_manager.cpp.
bool robot_calibration::ChainManager::getState | ( | sensor_msgs::JointState * | state | ) |
Get the current JointState message.
Definition at line 122 of file chain_manager.cpp.
trajectory_msgs::JointTrajectoryPoint robot_calibration::ChainManager::makePoint | ( | const sensor_msgs::JointState & | state, |
const std::vector< std::string > & | joints | ||
) | [private] |
Definition at line 130 of file chain_manager.cpp.
bool robot_calibration::ChainManager::moveToState | ( | const sensor_msgs::JointState & | state | ) |
Send commands to all managed joints. The ChainManager automatically figures out which controller to send these to.
Definition at line 154 of file chain_manager.cpp.
void robot_calibration::ChainManager::stateCallback | ( | const sensor_msgs::JointStateConstPtr & | msg | ) | [private] |
Definition at line 84 of file chain_manager.cpp.
Wait for joints to settle.
Definition at line 231 of file chain_manager.cpp.
std::vector<boost::shared_ptr<ChainController> > robot_calibration::ChainManager::controllers_ [private] |
Definition at line 115 of file chain_manager.h.
double robot_calibration::ChainManager::duration_ [private] |
Definition at line 113 of file chain_manager.h.
Definition at line 116 of file chain_manager.h.
sensor_msgs::JointState robot_calibration::ChainManager::state_ [private] |
Definition at line 112 of file chain_manager.h.
boost::mutex robot_calibration::ChainManager::state_mutex_ [private] |
Definition at line 114 of file chain_manager.h.
Definition at line 111 of file chain_manager.h.
double robot_calibration::ChainManager::velocity_factor_ [private] |
Definition at line 117 of file chain_manager.h.