IRI ROS Specific Driver Class. More...
#include <arm_movements_by_pose_alg.h>
Public Types | |
typedef iri_arm_movements_by_pose::ArmMovementsByPoseConfig | Config |
define config type | |
Public Member Functions | |
ArmMovementsByPoseAlgorithm (void) | |
constructor | |
void | config_update (Config &new_cfg, uint32_t level=0) |
config update | |
std::vector < geometry_msgs::PoseStamped > | get_initial_position () |
Returns initial pose. | |
std::vector < geometry_msgs::PoseStamped > | get_transformed_poses (std::vector< geometry_msgs::PoseStamped > poses, std::vector< float > plane_coefficients) |
PUBLIC ALGORITHM. | |
void | lock (void) |
Lock Algorithm. | |
geometry_msgs::Quaternion | rotate_pitch (geometry_msgs::Quaternion orientation_msgs, double angle) |
Rotates Y axis. | |
geometry_msgs::Quaternion | rotate_roll (geometry_msgs::Quaternion orientation_msgs, double angle) |
Rotates X axis. | |
geometry_msgs::Quaternion | rotate_yaw (geometry_msgs::Quaternion orientation_msgs, double angle) |
Rotates X axis. | |
bool | try_enter (void) |
Tries Access to Algorithm. | |
void | unlock (void) |
Unlock Algorithm. | |
~ArmMovementsByPoseAlgorithm (void) | |
Destructor. | |
Public Attributes | |
Config | config_ |
config variable | |
std::string | tf_target_str |
Target TF frame name. | |
Protected Attributes | |
CMutex | alg_mutex_ |
define config type | |
Private Member Functions | |
geometry_msgs::PoseStamped | get_pose_from_tf_with_offset (geometry_msgs::PoseStamped pose_orig, std::vector< float > plane_coefs) |
Private Attributes | |
double | hand_size_offset |
bool | robot_wam |
tf::TransformBroadcaster | tf_br |
tf::TransformListener | tf_listener |
double | wam_security_distance |
IRI ROS Specific Driver Class.
Definition at line 46 of file arm_movements_by_pose_alg.h.
typedef iri_arm_movements_by_pose::ArmMovementsByPoseConfig ArmMovementsByPoseAlgorithm::Config |
define config type
Define a Config type with the ArmMovementsByPoseConfig. All driver implementations will then use the same variable type Config.
Definition at line 77 of file arm_movements_by_pose_alg.h.
constructor
In this constructor parameters related to the specific driver can be initalized. Those parameters can be also set in the openDriver() function. Attributes from the main node driver class IriBaseDriver such as loop_rate, may be also overload here.
Definition at line 3 of file arm_movements_by_pose_alg.cpp.
Destructor.
This destructor is called when the object is about to be destroyed.
Definition at line 7 of file arm_movements_by_pose_alg.cpp.
void ArmMovementsByPoseAlgorithm::config_update | ( | Config & | new_cfg, |
uint32_t | level = 0 |
||
) |
config update
In this function the driver parameters must be updated with the input config variable. Then the new configuration state will be stored in the Config attribute.
new_cfg | the new driver configuration state |
level | level in which the update is taken place |
Definition at line 11 of file arm_movements_by_pose_alg.cpp.
std::vector< geometry_msgs::PoseStamped > ArmMovementsByPoseAlgorithm::get_initial_position | ( | ) |
Returns initial pose.
Returns initial pose
Definition at line 53 of file arm_movements_by_pose_alg.cpp.
geometry_msgs::PoseStamped ArmMovementsByPoseAlgorithm::get_pose_from_tf_with_offset | ( | geometry_msgs::PoseStamped | pose_orig, |
std::vector< float > | plane_coefs | ||
) | [private] |
Definition at line 27 of file arm_movements_by_pose_alg.cpp.
std::vector< geometry_msgs::PoseStamped > ArmMovementsByPoseAlgorithm::get_transformed_poses | ( | std::vector< geometry_msgs::PoseStamped > | poses, |
std::vector< float > | plane_coefficients | ||
) |
PUBLIC ALGORITHM.
Returns poses for requested moves
Process arm poses for requested move
Definition at line 109 of file arm_movements_by_pose_alg.cpp.
void ArmMovementsByPoseAlgorithm::lock | ( | void | ) | [inline] |
Lock Algorithm.
Locks access to the Algorithm class
Definition at line 102 of file arm_movements_by_pose_alg.h.
geometry_msgs::Quaternion ArmMovementsByPoseAlgorithm::rotate_pitch | ( | geometry_msgs::Quaternion | orientation_msgs, |
double | angle | ||
) |
Rotates Y axis.
Rotates the quaternion pitch
Definition at line 128 of file arm_movements_by_pose_alg.cpp.
geometry_msgs::Quaternion ArmMovementsByPoseAlgorithm::rotate_roll | ( | geometry_msgs::Quaternion | orientation_msgs, |
double | angle | ||
) |
Rotates X axis.
Rotates the quaternion roll
Definition at line 145 of file arm_movements_by_pose_alg.cpp.
geometry_msgs::Quaternion ArmMovementsByPoseAlgorithm::rotate_yaw | ( | geometry_msgs::Quaternion | orientation_msgs, |
double | angle | ||
) |
Rotates X axis.
Rotates the quaternion roll
Definition at line 162 of file arm_movements_by_pose_alg.cpp.
bool ArmMovementsByPoseAlgorithm::try_enter | ( | void | ) | [inline] |
Tries Access to Algorithm.
Tries access to Algorithm
Definition at line 118 of file arm_movements_by_pose_alg.h.
void ArmMovementsByPoseAlgorithm::unlock | ( | void | ) | [inline] |
Unlock Algorithm.
Unlocks access to the Algorithm class
Definition at line 109 of file arm_movements_by_pose_alg.h.
CMutex ArmMovementsByPoseAlgorithm::alg_mutex_ [protected] |
define config type
Define a Config type with the ArmMovementsByPoseConfig. All driver implementations will then use the same variable type Config.
Definition at line 55 of file arm_movements_by_pose_alg.h.
config variable
This variable has all the driver parameters defined in the cfg config file. Is updated everytime function config_update() is called.
Definition at line 85 of file arm_movements_by_pose_alg.h.
double ArmMovementsByPoseAlgorithm::hand_size_offset [private] |
Definition at line 63 of file arm_movements_by_pose_alg.h.
bool ArmMovementsByPoseAlgorithm::robot_wam [private] |
Definition at line 65 of file arm_movements_by_pose_alg.h.
Definition at line 59 of file arm_movements_by_pose_alg.h.
Definition at line 60 of file arm_movements_by_pose_alg.h.
std::string ArmMovementsByPoseAlgorithm::tf_target_str |
Target TF frame name.
Target TF frame name
Definition at line 191 of file arm_movements_by_pose_alg.h.
double ArmMovementsByPoseAlgorithm::wam_security_distance [private] |
Definition at line 64 of file arm_movements_by_pose_alg.h.