Controller for executing a gripper command action for simple single-dof grippers. More...
#include <two_finger_controller.h>
Classes | |
struct | Commands |
Store position and max effort in struct to allow easier realtime buffer usage. More... | |
Public Member Functions | |
double | gap2Pos (double gap) |
GripperActionControllerTwo () | |
double | pos2Gap (double pos) |
Non Real-Time Safe Functions | |
bool | init (HardwareInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) |
Real-Time Safe Functions | |
void | starting (const ros::Time &time) |
Holds the current position. | |
void | stopping (const ros::Time &time) |
Cancels the active action goal, if any. | |
void | update (const ros::Time &time, const ros::Duration &period) |
Public Attributes | |
realtime_tools::RealtimeBuffer < Commands > | command_ |
Commands | command_struct_ |
Commands | command_struct_rt_ |
Private Types | |
typedef actionlib::ActionServer < control_msgs::GripperCommandAction > | ActionServer |
typedef boost::shared_ptr < ActionServer > | ActionServerPtr |
typedef ActionServer::GoalHandle | GoalHandle |
typedef HardwareInterfaceAdapter < HardwareInterface > | HwIfaceAdapter |
typedef realtime_tools::RealtimeServerGoalHandle < control_msgs::GripperCommandAction > | RealtimeGoalHandle |
typedef boost::shared_ptr < RealtimeGoalHandle > | RealtimeGoalHandlePtr |
Private Member Functions | |
void | cancelCB (GoalHandle gh) |
void | checkForSuccess (const ros::Time &time, double error_position, double current_position, double current_velocity, double current_effort, double max_effort) |
Check for success and publish appropriate result and feedback. | |
void | goalCB (GoalHandle gh) |
void | preemptActiveGoal () |
void | setHoldPosition (const ros::Time &time) |
Private Attributes | |
ros::Publisher | _gapPub |
double | _lastPosition |
ros::Duration | action_monitor_period_ |
ActionServerPtr | action_server_ |
double | computed_command_ |
Computed command. | |
ros::NodeHandle | controller_nh_ |
double | default_max_effort_ |
Max allowed effort. | |
ros::Timer | goal_handle_timer_ |
double | goal_tolerance_ |
ros::Time | last_movement_time_ |
Store stall time. | |
HwIfaceAdapter | left_hw_iface_adapter_ |
Adapts desired goal state to HW interface. | |
hardware_interface::JointHandle | leftjoint_ |
Handles to controlled joints. | |
std::string | leftJoint_name_ |
Controlled joint names. | |
std::string | name_ |
Controller name. | |
control_msgs::GripperCommandResultPtr | pre_alloc_result_ |
HwIfaceAdapter | right_hw_iface_adapter_ |
Adapts desired goal state to HW interface. | |
hardware_interface::JointHandle | rightjoint_ |
Handles to controlled joints. | |
std::string | rightJoint_name_ |
Controlled joint names. | |
RealtimeGoalHandlePtr | rt_active_goal_ |
Currently active action goal, if any. | |
double | stall_timeout_ |
double | stall_velocity_threshold_ |
Stall related parameters. | |
bool | update_hold_position_ |
bool | verbose_ |
Hard coded verbose flag to help in debugging. |
Controller for executing a gripper command action for simple single-dof grippers.
HardwareInterface | Controller hardware interface. Currently hardware_interface::PositionJointInterface and hardware_interface::EffortJointInterface are supported out-of-the-box. |
Definition at line 79 of file two_finger_controller.h.
typedef actionlib::ActionServer<control_msgs::GripperCommandAction> gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::ActionServer [private] |
Definition at line 138 of file two_finger_controller.h.
typedef boost::shared_ptr<ActionServer> gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::ActionServerPtr [private] |
Definition at line 139 of file two_finger_controller.h.
typedef ActionServer::GoalHandle gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::GoalHandle [private] |
Definition at line 140 of file two_finger_controller.h.
typedef HardwareInterfaceAdapter<HardwareInterface> gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::HwIfaceAdapter [private] |
Definition at line 144 of file two_finger_controller.h.
typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::GripperCommandAction> gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::RealtimeGoalHandle [private] |
Definition at line 141 of file two_finger_controller.h.
typedef boost::shared_ptr<RealtimeGoalHandle> gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::RealtimeGoalHandlePtr [private] |
Definition at line 142 of file two_finger_controller.h.
gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::GripperActionControllerTwo | ( | ) |
Definition at line 109 of file gripper_action_controller_impl.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::cancelCB | ( | GoalHandle | gh | ) | [private] |
Definition at line 279 of file gripper_action_controller_impl.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::checkForSuccess | ( | const ros::Time & | time, |
double | error_position, | ||
double | current_position, | ||
double | current_velocity, | ||
double | current_effort, | ||
double | max_effort | ||
) | [private] |
Check for success and publish appropriate result and feedback.
Definition at line 315 of file gripper_action_controller_impl.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::gap2Pos | ( | double | gap | ) | [inline] |
Definition at line 113 of file two_finger_controller.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::goalCB | ( | GoalHandle | gh | ) | [private] |
Definition at line 237 of file gripper_action_controller_impl.h.
bool gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::init | ( | HardwareInterface * | hw, |
ros::NodeHandle & | root_nh, | ||
ros::NodeHandle & | controller_nh | ||
) | [virtual] |
Reimplemented from controller_interface::Controller< HardwareInterface >.
Definition at line 114 of file gripper_action_controller_impl.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::pos2Gap | ( | double | pos | ) | [inline] |
Definition at line 129 of file two_finger_controller.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::preemptActiveGoal | ( | ) | [inline, private] |
Definition at line 93 of file gripper_action_controller_impl.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::setHoldPosition | ( | const ros::Time & | time | ) | [private] |
Definition at line 300 of file gripper_action_controller_impl.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::starting | ( | const ros::Time & | time | ) | [inline, virtual] |
Holds the current position.
Reimplemented from controller_interface::ControllerBase.
Definition at line 69 of file gripper_action_controller_impl.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::stopping | ( | const ros::Time & | time | ) | [inline, virtual] |
Cancels the active action goal, if any.
Reimplemented from controller_interface::ControllerBase.
Definition at line 86 of file gripper_action_controller_impl.h.
void gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::update | ( | const ros::Time & | time, |
const ros::Duration & | period | ||
) | [virtual] |
Implements controller_interface::ControllerBase.
Definition at line 212 of file gripper_action_controller_impl.h.
ros::Publisher gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::_gapPub [private] |
Definition at line 165 of file two_finger_controller.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::_lastPosition [private] |
Definition at line 137 of file two_finger_controller.h.
ros::Duration gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::action_monitor_period_ [private] |
Definition at line 161 of file two_finger_controller.h.
ActionServerPtr gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::action_server_ [private] |
Definition at line 166 of file two_finger_controller.h.
realtime_tools::RealtimeBuffer<Commands> gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::command_ |
Definition at line 110 of file two_finger_controller.h.
Commands gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::command_struct_ |
Definition at line 111 of file two_finger_controller.h.
Commands gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::command_struct_rt_ |
Definition at line 111 of file two_finger_controller.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::computed_command_ [private] |
Computed command.
Definition at line 177 of file two_finger_controller.h.
ros::NodeHandle gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::controller_nh_ [private] |
Definition at line 164 of file two_finger_controller.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::default_max_effort_ [private] |
Max allowed effort.
Definition at line 180 of file two_finger_controller.h.
ros::Timer gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::goal_handle_timer_ [private] |
Definition at line 169 of file two_finger_controller.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::goal_tolerance_ [private] |
Definition at line 181 of file two_finger_controller.h.
ros::Time gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::last_movement_time_ [private] |
Store stall time.
Definition at line 176 of file two_finger_controller.h.
HwIfaceAdapter gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::left_hw_iface_adapter_ [private] |
Adapts desired goal state to HW interface.
Definition at line 155 of file two_finger_controller.h.
hardware_interface::JointHandle gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::leftjoint_ [private] |
Handles to controlled joints.
Definition at line 150 of file two_finger_controller.h.
std::string gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::leftJoint_name_ [private] |
Controlled joint names.
Definition at line 152 of file two_finger_controller.h.
std::string gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::name_ [private] |
Controller name.
Definition at line 149 of file two_finger_controller.h.
control_msgs::GripperCommandResultPtr gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::pre_alloc_result_ [private] |
Definition at line 159 of file two_finger_controller.h.
HwIfaceAdapter gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::right_hw_iface_adapter_ [private] |
Adapts desired goal state to HW interface.
Definition at line 156 of file two_finger_controller.h.
hardware_interface::JointHandle gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::rightjoint_ [private] |
Handles to controlled joints.
Definition at line 151 of file two_finger_controller.h.
std::string gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::rightJoint_name_ [private] |
Controlled joint names.
Definition at line 153 of file two_finger_controller.h.
RealtimeGoalHandlePtr gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::rt_active_goal_ [private] |
Currently active action goal, if any.
Definition at line 158 of file two_finger_controller.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::stall_timeout_ [private] |
Definition at line 179 of file two_finger_controller.h.
double gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::stall_velocity_threshold_ [private] |
Stall related parameters.
Definition at line 179 of file two_finger_controller.h.
bool gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::update_hold_position_ [private] |
Definition at line 146 of file two_finger_controller.h.
bool gripper_action_controller::GripperActionControllerTwo< HardwareInterface >::verbose_ [private] |
Hard coded verbose flag to help in debugging.
Definition at line 148 of file two_finger_controller.h.