Public Types | Public Member Functions | Static Public Attributes | Protected Member Functions | Static Protected Member Functions | Protected Attributes
RobotNavigator Class Reference

#include <RobotNavigator.h>

List of all members.

Public Types

typedef boost::shared_ptr
< RobotNavigator
Ptr

Public Member Functions

 RobotNavigator ()
virtual void run ()
virtual ~RobotNavigator ()

Static Public Attributes

static std::string MARKER_ARM_LINK = "arm_links"
static std::string MARKER_ATTACHED_OBJECT = "attached_object"
static std::string NAVIGATOR_NAMESPACE = "navigator"
static std::string NODE_NAME = "robot_pick_place"
static const tf::Transform PLACE_RECTIFICATION_TF
static std::string VISUALIZATION_TOPIC = "visualization_markers"

Protected Member Functions

void addDetectedObjectToLocalPlanningScene (arm_navigation_msgs::CollisionObject &obj)
void addDetectedObjectToLocalPlanningScene (const household_objects_database_msgs::DatabaseModelPoseList &model)
void addDetectedTableToLocalPlanningScene (const tabletop_object_detector::Table &table)
void addMarker (std::string name, visualization_msgs::Marker &marker)
void addMarker (std::string name, visualization_msgs::MarkerArray &marker)
bool attachCollisionObject (const std::string &group_name, const std::string &collision_object_name, const object_manipulation_msgs::Grasp &grasp)
void attachCollisionObjectCallback (const std::string &group_name)
virtual bool attemptGraspSequence (const std::string &group_name, const object_manipulator::GraspExecutionInfo &gei, bool performRecoveryMove=true)
virtual bool attemptPlaceSequence (const std::string &group_name, const object_manipulator::PlaceExecutionInfo &pei)
virtual void callbackPublishMarkers (const ros::TimerEvent &evnt)
void collisionObjToMarker (const arm_navigation_msgs::CollisionObject &obj, visualization_msgs::Marker &marker)
virtual bool createCandidateGoalPoses (std::vector< geometry_msgs::PoseStamped > &placePoses)=0
virtual bool createPickMoveSequence (const object_manipulation_msgs::PickupGoal &pickupGoal, const std::vector< object_manipulation_msgs::Grasp > &grasps_candidates, std::vector< object_manipulator::GraspExecutionInfo > &grasp_sequence, std::vector< object_manipulation_msgs::Grasp > &valid_grasps)
virtual bool createPlaceMoveSequence (const object_manipulation_msgs::PlaceGoal &placeGoal, const std::vector< geometry_msgs::PoseStamped > &place_poses, std::vector< object_manipulator::PlaceExecutionInfo > &place_sequence)
bool detachCollisionObject (const std::string &group_name, const geometry_msgs::PoseStamped &place_pose, const object_manipulation_msgs::Grasp &grasp)
void detachCollisionObjectCallback (const std::string &group_name)
virtual void fetchParameters (std::string nameSpace="")
const
arm_navigation_msgs::CollisionObject
getCollisionObject (unsigned int model_id)
virtual
trajectory_msgs::JointTrajectory 
getGripperTrajectory (int graspMove=object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP)
virtual std::vector< std::string > getJointNames (const std::string &group)
visualization_msgs::Marker & getMarker (std::string name)
bool getMeshFromDatabasePose (const household_objects_database_msgs::DatabaseModelPose &model_pose, arm_navigation_msgs::CollisionObject &obj, const geometry_msgs::PoseStamped &pose)
bool hasMarker (std::string)
virtual bool moveArm (const std::string &group_name, const std::vector< double > &joint_positions)
virtual bool moveArmThroughPickSequence ()
virtual bool moveArmThroughPlaceSequence ()
virtual bool moveArmToSide ()
virtual bool performGraspPlanning ()
virtual bool performPickGraspPlanning ()
virtual bool performPlaceGraspPlanning ()
virtual bool performRecognition ()
virtual bool performSegmentation ()
virtual bool performTrajectoryFiltering (const std::string &group_name, trajectory_msgs::JointTrajectory &jt)
void printJointTrajectory (const trajectory_msgs::JointTrajectory &jt)
void printTiming ()
void revertPlanningScene ()
virtual void setup ()
void startCycleTimer ()
virtual bool trajectoryFinishedCallback (bool storeLastTraj, TrajectoryExecutionDataVector tedv)
bool updateChangesToPlanningScene ()
void updateCurrentJointStateToLastTrajectoryPoint (const trajectory_msgs::JointTrajectory &traj)
bool validateJointTrajectory (trajectory_msgs::JointTrajectory &jt)

