00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef MOVE_ARM_UTILS_H
00039 #define MOVE_ARM_UTILS_H
00040 #include <planning_environment/models/collision_models.h>
00041 #include <arm_navigation_msgs/PlanningScene.h>
00042 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00043 #include <planning_environment/models/model_utils.h>
00044 #include <rosgraph_msgs/Clock.h>
00045 #include <std_msgs/ColorRGBA.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00048 #include <kinematics_msgs/GetPositionIK.h>
00049 #include <actionlib/server/simple_action_server.h>
00050 #include <actionlib/client/simple_action_client.h>
00051 #include <actionlib/client/simple_client_goal_state.h>
00052 #include <arm_navigation_msgs/GetMotionPlan.h>
00053 #include <arm_navigation_msgs/GetStateValidity.h>
00054 #include <trajectory_msgs/JointTrajectory.h>
00055 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00056 #include <arm_navigation_msgs/convert_messages.h>
00057
00058 #include <srs_assisted_arm_navigation/move_arm_utils/move_arm_warehouse_logger_reader.h>
00059 #include <arm_navigation_msgs/convert_messages.h>
00060 #include <visualization_msgs/InteractiveMarker.h>
00061 #include <interactive_markers/interactive_marker_server.h>
00062 #include <interactive_markers/menu_handler.h>
00063 #include <interactive_markers/tools.h>
00064 #include <arm_navigation_msgs/CollisionObject.h>
00065 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00066 #include <control_msgs/FollowJointTrajectoryAction.h>
00067
00068 #include <std_srvs/Empty.h>
00069 #include <pr2_mechanism_msgs/ListControllers.h>
00070 #include <pr2_mechanism_msgs/LoadController.h>
00071 #include <pr2_mechanism_msgs/UnloadController.h>
00072 #include <pr2_mechanism_msgs/SwitchController.h>
00073 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00074
00075
00076
00077 typedef map<std::string, interactive_markers::MenuHandler::EntryHandle> MenuEntryMap;
00078 typedef map<std::string, MenuEntryMap> MenuMap;
00079 typedef map<std::string, interactive_markers::MenuHandler> MenuHandlerMap;
00080
00085 namespace planning_scene_utils
00086 {
00087
00088 inline static geometry_msgs::Pose toGeometryPose(tf::Transform transform)
00089 {
00090 geometry_msgs::Pose toReturn;
00091 toReturn.position.x = transform.getOrigin().x();
00092 toReturn.position.y = transform.getOrigin().y();
00093 toReturn.position.z = transform.getOrigin().z();
00094 toReturn.orientation.x = transform.getRotation().x();
00095 toReturn.orientation.y = transform.getRotation().y();
00096 toReturn.orientation.z = transform.getRotation().z();
00097 toReturn.orientation.w = transform.getRotation().w();
00098 return toReturn;
00099 }
00100
00101 inline static tf::Transform toBulletTransform(geometry_msgs::Pose pose)
00102 {
00103 tf::Quaternion quat =
00104 tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
00105 tf::Vector3 vec = tf::Vector3(pose.position.x, pose.position.y, pose.position.z);
00106 return tf::Transform(quat, vec);
00107 }
00108
00109 inline static std::string getPlanningSceneNameFromId(const unsigned int id) {
00110 std::stringstream ss;
00111 ss << "Planning Scene " << id;
00112 return ss.str();
00113 }
00114
00115 inline static unsigned int getPlanningSceneIdFromName(const std::string& name) {
00116 std::stringstream ss(name);
00117 std::string temp;
00118 ss >> temp;
00119 ss >> temp;
00120 unsigned int ret;
00121 ss >> ret;
00122 return ret;
00123 }
00124
00125 inline static std::string getMotionPlanRequestNameFromId(const unsigned int id) {
00126 std::stringstream ss;
00127 ss << "MPR " << id;
00128 return ss.str();
00129 }
00130
00131 inline static std::string getTrajectoryNameFromId(const unsigned int id) {
00132 std::stringstream ss;
00133 ss << "Trajectory " << id;
00134 return ss.str();
00135 }
00136
00142 inline static std::string getResultErrorFromCode(int error_code)
00143 {
00144 std::string result;
00145 if(error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
00146 result = "Success";
00147 else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_GOAL)
00148 result = "Invalid Goal";
00149 else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS)
00150 result = "Invalid Joints";
00151 else if(error_code == control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP)
00152 result = "Old header timestamp";
00153 else if(error_code == control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED)
00154 result = "Path tolerance violated";
00155 else if(error_code == control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED)
00156 result = "Goal tolerance violated";
00157 return result;
00158 }
00159
00165 enum PositionType
00166 {
00167 StartPosition, GoalPosition
00168 };
00169
00177 enum RenderType
00178 {
00179 CollisionMesh, VisualMesh, PaddingMesh
00180 };
00181
00188 enum TrajectoryRenderType
00189 {
00190 Kinematic,
00191 Temporal,
00192 };
00193
00194
00195
00196 class PlanningSceneEditor;
00197
00203 class PlanningSceneData
00204 {
00205 protected:
00206 std::string name_;
00207 unsigned int id_;
00208 std::string host_;
00209 ros::Time timestamp_;
00210 arm_navigation_msgs::PlanningScene planning_scene_;
00211 std::vector<std::string> pipeline_stages_;
00212 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes_;
00213 std::set<unsigned int> motion_plan_requests_;
00214
00215 public:
00216 PlanningSceneData();
00217 PlanningSceneData(unsigned int id, const ros::Time& timestamp, const arm_navigation_msgs::PlanningScene& scene);
00218
00220 inline std::string getHostName() const
00221 {
00222 return host_;
00223 }
00224
00226 inline void setHostName(std::string host)
00227 {
00228 host_ = host;
00229 }
00230
00232 inline std::string getName() const
00233 {
00234 return name_;
00235 }
00236
00237 inline unsigned int getId() const
00238 {
00239 return id_;
00240 }
00241
00243 inline const ros::Time& getTimeStamp() const
00244 {
00245 return timestamp_;
00246 }
00247
00249 inline arm_navigation_msgs::PlanningScene& getPlanningScene()
00250 {
00251 return planning_scene_;
00252 }
00253
00255 inline void setTimeStamp(const ros::Time& time)
00256 {
00257 timestamp_ = time;
00258 planning_scene_.robot_state.joint_state.header.stamp = time;
00259 }
00260
00261 inline void setId(unsigned int id) {
00262 id_ = id;
00263 name_ = getPlanningSceneNameFromId(id);
00264 }
00265
00267 inline void setPlanningScene(const arm_navigation_msgs::PlanningScene& scene)
00268 {
00269 planning_scene_ = scene;
00270 timestamp_ = scene.robot_state.joint_state.header.stamp;
00271 }
00272
00276 inline std::vector<std::string>& getPipelineStages()
00277 {
00278 return pipeline_stages_;
00279 }
00280
00282 inline void setPipelineStages(std::vector<std::string>& stages)
00283 {
00284 pipeline_stages_ = stages;
00285 }
00286
00288 inline std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& getErrorCodes()
00289 {
00290 return error_codes_;
00291 }
00292
00294 inline void setErrorCodes(std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes)
00295 {
00296 error_codes_ = error_codes;
00297 }
00298
00300 inline std::set<unsigned int>& getRequests()
00301 {
00302 return motion_plan_requests_;
00303 }
00304
00307 void getRobotState(planning_models::KinematicState* state);
00308
00309 bool hasMotionPlanRequestId(unsigned int id) const {
00310 return(motion_plan_requests_.find(id) != motion_plan_requests_.end());
00311 }
00312
00313 void addMotionPlanRequestId(unsigned int id) {
00314 motion_plan_requests_.insert(id);
00315 }
00316
00317 void removeMotionPlanRequestId(unsigned int id) {
00318 motion_plan_requests_.erase(id);
00319 }
00320
00321 unsigned int getNextMotionPlanRequestId() const {
00322 if(motion_plan_requests_.empty()) {
00323 return 0;
00324 }
00325 return (*motion_plan_requests_.rbegin())+1;
00326 }
00327 };
00328
00334 class MotionPlanRequestData
00335 {
00336 protected:
00337 std::string name_;
00338 unsigned int id_;
00339 std::string source_;
00340 unsigned int planning_scene_id_;
00341 std::string end_effector_link_;
00342 std::string group_name_;
00343 arm_navigation_msgs::MotionPlanRequest motion_plan_request_;
00344 bool is_start_editable_;
00345 bool is_goal_editable_;
00346 bool is_start_visible_;
00347 bool is_goal_visible_;
00348 bool should_refresh_colors_;
00349 bool has_refreshed_colors_;
00350 bool has_path_constraints_;
00351 bool has_good_goal_ik_solution_;
00352 bool has_good_start_ik_solution_;
00353 bool are_collisions_visible_;
00354 bool has_state_changed_;
00355 bool are_joint_controls_visible_;
00356
00357 double roll_tolerance_;
00358 double pitch_tolerance_;
00359 double yaw_tolerance_;
00360
00361 bool constrain_roll_;
00362 bool constrain_pitch_;
00363 bool constrain_yaw_;
00364
00365 std_msgs::ColorRGBA start_color_;
00366 std_msgs::ColorRGBA goal_color_;
00367 std::set<unsigned int> trajectories_;
00368 planning_models::KinematicState* start_state_;
00369 planning_models::KinematicState* goal_state_;
00370 tf::Transform last_good_start_pose_;
00371 tf::Transform last_good_goal_pose_;
00372 visualization_msgs::MarkerArray collision_markers_;
00373 RenderType render_type_;
00374 unsigned int next_trajectory_id_;
00375
00376 public:
00377 MotionPlanRequestData()
00378 {
00379 start_state_ = NULL;
00380 goal_state_ = NULL;
00381 }
00382
00383 MotionPlanRequestData(const planning_models::KinematicState* robot_state);
00384 MotionPlanRequestData(const unsigned int& id,
00385 const std::string& source,
00386 const arm_navigation_msgs::MotionPlanRequest& request,
00387 const planning_models::KinematicState* robot_state,
00388 const std::string& end_effector_link);
00389
00393 ros::Duration refresh_timer_;
00394
00395
00398 void setStartStateValues(std::map<std::string, double>& joint_values);
00399
00402 void setGoalStateValues(std::map<std::string, double>& joint_values);
00403
00406 void updateStartState();
00407
00410 void updateGoalState();
00411
00414 std::vector<std::string> getJointNamesInGoal();
00415
00420 bool isJointNameInGoal(std::string joint);
00421
00423 inline RenderType getRenderType() const
00424 {
00425 return render_type_;
00426 }
00427
00429 inline void setRenderType(RenderType renderType)
00430 {
00431 render_type_ = renderType;
00432 }
00433
00435 inline bool areJointControlsVisible() const
00436 {
00437 return are_joint_controls_visible_;
00438 }
00439
00443 void setJointControlsVisible(bool visible, PlanningSceneEditor* editor);
00444
00446 inline bool hasStateChanged() const
00447 {
00448 return has_state_changed_;
00449 }
00450
00452 inline void setStateChanged(bool changed)
00453 {
00454 has_state_changed_ = changed;
00455 }
00456
00458 inline visualization_msgs::MarkerArray& getCollisionMarkers()
00459 {
00460 return collision_markers_;
00461 }
00462
00464 inline bool areCollisionsVisible() const
00465 {
00466 return are_collisions_visible_;
00467 }
00468
00470 inline void setCollisionsVisible(bool visible)
00471 {
00472 are_collisions_visible_ = visible;
00473 }
00474
00476 inline void showCollisions()
00477 {
00478 setCollisionsVisible(true);
00479 }
00480
00482 inline void hideCollisions()
00483 {
00484 setCollisionsVisible(false);
00485 }
00486
00488 inline tf::Transform getLastGoodStartPose() const
00489 {
00490 return last_good_start_pose_;
00491 }
00492
00494 inline tf::Transform getLastGoodGoalPose() const
00495 {
00496 return last_good_goal_pose_;
00497 }
00498
00500 inline void setLastGoodStartPose(tf::Transform pose)
00501 {
00502 last_good_start_pose_ = pose;
00503 }
00504
00506 inline void setLastGoodGoalPose(tf::Transform pose)
00507 {
00508 last_good_goal_pose_ = pose;
00509 }
00510
00512 inline void setStartState(planning_models::KinematicState* state)
00513 {
00514 start_state_ = state;
00515 setStateChanged(true);
00516 }
00517
00519 inline void setGoalState(planning_models::KinematicState* state)
00520 {
00521 goal_state_ = state;
00522 setStateChanged(true);
00523 }
00524
00526 inline void reset()
00527 {
00528 if(start_state_ != NULL)
00529 {
00530 delete start_state_;
00531 start_state_ = NULL;
00532 }
00533
00534 if(goal_state_ != NULL)
00535 {
00536 delete goal_state_;
00537 goal_state_ = NULL;
00538 }
00539 }
00540
00542 inline bool hasGoodIKSolution(const PositionType& type) const
00543 {
00544 if(type == StartPosition) {
00545 return has_good_start_ik_solution_;
00546 } else {
00547 return has_good_goal_ik_solution_;
00548 }
00549 }
00550
00552 inline void setHasGoodIKSolution(const bool& solution, const PositionType& type)
00553 {
00554 if(type == StartPosition) {
00555 has_good_start_ik_solution_ = solution;
00556 } else {
00557 has_good_goal_ik_solution_ = solution;
00558 }
00559 }
00560
00562 inline planning_models::KinematicState* getStartState()
00563 {
00564 return start_state_;
00565 }
00566
00568 inline planning_models::KinematicState* getGoalState()
00569 {
00570 return goal_state_;
00571 }
00572
00575 inline std::string getGroupName() const
00576 {
00577 return group_name_;
00578 }
00579
00581 inline std::string getEndEffectorLink() const
00582 {
00583 return end_effector_link_;
00584 }
00585
00588 inline void setGroupName(const std::string& name)
00589 {
00590 group_name_ = name;
00591 }
00592
00594 inline void setEndEffectorLink(const std::string& name)
00595 {
00596 end_effector_link_ = name;
00597 }
00598
00600 inline const std::string& getName() const
00601 {
00602 return name_;
00603 }
00604
00605 unsigned int getId() const {
00606 return id_;
00607 }
00608
00610 inline void setId(const unsigned int id)
00611 {
00612 id_ = id;
00613 name_ = getMotionPlanRequestNameFromId(id_);
00614 }
00615
00617 inline bool shouldRefreshColors() const
00618 {
00619 return should_refresh_colors_;
00620 }
00621
00623 inline bool hasRefreshedColors() const
00624 {
00625 return has_refreshed_colors_;
00626 }
00627
00629 inline void setHasRefreshedColors(bool refresh)
00630 {
00631 has_refreshed_colors_ = refresh;
00632
00633 if(refresh)
00634 {
00635 should_refresh_colors_ = false;
00636 }
00637 }
00638
00641 inline void refreshColors()
00642 {
00643 should_refresh_colors_ = true;
00644 has_refreshed_colors_ = false;
00645 refresh_timer_ = ros::Duration(0.0);
00646 }
00647
00650 inline bool isStartVisible() const
00651 {
00652 return is_start_visible_;
00653 }
00654
00657 inline bool isEndVisible() const
00658 {
00659 return is_goal_visible_;
00660 }
00661
00663 inline void setStartVisible(bool visible)
00664 {
00665 is_start_visible_ = visible;
00666 }
00667
00669 inline void setEndVisible(bool visible)
00670 {
00671 is_goal_visible_ = visible;
00672 }
00673
00675 inline void show()
00676 {
00677 setStartVisible(true);
00678 setEndVisible(true);
00679 }
00680
00682 inline void hide()
00683 {
00684 setStartVisible(false);
00685 setEndVisible(false);
00686 }
00687
00689 inline void showStart()
00690 {
00691 setStartVisible(true);
00692 }
00693
00695 inline void showGoal()
00696 {
00697 setEndVisible(true);
00698 }
00699
00701 inline void hideStart()
00702 {
00703 setStartVisible(false);
00704 }
00705
00707 inline void hideGoal()
00708 {
00709 setEndVisible(false);
00710 }
00711
00713 inline const std_msgs::ColorRGBA& getStartColor() const
00714 {
00715 return start_color_;
00716 }
00717
00719 inline const std_msgs::ColorRGBA& getGoalColor() const
00720 {
00721 return goal_color_;
00722 }
00723
00725 inline void setStartColor(std_msgs::ColorRGBA color)
00726 {
00727 start_color_ = color;
00728 }
00729
00731 inline void setGoalColor(std_msgs::ColorRGBA color)
00732 {
00733 goal_color_ = color;
00734 }
00735
00738 inline void setStartEditable(bool editable)
00739 {
00740 is_start_editable_ = editable;
00741 }
00742
00745 inline void setGoalEditable(bool editable)
00746 {
00747 is_goal_editable_ = editable;
00748 }
00749
00752 inline bool isStartEditable() const
00753 {
00754 return is_start_editable_;
00755 }
00756
00759 inline bool isGoalEditable() const
00760 {
00761 return is_goal_editable_;
00762 }
00763
00765 inline void setSource(const std::string& source)
00766 {
00767 source_ = source;
00768 }
00769
00771 inline const std::string& getSource() const
00772 {
00773 return source_;
00774 }
00775
00777 inline arm_navigation_msgs::MotionPlanRequest& getMotionPlanRequest()
00778 {
00779 return motion_plan_request_;
00780 }
00781
00783 inline void setMotionPlanRequest(const arm_navigation_msgs::MotionPlanRequest& request)
00784 {
00785 motion_plan_request_ = request;
00786 setGroupName(request.group_name);
00787 updateStartState();
00788 updateGoalState();
00789 }
00790
00792 inline void setPlanningSceneId(const unsigned int id)
00793 {
00794 planning_scene_id_ = id;
00795 }
00796 inline unsigned int getPlanningSceneId() const
00797 {
00798 return planning_scene_id_;
00799 }
00800
00802 inline std::string getPlanningSceneName() const
00803 {
00804 return getPlanningSceneNameFromId(planning_scene_id_);
00805 }
00806
00808 inline std::set<unsigned int>& getTrajectories()
00809 {
00810 return trajectories_;
00811 }
00812
00813 unsigned int getNextTrajectoryId() const {
00814 if(trajectories_.empty()) return 0;
00815 return (*trajectories_.rbegin())+1;
00816 }
00817
00818 void addTrajectoryId(unsigned int id) {
00819 trajectories_.insert(id);
00820 }
00821
00822 void removeTrajectoryId(unsigned int id) {
00823 trajectories_.erase(id);
00824 }
00825
00826 bool hasTrajectoryId(unsigned int id) const {
00827 return (trajectories_.find(id) != trajectories_.end());
00828 }
00829
00830 bool hasPathConstraints() const {
00831 return has_path_constraints_;
00832 }
00833
00834 void setPathConstraints(bool has) {
00835 has_path_constraints_ = has;
00836 }
00837
00838 bool getConstrainRoll() const {
00839 return constrain_roll_;
00840 }
00841
00842 void setConstrainRoll(bool s) {
00843 constrain_roll_ = s;
00844 }
00845
00846 bool getConstrainPitch() const {
00847 return constrain_pitch_;
00848 }
00849
00850 void setConstrainPitch(bool s) {
00851 constrain_pitch_ = s;
00852 }
00853
00854 bool getConstrainYaw() const {
00855 return constrain_yaw_;
00856 }
00857
00858 void setConstrainYaw(bool s) {
00859 constrain_yaw_ = s;
00860 }
00861
00862 double getRollTolerance() const {
00863 return roll_tolerance_;
00864 }
00865
00866 void setRollTolerance(double s) {
00867 roll_tolerance_ = s;
00868 }
00869
00870 double getPitchTolerance() const {
00871 return pitch_tolerance_;
00872 }
00873
00874 void setPitchTolerance(double s) {
00875 pitch_tolerance_ = s;
00876 }
00877
00878 double getYawTolerance() const {
00879 return yaw_tolerance_;
00880 }
00881
00882 void setYawTolerance(double s) {
00883 yaw_tolerance_ = s;
00884 }
00885
00886 void setGoalAndPathPositionOrientationConstraints(arm_navigation_msgs::MotionPlanRequest& mpr,
00887 planning_scene_utils::PositionType type) const;
00888
00890 void updateCollisionMarkers(planning_environment::CollisionModels* cm_,
00891 ros::ServiceClient* distance_state_validity_service_client_);
00892 };
00893
00894
00900 class TrajectoryData
00901 {
00902 public:
00903
00904 enum MarkerType {
00905 VISUAL,
00906 COLLISION,
00907 PADDED
00908 };
00909
00910 protected:
00911 std::string name_;
00912 unsigned int id_;
00913 std::string source_;
00914 std::string group_name_;
00915 unsigned int planning_scene_id_;
00916 unsigned int motion_plan_request_Id_;
00917 trajectory_msgs::JointTrajectory trajectory_;
00918 trajectory_msgs::JointTrajectory trajectory_error_;
00919 bool is_visible_;
00920 MarkerType marker_type_;
00921 bool is_playing_;
00922 ros::Time playback_start_time_;
00923 bool collisions_visible_;
00924 bool state_changed_;
00925 std_msgs::ColorRGBA color_;
00926 unsigned int current_trajectory_point_;
00927 unsigned int trajectory_bad_point_;
00928 planning_models::KinematicState* current_state_;
00929 ros::Duration duration_;
00930 bool should_refresh_colors_;
00931 bool has_refreshed_colors_;
00932 visualization_msgs::MarkerArray collision_markers_;
00933 RenderType render_type_;
00934 TrajectoryRenderType trajectory_render_type_;
00935 ros::Duration time_to_stop_;
00936 public:
00937
00939 ros::Duration refresh_timer_;
00940
00942 arm_navigation_msgs::ArmNavigationErrorCodes trajectory_error_code_;
00943
00944 TrajectoryData();
00945 TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
00946 const trajectory_msgs::JointTrajectory& trajectory);
00947 TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
00948 const trajectory_msgs::JointTrajectory& trajectory, const trajectory_msgs::JointTrajectory& trajectory_error);
00949
00952 void advanceThroughTrajectory(int amount);
00953
00956 void advanceToNextClosestPoint(ros::Time time);
00957
00959 void updateCurrentState();
00960
00962 inline bool hasStateChanged() const
00963 {
00964 return state_changed_;
00965 }
00966
00968 inline void setStateChanged(bool changed)
00969 {
00970 state_changed_ = changed;
00971 }
00972
00974 inline visualization_msgs::MarkerArray& getCollisionMarkers()
00975 {
00976 return collision_markers_;
00977 }
00978
00980 inline bool areCollisionsVisible() const
00981 {
00982 return collisions_visible_;
00983 }
00984
00986 inline void setCollisionsVisible(bool shown)
00987 {
00988 collisions_visible_ = shown;
00989 }
00990
00992 inline void showCollisions()
00993 {
00994 setCollisionsVisible(true);
00995 }
00996
00998 inline void hideCollisions()
00999 {
01000 setCollisionsVisible(false);
01001 }
01002
01004 inline size_t getTrajectorySize() const
01005 {
01006 return trajectory_.points.size();
01007 }
01008
01010 inline bool shouldRefreshColors() const
01011 {
01012 return should_refresh_colors_;
01013 }
01014
01016 inline bool hasRefreshedColors() const
01017 {
01018 return has_refreshed_colors_;
01019 }
01020
01022 inline void setHasRefreshedColors(bool refresh)
01023 {
01024 has_refreshed_colors_ = refresh;
01025
01026 if(refresh)
01027 {
01028 should_refresh_colors_ = false;
01029 }
01030 }
01031
01034 inline void refreshColors()
01035 {
01036 should_refresh_colors_ = true;
01037 has_refreshed_colors_ = false;
01038 refresh_timer_ = ros::Duration(0.0);
01039 }
01040
01042 inline RenderType getRenderType() const
01043 {
01044 return render_type_;
01045 }
01046
01048 inline void setRenderType(RenderType renderType)
01049 {
01050 render_type_ = renderType;
01051 }
01052
01054 inline TrajectoryRenderType getTrajectoryRenderType() const
01055 {
01056 return trajectory_render_type_;
01057 }
01058
01060 inline void setTrajectoryRenderType(TrajectoryRenderType renderType)
01061 {
01062 trajectory_render_type_ = renderType;
01063 }
01064
01066 inline void reset()
01067 {
01068
01069 if(current_state_ != NULL)
01070 {
01071 delete current_state_;
01072 current_state_ = NULL;
01073 }
01074
01075 is_playing_ = false;
01076 is_visible_ = false;
01077 current_trajectory_point_ = 0;
01078 state_changed_ = false;
01079 }
01080
01083 inline planning_models::KinematicState* getCurrentState()
01084 {
01085 return current_state_;
01086 }
01087
01089 inline void setCurrentState(planning_models::KinematicState* state)
01090 {
01091 current_state_ = state;
01092 state_changed_ = true;
01093 }
01094
01096 inline void setMotionPlanRequestId(const unsigned int& Id)
01097 {
01098 motion_plan_request_Id_ = Id;
01099 }
01100
01102 inline const unsigned int& getMotionPlanRequestId() const
01103 {
01104 return motion_plan_request_Id_;
01105 }
01106
01107 inline void setPlanningSceneId(const unsigned int id) {
01108 planning_scene_id_ = id;
01109 }
01110
01111 unsigned int getPlanningScendId() const {
01112 return planning_scene_id_;
01113 }
01114
01116 inline void setCurrentPoint(unsigned int point)
01117 {
01118 current_trajectory_point_ = point;
01119 state_changed_ = true;
01120 }
01121
01123 inline unsigned int getCurrentPoint() const
01124 {
01125 return current_trajectory_point_;
01126 }
01127
01129 inline unsigned int getBadPoint() const
01130 {
01131 return trajectory_bad_point_;
01132 }
01133
01135 inline void setGroupname(const std::string& group_name)
01136 {
01137 group_name_ = group_name;
01138 }
01139
01141 inline bool isPlaying() const
01142 {
01143 return is_playing_;
01144 }
01145
01147 inline void play()
01148 {
01149 is_playing_ = true;
01150 playback_start_time_ = ros::Time::now();
01151 }
01152
01154 inline void stop()
01155 {
01156 is_playing_ = false;
01157 }
01158
01160 inline bool isVisible() const
01161 {
01162 return is_visible_;
01163 }
01164
01166 inline void setVisible(bool visible)
01167 {
01168 is_visible_ = visible;
01169 }
01170
01171 inline MarkerType getMarkerType() const
01172 {
01173 return marker_type_;
01174 }
01175
01177 inline void setMarkerType(MarkerType mt)
01178 {
01179 marker_type_ = mt;
01180 }
01181
01183 inline void show()
01184 {
01185 setVisible(true);
01186 }
01187
01189 inline void hide()
01190 {
01191 setVisible(false);
01192 }
01193
01196 inline const ros::Duration& getDuration() const
01197 {
01198 return duration_;
01199 }
01200
01202 inline void setDuration(const ros::Duration& duration)
01203 {
01204 duration_ = duration;
01205 }
01206
01208 inline const std_msgs::ColorRGBA& getColor() const
01209 {
01210 return color_;
01211 }
01212
01214 inline void setColor(const std_msgs::ColorRGBA& color)
01215 {
01216 color_ = color;
01217 }
01218
01220 inline std::string getSource()
01221 {
01222 return source_;
01223 }
01224
01226 inline trajectory_msgs::JointTrajectory& getTrajectory()
01227 {
01228 return trajectory_;
01229 }
01230
01232 inline void setSource(const std::string& source)
01233 {
01234 source_ = source;
01235 }
01236
01238 inline void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
01239 {
01240 trajectory_ = trajectory;
01241 }
01242
01244 inline trajectory_msgs::JointTrajectory& getTrajectoryError()
01245 {
01246 return trajectory_error_;
01247 }
01248
01250 inline void setTrajectoryError(const trajectory_msgs::JointTrajectory& trajectory_error)
01251 {
01252 trajectory_error_ = trajectory_error;
01253 }
01254
01256 inline unsigned int getId() const
01257 {
01258 return id_;
01259 }
01260
01262 inline void setId(const unsigned int& id)
01263 {
01264 id_ = id;
01265 name_ = getTrajectoryNameFromId(id_);
01266 }
01267
01268 const std::string& getName() const {
01269 return name_;
01270 }
01271
01273 inline const std::string& getGroupName() const
01274 {
01275 return group_name_;
01276 }
01277
01279 inline void setBadPoint(unsigned int point)
01280 {
01281 trajectory_bad_point_ = point;
01282 }
01283
01285 inline void setGroupName(std::string name)
01286 {
01287 group_name_ = name;
01288 }
01289
01292 void updateCollisionMarkers(planning_environment::CollisionModels* cm_, MotionPlanRequestData& motionPlanRequest,
01293 ros::ServiceClient* distance_state_validity_service_client_);
01294 };
01295
01301 struct PlanningSceneParameters
01302 {
01303 std::string left_ik_name_;
01304 std::string right_ik_name_;
01305 std::string non_coll_left_ik_name_;
01306 std::string non_coll_right_ik_name_;
01307 std::string left_interpolate_service_name_;
01308 std::string right_interpolate_service_name_;
01309 std::string planner_1_service_name_;
01310 std::string planner_2_service_name_;
01311 std::string proximity_space_service_name_;
01312 std::string proximity_space_validity_name_;
01313 std::string set_planning_scene_diff_name_;
01314 std::string trajectory_filter_1_service_name_;
01315 std::string trajectory_filter_2_service_name_;
01316 std::string proximity_space_planner_name_;
01317 std::string vis_topic_name_;
01318 std::string right_ik_link_;
01319 std::string left_ik_link_;
01320 std::string left_redundancy_;
01321 std::string right_redundancy_;
01322 std::string right_arm_group_;
01323 std::string left_arm_group_;
01324 std::string execute_left_trajectory_;
01325 std::string execute_right_trajectory_;
01326 std::string list_controllers_service_;
01327 std::string unload_controllers_service_;
01328 std::string load_controllers_service_;
01329 std::string switch_controllers_service_;
01330 std::string gazebo_model_name_;
01331 std::string robot_description_param_;
01332 bool use_robot_data_;
01333 bool sync_robot_state_with_gazebo_;
01334 };
01335
01340 class PlanningSceneEditor
01341 {
01342 public:
01347 enum GeneratedShape
01348 {
01349 Box, Cylinder, Sphere
01350 };
01351 protected:
01352
01361 enum MonitorStatus
01362 {
01363 idle, Executing, WaitingForStop, Done
01364 };
01365
01374 struct StateRegistry
01375 {
01376 planning_models::KinematicState* state;
01377 std::string source;
01378 };
01379
01385 struct SelectableObject
01386 {
01387 SelectableObject() {
01388 attach_ = false;
01389 detach_ = false;
01390 attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01391 collision_object_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01392 }
01393
01394 bool attach_;
01395 bool detach_;
01396 arm_navigation_msgs::AttachedCollisionObject attached_collision_object_;
01397 arm_navigation_msgs::CollisionObject collision_object_;
01398 visualization_msgs::InteractiveMarker selection_marker_;
01399 visualization_msgs::InteractiveMarker control_marker_;
01400 std_msgs::ColorRGBA color_;
01401
01402 std::string id_;
01403 };
01404
01410 struct IKController
01411 {
01412 unsigned int motion_plan_id_;
01413 visualization_msgs::InteractiveMarker start_controller_;
01414 visualization_msgs::InteractiveMarker end_controller_;
01415 };
01416
01421 virtual void updateState()
01422 {
01423 }
01424
01429 virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;
01430
01435 virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;
01436
01437 virtual void attachObjectCallback(const std::string& name) = 0;
01438
01439
01440 void changeToAttached(const std::string& name);
01441
01447 virtual void selectedTrajectoryCurrentPointChanged( unsigned int new_current_point ) {}
01448
01449 boost::recursive_mutex lock_scene_;
01450 arm_navigation_msgs::ArmNavigationErrorCodes last_collision_set_error_code_;
01451 move_arm_warehouse::MoveArmWarehouseLoggerReader* move_arm_warehouse_logger_reader_;
01452 planning_environment::CollisionModels* cm_;
01453 planning_models::KinematicState* robot_state_;
01454 PlanningSceneParameters params_;
01455 ros::NodeHandle nh_;
01456 ros::Publisher clock_publisher_;
01457 ros::Publisher joint_state_publisher_;
01458 ros::Publisher vis_marker_array_publisher_;
01459 ros::Publisher vis_marker_publisher_;
01460 ros::Subscriber joint_state_subscriber_;
01461 ros::Subscriber r_arm_controller_state_subscriber_;
01462 ros::Subscriber l_arm_controller_state_subscriber_;
01463 ros::ServiceClient collision_proximity_planner_client_;
01464 ros::ServiceClient distance_aware_service_client_;
01465 ros::ServiceClient distance_state_validity_service_client_;
01466 ros::ServiceClient set_planning_scene_diff_client_;
01467 ros::ServiceClient left_ik_service_client_;
01468 ros::ServiceClient left_interpolate_service_client_;
01469 ros::ServiceClient non_coll_left_ik_service_client_;
01470 ros::ServiceClient non_coll_right_ik_service_client_;
01471 ros::ServiceClient planning_1_service_client_;
01472 ros::ServiceClient planning_2_service_client_;
01473 ros::ServiceClient right_ik_service_client_;
01474 ros::ServiceClient right_interpolate_service_client_;
01475 ros::ServiceClient trajectory_filter_1_service_client_;
01476 ros::ServiceClient trajectory_filter_2_service_client_;
01477 ros::ServiceClient gazebo_joint_state_client_;
01478 ros::ServiceClient list_controllers_client_;
01479 ros::ServiceClient load_controllers_client_;
01480 ros::ServiceClient unload_controllers_client_;
01481 ros::ServiceClient switch_controllers_client_;
01482 ros::ServiceClient pause_gazebo_client_;
01483 ros::ServiceClient unpause_gazebo_client_;
01484 ros::ServiceClient set_link_properties_client_;
01485 ros::ServiceClient get_link_properties_client_;
01486
01487 std::map<std::string, double> robot_state_joint_values_;
01488 std::vector<ros::Time> last_creation_time_query_;
01489 tf::TransformBroadcaster transform_broadcaster_;
01490 tf::TransformListener transform_listener_;
01491 std::map<std::string, actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>*> arm_controller_map_;
01492 unsigned int max_trajectory_id_;
01493 unsigned int max_collision_object_id_;
01494
01495 bool warehouse_data_loaded_once_;
01496 int active_planner_index_;
01497 bool use_primary_filter_;
01498
01499 trajectory_msgs::JointTrajectory logged_trajectory_;
01500 trajectory_msgs::JointTrajectory logged_trajectory_controller_error_;
01501 ros::Time logged_trajectory_start_time_;
01502 int logged_trajectory_controller_error_code_;
01503
01504 bool send_collision_markers_;
01505 std::string collision_marker_state_;
01506 visualization_msgs::MarkerArray collision_markers_;
01507 planning_models::KinematicState* paused_collision_state_;
01508 std_msgs::ColorRGBA point_color_;
01509 std::vector<geometry_msgs::TransformStamped> robot_transforms_;
01510
01511 interactive_markers::MenuHandler::FeedbackCallback attached_collision_object_feedback_ptr_;
01512 interactive_markers::MenuHandler::FeedbackCallback collision_object_selection_feedback_ptr_;
01513 interactive_markers::MenuHandler::FeedbackCallback collision_object_movement_feedback_ptr_;
01514 interactive_markers::MenuHandler::FeedbackCallback ik_control_feedback_ptr_;
01515 interactive_markers::MenuHandler::FeedbackCallback joint_control_feedback_ptr_;
01516
01517 interactive_markers::InteractiveMarkerServer* interactive_marker_server_;
01518
01519 std::map<std::string, SelectableObject>* selectable_objects_;
01520 std::map<std::string, IKController>* ik_controllers_;
01521
01522 std::string current_planning_scene_name_;
01523 std::string selected_motion_plan_name_;
01524 std::string selected_trajectory_name_;
01525 std::string logged_group_name_;
01526 std::string logged_motion_plan_request_;
01527 std::map<string, MenuEntryMap> menu_entry_maps_;
01528 MenuHandlerMap menu_handler_map_;
01529
01530 std::map<string, ros::ServiceClient*>* collision_aware_ik_services_;
01531 std::map<string, ros::ServiceClient*>* non_collision_aware_ik_services_;
01532 std::map<string, ros::ServiceClient*>* interpolated_ik_services_;
01533 std::map<string, arm_navigation_msgs::ArmNavigationErrorCodes> error_map_;
01534 std::vector<StateRegistry> states_;
01535
01536 interactive_markers::MenuHandler::EntryHandle last_resize_handle_;
01537
01538 std_msgs::ColorRGBA last_collision_object_color_;
01539 std_msgs::ColorRGBA last_mesh_object_color_;
01540
01541 MonitorStatus monitor_status_;
01542 ros::Time time_of_controller_done_callback_;
01543 ros::Time time_of_last_moving_notification_;
01544
01545 ros::Time last_marker_start_time_;
01546 ros::Duration marker_dt_;
01547
01548 void attachCollisionObject(const std::string& name,
01549 const std::string& link_name,
01550 const std::vector<std::string>& touch_links);
01551
01555 void createSelectableMarkerFromCollisionObject(arm_navigation_msgs::CollisionObject& object, std::string name,
01556 std::string description, std_msgs::ColorRGBA color, bool insert_selectable=true);
01557
01558
01559
01560 public:
01561
01563 std::map<std::string, PlanningSceneData> planning_scene_map_;
01564
01566 std::map<std::string, std::map<std::string, TrajectoryData> > trajectory_map_;
01567
01569 std::map<std::string, MotionPlanRequestData> motion_plan_map_;
01570
01572 std::map<std::string, bool> joint_clicked_map_;
01573
01575 std::map<std::string, tf::Transform> joint_prev_transform_map_;
01576
01577 PlanningSceneEditor();
01578 PlanningSceneEditor(PlanningSceneParameters& params);
01579 ~PlanningSceneEditor();
01580
01588 bool filterTrajectory(MotionPlanRequestData& requestData, TrajectoryData& trajectory, unsigned int& filter_id);
01589
01598 bool getAllAssociatedMotionPlanRequests(const unsigned int id,
01599 std::vector<unsigned int>& ids,
01600 std::vector<std::string>& stages,
01601 std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);
01602
01609 bool getAllAssociatedPausedStates(const unsigned int id,
01610 std::vector<ros::Time>& paused_times);
01611
01618 bool getAllAssociatedTrajectorySources(const unsigned int planning_id,
01619 const unsigned int mpr_id,
01620 std::vector<unsigned int>& trajectory_ids,
01621 std::vector<std::string>& trajectory_sources);
01622
01628 bool getAllPlanningSceneTimes(std::vector<ros::Time>& planning_scene_times,
01629 vector<unsigned int>& planning_scene_ids);
01630
01631
01641 bool getMotionPlanRequest(const ros::Time& time, const std::string& stage,
01642 arm_navigation_msgs::MotionPlanRequest& mpr, std::string& id,
01643 std::string& planning_scene_id);
01644
01652 bool getPausedState(const unsigned int id,
01653 const ros::Time& paused_time,
01654 head_monitor_msgs::HeadMonitorFeedback& paused_state);
01655
01664 bool getPlanningSceneOutcomes(const unsigned int id,
01665 std::vector<std::string>& pipeline_stages,
01666 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes,
01667 std::map<std::string, arm_navigation_msgs::ArmNavigationErrorCodes>& error_map);
01668
01669
01676 bool loadPlanningScene(const ros::Time& time,
01677 const unsigned int id);
01678
01689 bool planToKinematicState(const planning_models::KinematicState& state, const std::string& group_name,
01690 const std::string& end_effector_name, unsigned int& trajectoryid_Out,
01691 unsigned int& planning_scene_id);
01692
01699 bool planToRequest(MotionPlanRequestData& data, unsigned int& trajectoryid_Out);
01700
01707 bool planToRequest(const std::string& requestid, unsigned int& trajectoryid_Out);
01708
01715 bool playTrajectory(MotionPlanRequestData& requestData, TrajectoryData& data);
01716
01722 bool sendPlanningScene(PlanningSceneData& data);
01723
01734 bool solveIKForEndEffectorPose(MotionPlanRequestData& mpr, PositionType type, bool coll_aware = true,
01735 double change_redundancy = 0.0);
01736
01737
01745 interactive_markers::MenuHandler::EntryHandle registerMenuEntry(std::string menu, std::string entryName,
01746 interactive_markers::MenuHandler::FeedbackCallback& callback);
01747
01748
01749
01758 interactive_markers::MenuHandler::EntryHandle registerSubMenuEntry(std::string menu, std::string name,
01759 std::string subMenu,
01760 interactive_markers::MenuHandler::FeedbackCallback& callback);
01761
01766 std::string createNewPlanningScene();
01767
01774 virtual void onPlanningSceneLoaded(int scene, int numScenes)
01775 {
01776 }
01777
01782 void collisionObjectMovementCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01783
01788 void collisionObjectSelectionCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01789
01790 void attachedCollisionObjectInteractiveCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01791
01802 std::string createCollisionObject(const std::string& name, geometry_msgs::Pose pose, GeneratedShape shape, float scaleX, float scaleY,
01803 float scaleZ, std_msgs::ColorRGBA color,
01804 bool selectable);
01805
01806 std::string createMeshObject(const std::string& name,
01807 geometry_msgs::Pose pose,
01808 const std::string& filename,
01809 const tf::Vector3& scale,
01810 std_msgs::ColorRGBA color);
01817 void createIKController(MotionPlanRequestData& data, PositionType type, bool rePose = true);
01818
01825 void createIkControllersFromMotionPlanRequest(MotionPlanRequestData& data, bool rePose = true);
01826
01832 void createJointMarkers(MotionPlanRequestData& data, planning_scene_utils::PositionType position);
01833
01834
01846 void createMotionPlanRequest(const planning_models::KinematicState& start_state,
01847 const planning_models::KinematicState& end_state,
01848 const std::string& group_name,
01849 const std::string& end_effector_name,
01850 const unsigned int& planning_scene_name,
01851 const bool fromRobotState,
01852 unsigned int& motionPlan_id_Out);
01853
01854
01862 void initMotionPlanRequestData(const unsigned int& planning_scene_id,
01863 const std::vector<unsigned int>& ids,
01864 const std::vector<std::string>& stages,
01865 const std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);
01866
01867
01873 void deleteJointMarkers(MotionPlanRequestData& data, PositionType type);
01874
01879 void deleteKinematicStates();
01880
01885 void deleteMotionPlanRequest(const unsigned int& id,
01886 std::vector<unsigned int>& erased_trajectories);
01887
01892 void deleteTrajectory(unsigned int mpr_id, unsigned int traj_id);
01893
01894
01902
01903
01904
01905
01906
01911 void executeTrajectory(const std::string& mpr_name, const std::string& traj_name);
01912
01917 void executeTrajectory(TrajectoryData& data);
01918
01925 void getAllRobotStampedTransforms(const planning_models::KinematicState& state,
01926 vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp);
01927
01932 void getMotionPlanningMarkers(visualization_msgs::MarkerArray& arr);
01933
01938 void getTrajectoryMarkers(visualization_msgs::MarkerArray& arr);
01939
01944 void IKControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01945
01950 void JointControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
01951
01957 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
01958 const control_msgs::FollowJointTrajectoryResultConstPtr& result);
01959
01963 void armHasStoppedMoving();
01964
01969 void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
01970
01975 void jointTrajectoryControllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& joint_state);
01976
01977
01981 void loadAllWarehouseData();
01982
01992 void makeInteractive1DOFRotationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
01993 float scale = 1.0f, float angle = 0.0f);
01994
02004 void makeInteractive1DOFTranslationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
02005 float scale = 1.0f, float value = 0.0f);
02006
02010 void printTrajectoryPoint(const std::vector<std::string>& joint_names, const std::vector<double>& joint_values);
02011
02017 void randomlyPerturb(MotionPlanRequestData& mpr, PositionType type);
02018
02023 void savePlanningScene(PlanningSceneData& data, bool copy = false);
02024
02028 void sendMarkers();
02029
02033 void sendTransformsAndClock();
02034
02035 void makeSelectableAttachedObjectFromPlanningScene(const arm_navigation_msgs::PlanningScene& scene,
02036 arm_navigation_msgs::AttachedCollisionObject& att);
02037
02044 void setCurrentPlanningScene(std::string id, bool loadRequests = true, bool loadTrajectories = true);
02045
02052 void setIKControlsVisible(std::string id, PositionType type, bool visible);
02053
02062 void setJointState(MotionPlanRequestData& data, PositionType position, std::string& jointName, tf::Transform value);
02063
02068 void updateJointStates();
02069
02070 bool hasTrajectory(const std::string& mpr_name,
02071 const std::string& traj_name) const;
02072
02077 inline std::string generateNewCollisionObjectId()
02078 {
02079 std::stringstream stream;
02080 stream << "collision_object_";
02081 max_collision_object_id_++;
02082 stream << max_collision_object_id_;
02083 return stream.str();
02084 }
02085
02090 inline unsigned int generateNewPlanningSceneId()
02091 {
02092 return move_arm_warehouse_logger_reader_->determineNextPlanningSceneId();
02093 }
02094
02095
02100 void deleteCollisionObject(std::string& name);
02101
02102
02103 inline void lockScene()
02104 {
02105 lock_scene_.lock();
02106 }
02107
02108 inline void unlockScene()
02109 {
02110 lock_scene_.unlock();
02111 }
02112
02113 inline planning_environment::CollisionModels* getCollisionModel()
02114 {
02115 return cm_;
02116 }
02117
02118 inline void setCollisionModel(planning_environment::CollisionModels* model, bool shouldDelete = false)
02119 {
02120 if(shouldDelete && cm_ != NULL)
02121 {
02122 delete cm_;
02123 cm_ = model;
02124 }
02125 cm_ = model;
02126 }
02127
02128 inline planning_models::KinematicState* getRobotState()
02129 {
02130 return robot_state_;
02131 }
02132
02133 inline void setRobotState(planning_models::KinematicState* robot_state, bool shouldDelete = true)
02134 {
02135 if(shouldDelete && robot_state_ != NULL)
02136 {
02137 delete robot_state_;
02138 robot_state_ = NULL;
02139 }
02140
02141 robot_state_ = robot_state;
02142 }
02143
02144 inline move_arm_warehouse::MoveArmWarehouseLoggerReader* getLoggerReader()
02145 {
02146 return move_arm_warehouse_logger_reader_;
02147 }
02148
02149 inline void setLoggerReader(move_arm_warehouse::MoveArmWarehouseLoggerReader* loggerReader,
02150 bool shouldDelete = true)
02151 {
02152 if(move_arm_warehouse_logger_reader_ != NULL)
02153 {
02154 delete move_arm_warehouse_logger_reader_;
02155 move_arm_warehouse_logger_reader_ = NULL;
02156 }
02157
02158 move_arm_warehouse_logger_reader_ = loggerReader;
02159 }
02160 };
02161
02162 }
02163 #endif