#include <cob_lookat_action_server.h>
Public Member Functions | |
CobLookAtAction (std::string action_name) | |
void | goalCB (const cob_lookat_action::LookAtGoalConstPtr &goal) |
bool | init () |
~CobLookAtAction (void) | |
Protected Attributes | |
ros::Duration | buffer_duration_ |
std::string | chain_base_link_ |
KDL::Chain | chain_main_ |
std::string | chain_tip_link_ |
actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > * | fjt_ac_ |
std::string | fjt_name_ |
std::shared_ptr< KDL::ChainFkSolverPos_recursive > | fk_solver_pos_ |
std::shared_ptr< KDL::ChainFkSolverPos_recursive > | fk_solver_pos_main_ |
std::shared_ptr< KDL::ChainIkSolverPos_LMA > | ik_solver_pos_ |
std::vector< std::string > | joint_names_ |
actionlib::SimpleActionServer< cob_lookat_action::LookAtAction > * | lookat_as_ |
cob_lookat_action::LookAtFeedback | lookat_fb_ |
std::string | lookat_name_ |
cob_lookat_action::LookAtResult | lookat_res_ |
actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > * | mbl_ac_ |
std::string | mbl_name_ |
ros::NodeHandle | nh_ |
std::shared_ptr< tf2_ros::Buffer > | tf_buffer_ |
std::shared_ptr< tf2_ros::TransformListener > | tf_listener_ |
Definition at line 45 of file cob_lookat_action_server.h.
|
inline |
Definition at line 75 of file cob_lookat_action_server.h.
|
inline |
Definition at line 81 of file cob_lookat_action_server.h.
void CobLookAtAction::goalCB | ( | const cob_lookat_action::LookAtGoalConstPtr & | goal | ) |
transform pointing_frame to offset
compose lookat_lin joint
fixed pointing offset
chain_lookat
chain_base
chain composition
set up solver
transform target_frame to p_in
solution valid?
check FK diff (target vs. q_out)
solution valid?
index correction
check FK based on (base +) main + offset
solution valid?
check lin_axis value positive
check lookat offset from lin_axis
compose FJT goal
compute time_from_start
disable FJT constraints
compose MBL goal
execute FJT (and MBL) goal
wait for FJT (and MBL) to finish
actions successful?
Definition at line 76 of file cob_lookat_action_server.cpp.
bool CobLookAtAction::init | ( | ) |
get parameters from parameter server
parse robot_description and generate KDL chains
Definition at line 26 of file cob_lookat_action_server.cpp.
|
protected |
Definition at line 71 of file cob_lookat_action_server.h.
|
protected |
Definition at line 61 of file cob_lookat_action_server.h.
|
protected |
Definition at line 64 of file cob_lookat_action_server.h.
|
protected |
Definition at line 62 of file cob_lookat_action_server.h.
|
protected |
Definition at line 51 of file cob_lookat_action_server.h.
|
protected |
Definition at line 54 of file cob_lookat_action_server.h.
|
protected |
Definition at line 66 of file cob_lookat_action_server.h.
|
protected |
Definition at line 65 of file cob_lookat_action_server.h.
|
protected |
Definition at line 67 of file cob_lookat_action_server.h.
|
protected |
Definition at line 60 of file cob_lookat_action_server.h.
|
protected |
Definition at line 53 of file cob_lookat_action_server.h.
|
protected |
Definition at line 57 of file cob_lookat_action_server.h.
|
protected |
Definition at line 56 of file cob_lookat_action_server.h.
|
protected |
Definition at line 58 of file cob_lookat_action_server.h.
|
protected |
Definition at line 52 of file cob_lookat_action_server.h.
|
protected |
Definition at line 55 of file cob_lookat_action_server.h.
|
protected |
Definition at line 49 of file cob_lookat_action_server.h.
|
protected |
Definition at line 69 of file cob_lookat_action_server.h.
|
protected |
Definition at line 70 of file cob_lookat_action_server.h.