Static Protected Member Functions

static std::string makeCollisionObjectNameFromModelId (unsigned int model_id)

Protected Attributes

tf::TransformListener _TfListener
boost::shared_ptr
< TrajectoryControllerHandler
arm_controller_handler_
std::string arm_group_name_
ros::Publisher attached_object_publisher_
planning_environment::CollisionModels cm_
std::map< std::string,
object_manipulation_msgs::Grasp > 
current_grasp_map_
std::map< std::string,
std::string > 
current_grasped_object_name_
geometry_msgs::PoseStamped current_place_location_
arm_navigation_msgs::PlanningScene current_planning_scene_
unsigned int current_planning_scene_id_
planning_models::KinematicStatecurrent_robot_state_
ros::WallTime cycle_start_time_
boost::condition_variable execution_completed_
ros::WallDuration execution_duration_
boost::mutex execution_mutex_
boost::function< bool(TrajectoryExecutionDataVector)> grasp_action_finished_function_
std::string grasp_action_service_
std::vector
< object_manipulation_msgs::Grasp > 
grasp_candidates_
boost::shared_ptr
< GraspActionServerClient
grasp_exec_action_client_
std::vector
< object_manipulator::GraspExecutionInfo
grasp_pick_sequence_
object_manipulation_msgs::PickupGoal grasp_pickup_goal_
object_manipulation_msgs::PlaceGoal grasp_place_goal_
std::vector
< object_manipulator::PlaceExecutionInfo
grasp_place_sequence_
ros::ServiceClient grasp_planning_client
ros::WallDuration grasp_planning_duration_
std::string grasp_planning_service_
boost::shared_ptr
< GraspSequenceValidator
grasp_tester_
boost::shared_ptr
< TrajectoryControllerHandler
gripper_controller_handler_
std::string gripper_group_name_
std::string gripper_link_name_
std::string ik_plugin_name_
boost::shared_ptr
< TrajectoryRecorder
joint_state_recorder_
std::string joint_states_topic_
unsigned int last_mpr_id_
TrajectoryExecutionDataVector last_trajectory_execution_data_vector_
ros::Publisher marker_array_publisher_
std::map< std::string,
visualization_msgs::Marker > 
marker_map_
ros::Timer marker_pub_timer_
ros::Publisher marker_publisher_
unsigned int max_mpr_id_
unsigned int max_trajectory_id_
std::string mesh_database_service_
std::string model_database_service_
ros::WallDuration motion_planning_duration_
ros::ServiceClient object_database_model_description_client_
ros::ServiceClient object_database_model_mesh_client_
std::map< std::string, bool > object_in_hand_map_
std::string path_planner_service_
ros::WallDuration perception_duration_
double pick_approach_distance_
double place_approach_distance_
double place_retreat_distance_
boost::shared_ptr
< PlaceSequenceValidator
place_tester_
arm_navigation_msgs::PlanningScene planning_scene_diff_
ros::WallDuration planning_scene_duration_
std::string planning_scene_service_
ros::ServiceClient planning_service_client_
ros::ServiceClient rec_srv_
std::string recognition_service_
household_objects_database_msgs::GetModelDescription::Response recognized_model_description_
std::vector
< household_objects_database_msgs::DatabaseModelPoseList > 
recognized_models_
std::map< std::string,
geometry_msgs::PoseStamped > 
recognized_obj_pose_map_
ros::ServiceClient seg_srv_
tabletop_object_detector::TabletopSegmentation::Response segmentation_results_
std::string segmentation_service_
std::vector
< sensor_msgs::PointCloud
segmented_clusters_
ros::ServiceClient set_planning_scene_diff_client_
arm_navigation_msgs::CollisionObject table_
boost::function< bool(TrajectoryExecutionDataVector)> trajectories_finished_function_
bool trajectories_succeeded_
std::string trajectory_action_service_
trajectory_execution_monitor::TrajectoryExecutionMonitor trajectory_execution_monitor_
std::string trajectory_filter_service_
ros::ServiceClient trajectory_filter_service_client_
ros::WallDuration trajectory_filtering_duration_
std::string wrist_link_name_

