#include <carl_interactive_manipulation.h>
Public Member Functions | |
CarlInteractiveManipulation () | |
Constructor. | |
void | clearSegmentedObjects () |
clear all segmented objects from the interactive marker server | |
void | processHandMarkerFeedback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
Process feedback for the interactive marker on the JACO's end effector. | |
void | processPickupMarkerFeedback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
void | processRemoveMarkerFeedback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) |
Process feedback for objects that are selected for removal. | |
void | segmentedObjectsCallback (const rail_manipulation_msgs::SegmentedObjectList::ConstPtr &objectList) |
callback for segmented objects to be displayed | |
void | updateJoints (const sensor_msgs::JointState::ConstPtr &msg) |
Callback for the joint state listener. | |
void | updateMarkerPosition () |
Update the interactive marker on the JACO's end effector to move based on the the current joint state of the arm. | |
Private Member Functions | |
void | armCollisionRecovery () |
If interactive markers cause the arm to be in a dangerous collision, briefly reverse arm motion. | |
bool | isArmRetracted () |
Determine if the arm is in the retracted position. | |
void | makeHandMarker () |
Create the interactive marker on the JACO's end effector, including pose controls and menus. | |
bool | removeObjectMarker (int index) |
Remove a manipulation object marker. | |
void | sendStopCommand () |
Send a 0 velocity command to the robot's arm. | |
Private Attributes | |
actionlib::SimpleActionClient < rail_manipulation_msgs::ArmAction > | acArm |
actionlib::SimpleActionClient < rail_manipulation_msgs::GripperAction > | acGripper |
actionlib::SimpleActionClient < rail_manipulation_msgs::PickupAction > | acPickup |
boost::recursive_mutex | api_mutex |
ros::ServiceClient | armAngularPositionClient |
ros::ServiceClient | armCartesianPositionClient |
ros::ServiceClient | armEStopClient |
ros::Publisher | cartesianCmd |
ros::ServiceClient | detachObjectsClient |
bool | disableArmMarkerCommands |
ros::ServiceClient | eraseTrajectoriesClient |
boost::shared_ptr < interactive_markers::InteractiveMarkerServer > | imServer |
interactive marker server | |
ros::ServiceClient | jacoFkClient |
forward kinematics | |
std::vector< float > | joints |
current joint state | |
ros::Subscriber | jointStateSubscriber |
ros::Time | lastRetractedFeedback |
bool | lockPose |
flag to stop the arm from updating on pose changes, this is used to prevent the slight movement when left clicking on the center of the marker | |
std::vector< float > | markerPose |
current pose of the gripper marker | |
interactive_markers::MenuHandler | menuHandler |
interactive marker menu handler | |
bool | movingArm |
ros::NodeHandle | n |
interactive_markers::MenuHandler | objectMenuHandler |
object interactive markers menu handler | |
ros::ServiceClient | qeClient |
rotation representation conversion client | |
std::vector < interactive_markers::MenuHandler > | recognizedMenuHandlers |
list of customized menu handlers for recognized objects | |
ros::Subscriber | recognizedObjectsSubscriber |
ros::ServiceClient | removeObjectClient |
std::vector< float > | retractPos |
ros::Publisher | safetyErrorPublisher |
rail_manipulation_msgs::SegmentedObjectList | segmentedObjectList |
list of segmented objects in the rail_manipulation_msgs form | |
std::vector < visualization_msgs::InteractiveMarker > | segmentedObjects |
list of segmented objects as interactive markers | |
bool | usingPickup |
bool | usingRecognition |
Definition at line 60 of file carl_interactive_manipulation.h.
Constructor.
Definition at line 5 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::armCollisionRecovery | ( | ) | [private] |
If interactive markers cause the arm to be in a dangerous collision, briefly reverse arm motion.
Definition at line 554 of file carl_interactive_manipulation.cpp.
clear all segmented objects from the interactive marker server
Definition at line 198 of file carl_interactive_manipulation.cpp.
bool CarlInteractiveManipulation::isArmRetracted | ( | ) | [private] |
Determine if the arm is in the retracted position.
Definition at line 667 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::makeHandMarker | ( | ) | [private] |
Create the interactive marker on the JACO's end effector, including pose controls and menus.
Definition at line 214 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::processHandMarkerFeedback | ( | const visualization_msgs::InteractiveMarkerFeedbackConstPtr & | feedback | ) |
Process feedback for the interactive marker on the JACO's end effector.
feedback | interactive marker feedback |
Definition at line 378 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::processPickupMarkerFeedback | ( | const visualization_msgs::InteractiveMarkerFeedbackConstPtr & | feedback | ) |
/brief Process feedback for objects that can be recognized.
feedback | interactive marker feedback /brief Process feedback for objects that can be picked up. |
feedback | interactive marker feedback |
Definition at line 316 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::processRemoveMarkerFeedback | ( | const visualization_msgs::InteractiveMarkerFeedbackConstPtr & | feedback | ) |
Process feedback for objects that are selected for removal.
feedback | interactive marker feedback |
Definition at line 352 of file carl_interactive_manipulation.cpp.
bool CarlInteractiveManipulation::removeObjectMarker | ( | int | index | ) | [private] |
Remove a manipulation object marker.
index | object index |
Definition at line 365 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::segmentedObjectsCallback | ( | const rail_manipulation_msgs::SegmentedObjectList::ConstPtr & | objectList | ) |
callback for segmented objects to be displayed
objectList | list of segmented objects for displaying |
Definition at line 97 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::sendStopCommand | ( | ) | [private] |
Send a 0 velocity command to the robot's arm.
Definition at line 513 of file carl_interactive_manipulation.cpp.
void CarlInteractiveManipulation::updateJoints | ( | const sensor_msgs::JointState::ConstPtr & | msg | ) |
Callback for the joint state listener.
msg | new joint state message |
Definition at line 77 of file carl_interactive_manipulation.cpp.
Update the interactive marker on the JACO's end effector to move based on the the current joint state of the arm.
Definition at line 535 of file carl_interactive_manipulation.cpp.
actionlib::SimpleActionClient<rail_manipulation_msgs::ArmAction> CarlInteractiveManipulation::acArm [private] |
Definition at line 167 of file carl_interactive_manipulation.h.
actionlib::SimpleActionClient<rail_manipulation_msgs::GripperAction> CarlInteractiveManipulation::acGripper [private] |
Definition at line 166 of file carl_interactive_manipulation.h.
actionlib::SimpleActionClient<rail_manipulation_msgs::PickupAction> CarlInteractiveManipulation::acPickup [private] |
Definition at line 168 of file carl_interactive_manipulation.h.
boost::recursive_mutex CarlInteractiveManipulation::api_mutex [private] |
Definition at line 171 of file carl_interactive_manipulation.h.
Definition at line 155 of file carl_interactive_manipulation.h.
Definition at line 156 of file carl_interactive_manipulation.h.
Definition at line 157 of file carl_interactive_manipulation.h.
Definition at line 149 of file carl_interactive_manipulation.h.
Definition at line 163 of file carl_interactive_manipulation.h.
bool CarlInteractiveManipulation::disableArmMarkerCommands [private] |
Definition at line 183 of file carl_interactive_manipulation.h.
Definition at line 158 of file carl_interactive_manipulation.h.
boost::shared_ptr<interactive_markers::InteractiveMarkerServer> CarlInteractiveManipulation::imServer [private] |
interactive marker server
Definition at line 172 of file carl_interactive_manipulation.h.
forward kinematics
Definition at line 159 of file carl_interactive_manipulation.h.
std::vector<float> CarlInteractiveManipulation::joints [private] |
current joint state
Definition at line 178 of file carl_interactive_manipulation.h.
Definition at line 151 of file carl_interactive_manipulation.h.
Definition at line 186 of file carl_interactive_manipulation.h.
bool CarlInteractiveManipulation::lockPose [private] |
flag to stop the arm from updating on pose changes, this is used to prevent the slight movement when left clicking on the center of the marker
Definition at line 181 of file carl_interactive_manipulation.h.
std::vector<float> CarlInteractiveManipulation::markerPose [private] |
current pose of the gripper marker
Definition at line 179 of file carl_interactive_manipulation.h.
interactive marker menu handler
Definition at line 173 of file carl_interactive_manipulation.h.
bool CarlInteractiveManipulation::movingArm [private] |
Definition at line 182 of file carl_interactive_manipulation.h.
Definition at line 146 of file carl_interactive_manipulation.h.
object interactive markers menu handler
Definition at line 174 of file carl_interactive_manipulation.h.
rotation representation conversion client
Definition at line 160 of file carl_interactive_manipulation.h.
std::vector<interactive_markers::MenuHandler> CarlInteractiveManipulation::recognizedMenuHandlers [private] |
list of customized menu handlers for recognized objects
Definition at line 175 of file carl_interactive_manipulation.h.
Definition at line 152 of file carl_interactive_manipulation.h.
Definition at line 162 of file carl_interactive_manipulation.h.
std::vector<float> CarlInteractiveManipulation::retractPos [private] |
Definition at line 180 of file carl_interactive_manipulation.h.
Definition at line 150 of file carl_interactive_manipulation.h.
rail_manipulation_msgs::SegmentedObjectList CarlInteractiveManipulation::segmentedObjectList [private] |
list of segmented objects in the rail_manipulation_msgs form
Definition at line 177 of file carl_interactive_manipulation.h.
std::vector<visualization_msgs::InteractiveMarker> CarlInteractiveManipulation::segmentedObjects [private] |
list of segmented objects as interactive markers
Definition at line 176 of file carl_interactive_manipulation.h.
bool CarlInteractiveManipulation::usingPickup [private] |
Definition at line 184 of file carl_interactive_manipulation.h.
bool CarlInteractiveManipulation::usingRecognition [private] |
Definition at line 185 of file carl_interactive_manipulation.h.