#include <interaction_handler.h>
Public Member Functions | |
void | clearError () |
Clear any error settings. This makes the markers appear as if the state is no longer invalid. More... | |
void | clearLastEndEffectorMarkerPose (const EndEffectorInteraction &eef) |
Clear the last interactive_marker command pose for the given end-effector. More... | |
void | clearLastJointMarkerPose (const JointInteraction &vj) |
Clear the last interactive_marker command pose for the given joint. More... | |
void | clearLastMarkerPoses () |
Clear the last interactive_marker command poses for all end-effectors and joints. More... | |
void | clearMenuHandler () |
Remove the menu handler for this interaction handler. More... | |
void | clearPoseOffset (const EndEffectorInteraction &eef) |
Clear the interactive marker pose offset for the given end-effector. More... | |
void | clearPoseOffset (const JointInteraction &vj) |
Clear the interactive marker pose offset for the given joint. More... | |
void | clearPoseOffsets () |
Clear the pose offset for all end-effectors and virtual joints. More... | |
bool | getControlsVisible () const |
bool | getLastEndEffectorMarkerPose (const EndEffectorInteraction &eef, geometry_msgs::PoseStamped &pose) |
Get the last interactive_marker command pose for an end-effector. More... | |
bool | getLastJointMarkerPose (const JointInteraction &vj, geometry_msgs::PoseStamped &pose) |
Get the last interactive_marker command pose for a joint. More... | |
const std::shared_ptr< interactive_markers::MenuHandler > & | getMenuHandler () |
Get the menu handler that defines menus and callbacks for all interactive markers drawn by this interaction handler. More... | |
bool | getMeshesVisible () const |
const std::string & | getName () const |
bool | getPoseOffset (const EndEffectorInteraction &eef, geometry_msgs::Pose &m) |
Get the offset from EEF to its marker. More... | |
bool | getPoseOffset (const JointInteraction &vj, geometry_msgs::Pose &m) |
Get the offset from a joint to its marker. More... | |
moveit::core::RobotStateConstPtr | getState () const |
Get a copy of the RobotState maintained by this InteractionHandler. More... | |
const InteractionHandlerCallbackFn & | getUpdateCallback () const |
virtual void | handleEndEffector (const EndEffectorInteraction &eef, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
Update the internal state maintained by the handler using information from the received feedback message. More... | |
virtual void | handleGeneric (const GenericInteraction &g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
Update the internal state maintained by the handler using information from the received feedback message. More... | |
virtual void | handleJoint (const JointInteraction &vj, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
Update the internal state maintained by the handler using information from the received feedback message. More... | |
virtual bool | inError (const EndEffectorInteraction &eef) const |
Check if the marker corresponding to this end-effector leads to an invalid state. More... | |
virtual bool | inError (const GenericInteraction &g) const |
Check if the generic marker to an invalid state. More... | |
virtual bool | inError (const JointInteraction &vj) const |
Check if the marker corresponding to this joint leads to an invalid state. More... | |
InteractionHandler (const RobotInteractionPtr &robot_interaction, const std::string &name, const moveit::core::RobotState &initial_robot_state, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >()) | |
InteractionHandler (const RobotInteractionPtr &robot_interaction, const std::string &name, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >()) | |
InteractionHandler (const std::string &name, const moveit::core::RobotModelConstPtr &model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >()) | |
InteractionHandler (const std::string &name, const moveit::core::RobotState &initial_robot_state, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >()) | |
void | setControlsVisible (bool visible) |
void | setMenuHandler (const std::shared_ptr< interactive_markers::MenuHandler > &mh) |
Set the menu handler that defines menus and callbacks for all interactive markers drawn by this interaction handler. More... | |
void | setMeshesVisible (bool visible) |
void | setPoseOffset (const EndEffectorInteraction &eef, const geometry_msgs::Pose &m) |
Set the offset from EEF to its marker. More... | |
void | setPoseOffset (const JointInteraction &j, const geometry_msgs::Pose &m) |
Set the offset from a joint to its marker. More... | |
void | setState (const moveit::core::RobotState &state) |
Set a new RobotState for this InteractionHandler. More... | |
void | setUpdateCallback (const InteractionHandlerCallbackFn &callback) |
~InteractionHandler () override | |
![]() | |
moveit::core::RobotStateConstPtr | getState () const |
get read-only access to the state. More... | |
LockedRobotState (const moveit::core::RobotModelPtr &model) | |
LockedRobotState (const moveit::core::RobotState &state) | |
void | modifyState (const ModifyStateFunction &modify) |
void | setState (const moveit::core::RobotState &state) |
Set the state to the new value. More... | |
virtual | ~LockedRobotState () |
Protected Member Functions | |
bool | transformFeedbackPose (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const geometry_msgs::Pose &offset, geometry_msgs::PoseStamped &tpose) |
![]() | |
virtual void | robotStateChanged () |
Protected Attributes | |
const std::string | name_ |
const std::string | planning_frame_ |
std::shared_ptr< tf2_ros::Buffer > | tf_buffer_ |
![]() | |
boost::mutex | state_lock_ |
Private Types | |
typedef boost::function< void(InteractionHandler *)> | StateChangeCallbackFn |
Private Member Functions | |
bool | getErrorState (const std::string &name) const |
bool | setErrorState (const std::string &name, bool new_error_state) |
void | updateStateEndEffector (moveit::core::RobotState &state, const EndEffectorInteraction &eef, const geometry_msgs::Pose &pose, StateChangeCallbackFn &callback) |
void | updateStateGeneric (moveit::core::RobotState &state, const GenericInteraction &g, const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, StateChangeCallbackFn &callback) |
void | updateStateJoint (moveit::core::RobotState &state, const JointInteraction &vj, const geometry_msgs::Pose &pose, StateChangeCallbackFn &callback) |
Static Private Member Functions | |
static std::string | fixName (std::string name) |
Private Attributes | |
bool | display_controls_ |
bool | display_meshes_ |
std::set< std::string > | error_state_ |
KinematicOptionsMapPtr | kinematic_options_map_ |
std::shared_ptr< interactive_markers::MenuHandler > | menu_handler_ |
std::map< std::string, geometry_msgs::Pose > | offset_map_ |
boost::mutex | offset_map_lock_ |
std::map< std::string, geometry_msgs::PoseStamped > | pose_map_ |
boost::mutex | pose_map_lock_ |
boost::function< void(InteractionHandler *handler, bool error_state_changed)> | update_callback_ |
Additional Inherited Members | |
![]() | |
typedef boost::function< void(moveit::core::RobotState *)> | ModifyStateFunction |
Manage interactive markers to control a RobotState.
Each instance maintains one or more interactive markers to control various joints in one group of one RobotState. The group being controlled is maintained by the RobotInteraction object that contains this InteractionHandler object. All InteractionHandler objects in the same RobotInteraction are controlling the same group.
Definition at line 108 of file interaction_handler.h.
|
private |
Definition at line 262 of file interaction_handler.h.
robot_interaction::InteractionHandler::InteractionHandler | ( | const RobotInteractionPtr & | robot_interaction, |
const std::string & | name, | ||
const moveit::core::RobotState & | initial_robot_state, | ||
const std::shared_ptr< tf2_ros::Buffer > & | tf_buffer = std::shared_ptr<tf2_ros::Buffer>() |
||
) |
Definition at line 90 of file interaction_handler.cpp.
robot_interaction::InteractionHandler::InteractionHandler | ( | const RobotInteractionPtr & | robot_interaction, |
const std::string & | name, | ||
const std::shared_ptr< tf2_ros::Buffer > & | tf_buffer = std::shared_ptr<tf2_ros::Buffer>() |
||
) |
Definition at line 103 of file interaction_handler.cpp.
robot_interaction::InteractionHandler::InteractionHandler | ( | const std::string & | name, |
const moveit::core::RobotState & | initial_robot_state, | ||
const std::shared_ptr< tf2_ros::Buffer > & | tf_buffer = std::shared_ptr< tf2_ros::Buffer >() |
||
) |
robot_interaction::InteractionHandler::InteractionHandler | ( | const std::string & | name, |
const moveit::core::RobotModelConstPtr & | model, | ||
const std::shared_ptr< tf2_ros::Buffer > & | tf_buffer = std::shared_ptr< tf2_ros::Buffer >() |
||
) |
|
inlineoverride |
Definition at line 127 of file interaction_handler.h.
void robot_interaction::InteractionHandler::clearError | ( | ) |
Clear any error settings. This makes the markers appear as if the state is no longer invalid.
Definition at line 377 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearLastEndEffectorMarkerPose | ( | const EndEffectorInteraction & | eef | ) |
Clear the last interactive_marker command pose for the given end-effector.
The | target end-effector. |
Definition at line 199 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearLastJointMarkerPose | ( | const JointInteraction & | vj | ) |
Clear the last interactive_marker command pose for the given joint.
The | target joint. |
Definition at line 205 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearLastMarkerPoses | ( | ) |
Clear the last interactive_marker command poses for all end-effectors and joints.
Definition at line 211 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearMenuHandler | ( | ) |
Remove the menu handler for this interaction handler.
Definition at line 229 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearPoseOffset | ( | const EndEffectorInteraction & | eef | ) |
Clear the interactive marker pose offset for the given end-effector.
The | target end-effector. |
Definition at line 133 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearPoseOffset | ( | const JointInteraction & | vj | ) |
Clear the interactive marker pose offset for the given joint.
The | target joint. |
Definition at line 139 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::clearPoseOffsets | ( | ) |
Clear the pose offset for all end-effectors and virtual joints.
Definition at line 145 of file interaction_handler.cpp.
|
staticprivate |
Definition at line 115 of file interaction_handler.cpp.
bool robot_interaction::InteractionHandler::getControlsVisible | ( | ) | const |
Definition at line 471 of file interaction_handler.cpp.
|
private |
Get the error state for name. True if Interaction name is not in a valid pose.
Definition at line 400 of file interaction_handler.cpp.
bool robot_interaction::InteractionHandler::getLastEndEffectorMarkerPose | ( | const EndEffectorInteraction & | eef, |
geometry_msgs::PoseStamped & | pose | ||
) |
Get the last interactive_marker command pose for an end-effector.
The | end-effector in question. |
A | PoseStamped message containing the last (offset-removed) pose commanded for the end-effector. |
Definition at line 175 of file interaction_handler.cpp.
bool robot_interaction::InteractionHandler::getLastJointMarkerPose | ( | const JointInteraction & | vj, |
geometry_msgs::PoseStamped & | pose | ||
) |
Get the last interactive_marker command pose for a joint.
The | joint in question. |
A | PoseStamped message containing the last (offset-removed) pose commanded for the joint. |
Definition at line 187 of file interaction_handler.cpp.
const std::shared_ptr< interactive_markers::MenuHandler > & robot_interaction::InteractionHandler::getMenuHandler | ( | ) |
Get the menu handler that defines menus and callbacks for all interactive markers drawn by this interaction handler.
Definition at line 223 of file interaction_handler.cpp.
bool robot_interaction::InteractionHandler::getMeshesVisible | ( | ) | const |
Definition at line 459 of file interaction_handler.cpp.
|
inline |
Definition at line 131 of file interaction_handler.h.
bool robot_interaction::InteractionHandler::getPoseOffset | ( | const EndEffectorInteraction & | eef, |
geometry_msgs::Pose & | m | ||
) |
Get the offset from EEF to its marker.
The | target end-effector. |
The | pose offset (only valid if return value is true). |
Definition at line 151 of file interaction_handler.cpp.
bool robot_interaction::InteractionHandler::getPoseOffset | ( | const JointInteraction & | vj, |
geometry_msgs::Pose & | m | ||
) |
Get the offset from a joint to its marker.
vj | The joint. |
m | The pose offset (only valid if return value is true). |
Definition at line 163 of file interaction_handler.cpp.
moveit::core::RobotStateConstPtr robot_interaction::LockedRobotState::getState |
Get a copy of the RobotState maintained by this InteractionHandler.
Definition at line 56 of file locked_robot_state.cpp.
const InteractionHandlerCallbackFn & robot_interaction::InteractionHandler::getUpdateCallback | ( | ) | const |
Definition at line 447 of file interaction_handler.cpp.
|
virtual |
Update the internal state maintained by the handler using information from the received feedback message.
Definition at line 252 of file interaction_handler.cpp.
|
virtual |
Update the internal state maintained by the handler using information from the received feedback message.
Definition at line 235 of file interaction_handler.cpp.
|
virtual |
Update the internal state maintained by the handler using information from the received feedback message.
Definition at line 284 of file interaction_handler.cpp.
|
virtual |
Check if the marker corresponding to this end-effector leads to an invalid state.
Definition at line 362 of file interaction_handler.cpp.
|
virtual |
Check if the generic marker to an invalid state.
Definition at line 367 of file interaction_handler.cpp.
|
virtual |
Check if the marker corresponding to this joint leads to an invalid state.
Definition at line 372 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::setControlsVisible | ( | bool | visible | ) |
Definition at line 465 of file interaction_handler.cpp.
|
private |
Definition at line 385 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::setMenuHandler | ( | const std::shared_ptr< interactive_markers::MenuHandler > & | mh | ) |
Set the menu handler that defines menus and callbacks for all interactive markers drawn by this interaction handler.
A | menu handler. |
Definition at line 217 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::setMeshesVisible | ( | bool | visible | ) |
Definition at line 453 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::setPoseOffset | ( | const EndEffectorInteraction & | eef, |
const geometry_msgs::Pose & | m | ||
) |
Set the offset from EEF to its marker.
eef | The target end-effector. |
m | The pose of the marker in the frame of the end-effector parent. |
Definition at line 121 of file interaction_handler.cpp.
void robot_interaction::InteractionHandler::setPoseOffset | ( | const JointInteraction & | j, |
const geometry_msgs::Pose & | m | ||
) |
Set the offset from a joint to its marker.
j | The target joint. |
m | The pose of the marker in the frame of the joint parent. |
Definition at line 127 of file interaction_handler.cpp.
void robot_interaction::LockedRobotState::setState |
Set a new RobotState for this InteractionHandler.
Definition at line 62 of file locked_robot_state.cpp.
void robot_interaction::InteractionHandler::setUpdateCallback | ( | const InteractionHandlerCallbackFn & | callback | ) |
Definition at line 441 of file interaction_handler.cpp.
|
protected |
Definition at line 406 of file interaction_handler.cpp.
|
private |
Definition at line 330 of file interaction_handler.cpp.
|
private |
Definition at line 317 of file interaction_handler.cpp.
|
private |
Definition at line 346 of file interaction_handler.cpp.
|
private |
Definition at line 340 of file interaction_handler.h.
|
private |
Definition at line 337 of file interaction_handler.h.
|
private |
Definition at line 317 of file interaction_handler.h.
|
private |
Definition at line 313 of file interaction_handler.h.
|
private |
Definition at line 324 of file interaction_handler.h.
|
protected |
Definition at line 257 of file interaction_handler.h.
|
private |
Definition at line 296 of file interaction_handler.h.
|
private |
Definition at line 308 of file interaction_handler.h.
|
protected |
Definition at line 258 of file interaction_handler.h.
|
private |
Definition at line 305 of file interaction_handler.h.
|
private |
Definition at line 307 of file interaction_handler.h.
|
protected |
Definition at line 259 of file interaction_handler.h.
|
private |
Definition at line 334 of file interaction_handler.h.