Detailed Description

Definition at line 96 of file RobotNavigator.h.


Member Typedef Documentation

typedef boost::shared_ptr<RobotNavigator> RobotNavigator::Ptr

Definition at line 100 of file RobotNavigator.h.


Constructor & Destructor Documentation

Definition at line 54 of file RobotNavigator.cpp.

Definition at line 70 of file RobotNavigator.cpp.


Member Function Documentation

Definition at line 574 of file RobotNavigator.cpp.

void RobotNavigator::addDetectedObjectToLocalPlanningScene ( const household_objects_database_msgs::DatabaseModelPoseList &  model) [protected]

Definition at line 580 of file RobotNavigator.cpp.

Definition at line 532 of file RobotNavigator.cpp.

void RobotNavigator::addMarker ( std::string  name,
visualization_msgs::Marker &  marker 
) [protected]

Definition at line 1328 of file RobotNavigator.cpp.

void RobotNavigator::addMarker ( std::string  name,
visualization_msgs::MarkerArray &  marker 
) [protected]

Definition at line 1342 of file RobotNavigator.cpp.

bool RobotNavigator::attachCollisionObject ( const std::string &  group_name,
const std::string &  collision_object_name,
const object_manipulation_msgs::Grasp &  grasp 
) [protected]

Definition at line 1665 of file RobotNavigator.cpp.

void RobotNavigator::attachCollisionObjectCallback ( const std::string &  group_name) [protected]

Definition at line 1380 of file RobotNavigator.cpp.

bool RobotNavigator::attemptGraspSequence ( const std::string &  group_name,
const object_manipulator::GraspExecutionInfo gei,
bool  performRecoveryMove = true 
) [protected, virtual]

Definition at line 1398 of file RobotNavigator.cpp.

bool RobotNavigator::attemptPlaceSequence ( const std::string &  group_name,
const object_manipulator::PlaceExecutionInfo pei 
) [protected, virtual]

Definition at line 1555 of file RobotNavigator.cpp.

void RobotNavigator::callbackPublishMarkers ( const ros::TimerEvent evnt) [protected, virtual]

Definition at line 1370 of file RobotNavigator.cpp.

void RobotNavigator::collisionObjToMarker ( const arm_navigation_msgs::CollisionObject obj,
visualization_msgs::Marker &  marker 
) [protected]

Definition at line 1782 of file RobotNavigator.cpp.

virtual bool RobotNavigator::createCandidateGoalPoses ( std::vector< geometry_msgs::PoseStamped > &  placePoses) [protected, pure virtual]
bool RobotNavigator::createPickMoveSequence ( const object_manipulation_msgs::PickupGoal pickupGoal,
const std::vector< object_manipulation_msgs::Grasp > &  grasps_candidates,
std::vector< object_manipulator::GraspExecutionInfo > &  grasp_sequence,
std::vector< object_manipulation_msgs::Grasp > &  valid_grasps 
) [protected, virtual]

Definition at line 1160 of file RobotNavigator.cpp.

bool RobotNavigator::createPlaceMoveSequence ( const object_manipulation_msgs::PlaceGoal placeGoal,
const std::vector< geometry_msgs::PoseStamped > &  place_poses,
std::vector< object_manipulator::PlaceExecutionInfo > &  place_sequence 
) [protected, virtual]

Definition at line 1190 of file RobotNavigator.cpp.

bool RobotNavigator::detachCollisionObject ( const std::string &  group_name,
const geometry_msgs::PoseStamped &  place_pose,
const object_manipulation_msgs::Grasp &  grasp 
) [protected]

Definition at line 1739 of file RobotNavigator.cpp.

void RobotNavigator::detachCollisionObjectCallback ( const std::string &  group_name) [protected]

Definition at line 1388 of file RobotNavigator.cpp.

void RobotNavigator::fetchParameters ( std::string  nameSpace = "") [protected, virtual]

Definition at line 27 of file RobotNavigator.cpp.

const arm_navigation_msgs::CollisionObject * RobotNavigator::getCollisionObject ( unsigned int  model_id) [protected]

Definition at line 1841 of file RobotNavigator.cpp.

trajectory_msgs::JointTrajectory RobotNavigator::getGripperTrajectory ( int  graspMove = object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP) [protected, virtual]

Definition at line 1652 of file RobotNavigator.cpp.

std::vector< std::string > RobotNavigator::getJointNames ( const std::string &  group) [protected, virtual]

Definition at line 231 of file RobotNavigator.cpp.

visualization_msgs::Marker & RobotNavigator::getMarker ( std::string  name) [protected]

Definition at line 1365 of file RobotNavigator.cpp.

bool RobotNavigator::getMeshFromDatabasePose ( const household_objects_database_msgs::DatabaseModelPose &  model_pose,
arm_navigation_msgs::CollisionObject obj,
const geometry_msgs::PoseStamped &  pose 
) [protected]

Definition at line 732 of file RobotNavigator.cpp.

bool RobotNavigator::hasMarker ( std::string  name) [protected]

Definition at line 1353 of file RobotNavigator.cpp.

std::string RobotNavigator::makeCollisionObjectNameFromModelId ( unsigned int  model_id) [static, protected]

Definition at line 1834 of file RobotNavigator.cpp.

bool RobotNavigator::moveArm ( const std::string &  group_name,
const std::vector< double > &  joint_positions 
) [protected, virtual]

Definition at line 290 of file RobotNavigator.cpp.

bool RobotNavigator::moveArmThroughPickSequence ( ) [protected, virtual]

Definition at line 1220 of file RobotNavigator.cpp.

bool RobotNavigator::moveArmThroughPlaceSequence ( ) [protected, virtual]

Definition at line 1288 of file RobotNavigator.cpp.

bool RobotNavigator::moveArmToSide ( ) [protected, virtual]

Definition at line 516 of file RobotNavigator.cpp.

bool RobotNavigator::performGraspPlanning ( ) [protected, virtual]

Definition at line 977 of file RobotNavigator.cpp.

bool RobotNavigator::performPickGraspPlanning ( ) [protected, virtual]

Definition at line 788 of file RobotNavigator.cpp.

bool RobotNavigator::performPlaceGraspPlanning ( ) [protected, virtual]

Definition at line 902 of file RobotNavigator.cpp.

bool RobotNavigator::performRecognition ( ) [protected, virtual]

Definition at line 649 of file RobotNavigator.cpp.

bool RobotNavigator::performSegmentation ( ) [protected, virtual]

Definition at line 600 of file RobotNavigator.cpp.

bool RobotNavigator::performTrajectoryFiltering ( const std::string &  group_name,
trajectory_msgs::JointTrajectory &  jt 
) [protected, virtual]

Definition at line 470 of file RobotNavigator.cpp.

void RobotNavigator::printJointTrajectory ( const trajectory_msgs::JointTrajectory &  jt) [protected]

Definition at line 424 of file RobotNavigator.cpp.

void RobotNavigator::printTiming ( ) [protected]

Definition at line 1823 of file RobotNavigator.cpp.

Definition at line 218 of file RobotNavigator.cpp.

void RobotNavigator::run ( void  ) [virtual]

Definition at line 1850 of file RobotNavigator.cpp.

void RobotNavigator::setup ( void  ) [protected, virtual]

Definition at line 75 of file RobotNavigator.cpp.

void RobotNavigator::startCycleTimer ( ) [protected]

Definition at line 1812 of file RobotNavigator.cpp.

bool RobotNavigator::trajectoryFinishedCallback ( bool  storeLastTraj,
TrajectoryExecutionDataVector  tedv 
) [protected, virtual]

Definition at line 186 of file RobotNavigator.cpp.

Definition at line 256 of file RobotNavigator.cpp.

void RobotNavigator::updateCurrentJointStateToLastTrajectoryPoint ( const trajectory_msgs::JointTrajectory &  traj) [protected]

Definition at line 239 of file RobotNavigator.cpp.

bool RobotNavigator::validateJointTrajectory ( trajectory_msgs::JointTrajectory &  jt) [protected]

Definition at line 394 of file RobotNavigator.cpp.


Member Data Documentation

Definition at line 310 of file RobotNavigator.h.

Definition at line 239 of file RobotNavigator.h.

std::string RobotNavigator::arm_group_name_ [protected]

Definition at line 314 of file RobotNavigator.h.

Definition at line 300 of file RobotNavigator.h.

Definition at line 217 of file RobotNavigator.h.

std::map<std::string, object_manipulation_msgs::Grasp> RobotNavigator::current_grasp_map_ [protected]

Definition at line 278 of file RobotNavigator.h.

std::map<std::string, std::string> RobotNavigator::current_grasped_object_name_ [protected]

Definition at line 277 of file RobotNavigator.h.

geometry_msgs::PoseStamped RobotNavigator::current_place_location_ [protected]

Definition at line 288 of file RobotNavigator.h.

Definition at line 257 of file RobotNavigator.h.

Definition at line 220 of file RobotNavigator.h.

Definition at line 259 of file RobotNavigator.h.

Definition at line 291 of file RobotNavigator.h.

boost::condition_variable RobotNavigator::execution_completed_ [protected]

Definition at line 261 of file RobotNavigator.h.

Definition at line 292 of file RobotNavigator.h.

boost::mutex RobotNavigator::execution_mutex_ [protected]

Definition at line 262 of file RobotNavigator.h.

Definition at line 244 of file RobotNavigator.h.

std::string RobotNavigator::grasp_action_service_ [protected]

Definition at line 322 of file RobotNavigator.h.

std::vector<object_manipulation_msgs::Grasp> RobotNavigator::grasp_candidates_ [protected]

Definition at line 281 of file RobotNavigator.h.

Definition at line 249 of file RobotNavigator.h.

Definition at line 284 of file RobotNavigator.h.

Definition at line 279 of file RobotNavigator.h.

Definition at line 280 of file RobotNavigator.h.

Definition at line 285 of file RobotNavigator.h.

Definition at line 233 of file RobotNavigator.h.

Definition at line 297 of file RobotNavigator.h.

std::string RobotNavigator::grasp_planning_service_ [protected]

Definition at line 330 of file RobotNavigator.h.

boost::shared_ptr<GraspSequenceValidator> RobotNavigator::grasp_tester_ [protected]

Definition at line 254 of file RobotNavigator.h.

Definition at line 240 of file RobotNavigator.h.

std::string RobotNavigator::gripper_group_name_ [protected]

Definition at line 315 of file RobotNavigator.h.

std::string RobotNavigator::gripper_link_name_ [protected]

Definition at line 319 of file RobotNavigator.h.

std::string RobotNavigator::ik_plugin_name_ [protected]

Definition at line 337 of file RobotNavigator.h.

Definition at line 238 of file RobotNavigator.h.

std::string RobotNavigator::joint_states_topic_ [protected]

Definition at line 334 of file RobotNavigator.h.

unsigned int RobotNavigator::last_mpr_id_ [protected]

Definition at line 221 of file RobotNavigator.h.

Definition at line 225 of file RobotNavigator.h.

std::string RobotNavigator::MARKER_ARM_LINK = "arm_links" [static]

Definition at line 111 of file RobotNavigator.h.

Definition at line 303 of file RobotNavigator.h.

std::string RobotNavigator::MARKER_ATTACHED_OBJECT = "attached_object" [static]

Definition at line 112 of file RobotNavigator.h.

std::map<std::string,visualization_msgs::Marker> RobotNavigator::marker_map_ [protected]

Definition at line 308 of file RobotNavigator.h.

Definition at line 307 of file RobotNavigator.h.

Definition at line 304 of file RobotNavigator.h.

unsigned int RobotNavigator::max_mpr_id_ [protected]

Definition at line 222 of file RobotNavigator.h.

unsigned int RobotNavigator::max_trajectory_id_ [protected]

Definition at line 223 of file RobotNavigator.h.

std::string RobotNavigator::mesh_database_service_ [protected]

Definition at line 331 of file RobotNavigator.h.

std::string RobotNavigator::model_database_service_ [protected]

Definition at line 332 of file RobotNavigator.h.

Definition at line 295 of file RobotNavigator.h.

std::string RobotNavigator::NAVIGATOR_NAMESPACE = "navigator" [static]

Definition at line 110 of file RobotNavigator.h.

std::string RobotNavigator::NODE_NAME = "robot_pick_place" [static]

Definition at line 109 of file RobotNavigator.h.

Definition at line 234 of file RobotNavigator.h.

Definition at line 232 of file RobotNavigator.h.

std::map<std::string, bool> RobotNavigator::object_in_hand_map_ [protected]

Definition at line 276 of file RobotNavigator.h.

std::string RobotNavigator::path_planner_service_ [protected]

Definition at line 326 of file RobotNavigator.h.

Definition at line 293 of file RobotNavigator.h.

Definition at line 340 of file RobotNavigator.h.

Definition at line 341 of file RobotNavigator.h.

Initial value:
 tf::Transform(
                tf::Quaternion(tf::Vector3(1.0f,0.0f,0.0f),M_PI),tf::Vector3(0.0f,0.0f,0.0f))

Definition at line 116 of file RobotNavigator.h.

Definition at line 342 of file RobotNavigator.h.

boost::shared_ptr<PlaceSequenceValidator> RobotNavigator::place_tester_ [protected]

Definition at line 255 of file RobotNavigator.h.

Definition at line 258 of file RobotNavigator.h.

Definition at line 294 of file RobotNavigator.h.

std::string RobotNavigator::planning_scene_service_ [protected]

Definition at line 333 of file RobotNavigator.h.

Definition at line 230 of file RobotNavigator.h.

Definition at line 229 of file RobotNavigator.h.

std::string RobotNavigator::recognition_service_ [protected]

Definition at line 329 of file RobotNavigator.h.

household_objects_database_msgs::GetModelDescription::Response RobotNavigator::recognized_model_description_ [protected]

Definition at line 273 of file RobotNavigator.h.

std::vector<household_objects_database_msgs::DatabaseModelPoseList> RobotNavigator::recognized_models_ [protected]

Definition at line 272 of file RobotNavigator.h.

std::map<std::string, geometry_msgs::PoseStamped> RobotNavigator::recognized_obj_pose_map_ [protected]

Definition at line 271 of file RobotNavigator.h.

Definition at line 228 of file RobotNavigator.h.

Definition at line 266 of file RobotNavigator.h.

std::string RobotNavigator::segmentation_service_ [protected]

Definition at line 328 of file RobotNavigator.h.

Definition at line 267 of file RobotNavigator.h.

Definition at line 235 of file RobotNavigator.h.

Definition at line 268 of file RobotNavigator.h.

Definition at line 243 of file RobotNavigator.h.

Definition at line 263 of file RobotNavigator.h.

Definition at line 323 of file RobotNavigator.h.

Definition at line 242 of file RobotNavigator.h.

Definition at line 327 of file RobotNavigator.h.

Definition at line 231 of file RobotNavigator.h.

Definition at line 296 of file RobotNavigator.h.

std::string RobotNavigator::VISUALIZATION_TOPIC = "visualization_markers" [static]

Definition at line 113 of file RobotNavigator.h.

std::string RobotNavigator::wrist_link_name_ [protected]

Definition at line 318 of file RobotNavigator.h.


The documentation for this class was generated from the following files:


object_manipulation_tools
Author(s): Jnicho
autogenerated on Mon Oct 6 2014 00:56:17