$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * 00035 *********************************************************************/ 00036 00037 /* \author: Matthew Klingensmith */ 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 #include <move_arm_warehouse/move_arm_warehouse_logger_reader.h> 00058 #include <arm_navigation_msgs/convert_messages.h> 00059 #include <visualization_msgs/InteractiveMarker.h> 00060 #include <interactive_markers/interactive_marker_server.h> 00061 #include <interactive_markers/menu_handler.h> 00062 #include <interactive_markers/tools.h> 00063 #include <arm_navigation_msgs/CollisionObject.h> 00064 #include <planning_environment/monitors/kinematic_model_state_monitor.h> 00065 #include <control_msgs/FollowJointTrajectoryAction.h> 00066 #include <gazebo_msgs/SetModelConfiguration.h> 00067 #include <std_srvs/Empty.h> 00068 #include <pr2_mechanism_msgs/ListControllers.h> 00069 #include <pr2_mechanism_msgs/LoadController.h> 00070 #include <pr2_mechanism_msgs/UnloadController.h> 00071 #include <pr2_mechanism_msgs/SwitchController.h> 00072 #include <gazebo_msgs/SetLinkProperties.h> 00073 #include <gazebo_msgs/GetLinkProperties.h> 00074 00075 typedef map<std::string, interactive_markers::MenuHandler::EntryHandle> MenuEntryMap; 00076 typedef map<std::string, MenuEntryMap> MenuMap; 00077 typedef map<std::string, interactive_markers::MenuHandler> MenuHandlerMap; 00078 00083 namespace planning_scene_utils 00084 { 00085 00086 inline static std::string getPlanningSceneNameFromId(const unsigned int id) { 00087 std::stringstream ss; 00088 ss << "Planning Scene " << id; 00089 return ss.str(); 00090 } 00091 00092 inline static unsigned int getPlanningSceneIdFromName(const std::string& name) { 00093 std::stringstream ss(name); 00094 std::string temp; 00095 ss >> temp; 00096 ss >> temp; 00097 unsigned int ret; 00098 ss >> ret; 00099 return ret; 00100 } 00101 00102 inline static std::string getMotionPlanRequestNameFromId(const unsigned int id) { 00103 std::stringstream ss; 00104 ss << "MPR " << id; 00105 return ss.str(); 00106 } 00107 00108 inline static std::string getTrajectoryNameFromId(const unsigned int id) { 00109 std::stringstream ss; 00110 ss << "Trajectory " << id; 00111 return ss.str(); 00112 } 00113 00119 enum PositionType 00120 { 00121 StartPosition, GoalPosition 00122 }; 00123 00131 enum RenderType 00132 { 00133 CollisionMesh, VisualMesh, PaddingMesh 00134 }; 00135 00136 // Must be defined so that subsequent classes can reference. 00137 class PlanningSceneEditor; 00138 00144 class PlanningSceneData 00145 { 00146 protected: 00147 std::string name_; 00148 unsigned int id_; 00149 std::string host_; 00150 ros::Time timestamp_; 00151 arm_navigation_msgs::PlanningScene planning_scene_; 00152 std::vector<std::string> pipeline_stages_; 00153 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes_; 00154 std::set<unsigned int> motion_plan_requests_; 00155 00156 public: 00157 PlanningSceneData(); 00158 PlanningSceneData(unsigned int id, const ros::Time& timestamp, const arm_navigation_msgs::PlanningScene& scene); 00159 00161 inline std::string getHostName() const 00162 { 00163 return host_; 00164 } 00165 00167 inline void setHostName(std::string host) 00168 { 00169 host_ = host; 00170 } 00171 00173 inline std::string getName() const 00174 { 00175 return name_; 00176 } 00177 00178 inline unsigned int getId() const 00179 { 00180 return id_; 00181 } 00182 00184 inline const ros::Time& getTimeStamp() const 00185 { 00186 return timestamp_; 00187 } 00188 00190 inline arm_navigation_msgs::PlanningScene& getPlanningScene() 00191 { 00192 return planning_scene_; 00193 } 00194 00196 inline void setTimeStamp(const ros::Time& time) 00197 { 00198 timestamp_ = time; 00199 planning_scene_.robot_state.joint_state.header.stamp = time; 00200 } 00201 00202 inline void setId(unsigned int id) { 00203 id_ = id; 00204 name_ = getPlanningSceneNameFromId(id); 00205 } 00206 00208 inline void setPlanningScene(const arm_navigation_msgs::PlanningScene& scene) 00209 { 00210 planning_scene_ = scene; 00211 timestamp_ = scene.robot_state.joint_state.header.stamp; 00212 } 00213 00217 inline std::vector<std::string>& getPipelineStages() 00218 { 00219 return pipeline_stages_; 00220 } 00221 00223 inline void setPipelineStages(std::vector<std::string>& stages) 00224 { 00225 pipeline_stages_ = stages; 00226 } 00227 00229 inline std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& getErrorCodes() 00230 { 00231 return error_codes_; 00232 } 00233 00235 inline void setErrorCodes(std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes) 00236 { 00237 error_codes_ = error_codes; 00238 } 00239 00241 inline std::set<unsigned int>& getRequests() 00242 { 00243 return motion_plan_requests_; 00244 } 00245 00248 void getRobotState(planning_models::KinematicState* state); 00249 00250 bool hasMotionPlanRequestId(unsigned int id) const { 00251 return(motion_plan_requests_.find(id) != motion_plan_requests_.end()); 00252 } 00253 00254 void addMotionPlanRequestId(unsigned int id) { 00255 motion_plan_requests_.insert(id); 00256 } 00257 00258 void removeMotionPlanRequestId(unsigned int id) { 00259 motion_plan_requests_.erase(id); 00260 } 00261 00262 unsigned int getNextMotionPlanRequestId() const { 00263 if(motion_plan_requests_.empty()) { 00264 return 0; 00265 } 00266 return (*motion_plan_requests_.rbegin())+1; 00267 } 00268 }; 00269 00275 class MotionPlanRequestData 00276 { 00277 protected: 00278 std::string name_; 00279 unsigned int id_; 00280 std::string source_; 00281 unsigned int planning_scene_id_; 00282 std::string end_effector_link_; 00283 std::string group_name_; 00284 arm_navigation_msgs::MotionPlanRequest motion_plan_request_; 00285 bool is_start_editable_; 00286 bool is_goal_editable_; 00287 bool is_start_visible_; 00288 bool is_goal_visible_; 00289 bool should_refresh_colors_; 00290 bool has_refreshed_colors_; 00291 bool has_good_goal_ik_solution_; 00292 bool has_good_start_ik_solution_; 00293 bool are_collisions_visible_; 00294 bool has_state_changed_; 00295 bool are_joint_controls_visible_; 00296 std_msgs::ColorRGBA start_color_; 00297 std_msgs::ColorRGBA goal_color_; 00298 std::set<unsigned int> trajectories_; 00299 planning_models::KinematicState* start_state_; 00300 planning_models::KinematicState* goal_state_; 00301 btTransform last_good_start_pose_; 00302 btTransform last_good_goal_pose_; 00303 visualization_msgs::MarkerArray collision_markers_; 00304 RenderType render_type_; 00305 unsigned int next_trajectory_id_; 00306 00307 public: 00308 MotionPlanRequestData() 00309 { 00310 start_state_ = NULL; 00311 goal_state_ = NULL; 00312 } 00313 00314 MotionPlanRequestData(const planning_models::KinematicState* robot_state); 00315 MotionPlanRequestData(const unsigned int& id, const std::string& source, const arm_navigation_msgs::MotionPlanRequest& request, 00316 const planning_models::KinematicState* robot_state); 00317 00321 ros::Duration refresh_timer_; 00322 00323 00326 void setStartStateValues(std::map<std::string, double>& joint_values); 00327 00330 void setGoalStateValues(std::map<std::string, double>& joint_values); 00331 00334 void updateStartState(); 00335 00338 void updateGoalState(); 00339 00342 std::vector<std::string> getJointNamesInGoal(); 00343 00348 bool isJointNameInGoal(std::string joint); 00349 00351 inline RenderType getRenderType() const 00352 { 00353 return render_type_; 00354 } 00355 00357 inline void setRenderType(RenderType renderType) 00358 { 00359 render_type_ = renderType; 00360 } 00361 00363 inline bool areJointControlsVisible() const 00364 { 00365 return are_joint_controls_visible_; 00366 } 00367 00371 void setJointControlsVisible(bool visible, PlanningSceneEditor* editor); 00372 00374 inline bool hasStateChanged() const 00375 { 00376 return has_state_changed_; 00377 } 00378 00380 inline void setStateChanged(bool changed) 00381 { 00382 has_state_changed_ = changed; 00383 } 00384 00386 inline visualization_msgs::MarkerArray& getCollisionMarkers() 00387 { 00388 return collision_markers_; 00389 } 00390 00392 inline bool areCollisionsVisible() const 00393 { 00394 return are_collisions_visible_; 00395 } 00396 00398 inline void setCollisionsVisible(bool visible) 00399 { 00400 are_collisions_visible_ = visible; 00401 } 00402 00404 inline void showCollisions() 00405 { 00406 setCollisionsVisible(true); 00407 } 00408 00410 inline void hideCollisions() 00411 { 00412 setCollisionsVisible(false); 00413 } 00414 00416 inline btTransform getLastGoodStartPose() const 00417 { 00418 return last_good_start_pose_; 00419 } 00420 00422 inline btTransform getLastGoodGoalPose() const 00423 { 00424 return last_good_goal_pose_; 00425 } 00426 00428 inline void setLastGoodStartPose(btTransform pose) 00429 { 00430 last_good_start_pose_ = pose; 00431 } 00432 00434 inline void setLastGoodGoalPose(btTransform pose) 00435 { 00436 last_good_goal_pose_ = pose; 00437 } 00438 00440 inline void setStartState(planning_models::KinematicState* state) 00441 { 00442 start_state_ = state; 00443 setStateChanged(true); 00444 } 00445 00447 inline void setGoalState(planning_models::KinematicState* state) 00448 { 00449 goal_state_ = state; 00450 setStateChanged(true); 00451 } 00452 00454 inline void reset() 00455 { 00456 if(start_state_ != NULL) 00457 { 00458 delete start_state_; 00459 start_state_ = NULL; 00460 } 00461 00462 if(goal_state_ != NULL) 00463 { 00464 delete goal_state_; 00465 goal_state_ = NULL; 00466 } 00467 } 00468 00470 inline bool hasGoodIKSolution(const PositionType& type) const 00471 { 00472 if(type == StartPosition) { 00473 return has_good_start_ik_solution_; 00474 } else { 00475 return has_good_goal_ik_solution_; 00476 } 00477 } 00478 00480 inline void setHasGoodIKSolution(const bool& solution, const PositionType& type) 00481 { 00482 if(type == StartPosition) { 00483 has_good_start_ik_solution_ = solution; 00484 } else { 00485 has_good_goal_ik_solution_ = solution; 00486 } 00487 } 00488 00490 inline planning_models::KinematicState* getStartState() 00491 { 00492 return start_state_; 00493 } 00494 00496 inline planning_models::KinematicState* getGoalState() 00497 { 00498 return goal_state_; 00499 } 00500 00503 inline std::string getGroupName() const 00504 { 00505 return group_name_; 00506 } 00507 00509 inline std::string getEndEffectorLink() const 00510 { 00511 return end_effector_link_; 00512 } 00513 00516 inline void setGroupName(const std::string& name) 00517 { 00518 group_name_ = name; 00519 } 00520 00522 inline void setEndEffectorLink(const std::string& name) 00523 { 00524 end_effector_link_ = name; 00525 } 00526 00528 inline const std::string& getName() const 00529 { 00530 return name_; 00531 } 00532 00533 unsigned int getId() const { 00534 return id_; 00535 } 00536 00538 inline void setId(const unsigned int id) 00539 { 00540 id_ = id; 00541 name_ = getMotionPlanRequestNameFromId(id_); 00542 } 00543 00545 inline bool shouldRefreshColors() const 00546 { 00547 return should_refresh_colors_; 00548 } 00549 00551 inline bool hasRefreshedColors() const 00552 { 00553 return has_refreshed_colors_; 00554 } 00555 00557 inline void setHasRefreshedColors(bool refresh) 00558 { 00559 has_refreshed_colors_ = refresh; 00560 00561 if(refresh) 00562 { 00563 should_refresh_colors_ = false; 00564 } 00565 } 00566 00569 inline void refreshColors() 00570 { 00571 should_refresh_colors_ = true; 00572 has_refreshed_colors_ = false; 00573 refresh_timer_ = ros::Duration(0.0); 00574 } 00575 00578 inline bool isStartVisible() const 00579 { 00580 return is_start_visible_; 00581 } 00582 00585 inline bool isEndVisible() const 00586 { 00587 return is_goal_visible_; 00588 } 00589 00591 inline void setStartVisible(bool visible) 00592 { 00593 is_start_visible_ = visible; 00594 } 00595 00597 inline void setEndVisible(bool visible) 00598 { 00599 is_goal_visible_ = visible; 00600 } 00601 00603 inline void show() 00604 { 00605 setStartVisible(true); 00606 setEndVisible(true); 00607 } 00608 00610 inline void hide() 00611 { 00612 setStartVisible(false); 00613 setEndVisible(false); 00614 } 00615 00617 inline void showStart() 00618 { 00619 setStartVisible(true); 00620 } 00621 00623 inline void showGoal() 00624 { 00625 setEndVisible(true); 00626 } 00627 00629 inline void hideStart() 00630 { 00631 setStartVisible(false); 00632 } 00633 00635 inline void hideGoal() 00636 { 00637 setEndVisible(false); 00638 } 00639 00641 inline const std_msgs::ColorRGBA& getStartColor() const 00642 { 00643 return start_color_; 00644 } 00645 00647 inline const std_msgs::ColorRGBA& getGoalColor() const 00648 { 00649 return goal_color_; 00650 } 00651 00653 inline void setStartColor(std_msgs::ColorRGBA color) 00654 { 00655 start_color_ = color; 00656 } 00657 00659 inline void setGoalColor(std_msgs::ColorRGBA color) 00660 { 00661 goal_color_ = color; 00662 } 00663 00666 inline void setStartEditable(bool editable) 00667 { 00668 is_start_editable_ = editable; 00669 } 00670 00673 inline void setGoalEditable(bool editable) 00674 { 00675 is_goal_editable_ = editable; 00676 } 00677 00680 inline bool isStartEditable() const 00681 { 00682 return is_start_editable_; 00683 } 00684 00687 inline bool isGoalEditable() const 00688 { 00689 return is_goal_editable_; 00690 } 00691 00693 inline void setSource(const std::string& source) 00694 { 00695 source_ = source; 00696 } 00697 00699 inline const std::string& getSource() const 00700 { 00701 return source_; 00702 } 00703 00705 inline arm_navigation_msgs::MotionPlanRequest& getMotionPlanRequest() 00706 { 00707 return motion_plan_request_; 00708 } 00709 00711 inline void setMotionPlanRequest(const arm_navigation_msgs::MotionPlanRequest& request) 00712 { 00713 motion_plan_request_ = request; 00714 updateStartState(); 00715 updateGoalState(); 00716 00717 } 00718 00720 inline void setPlanningSceneId(const unsigned int id) 00721 { 00722 planning_scene_id_ = id; 00723 } 00724 inline unsigned int getPlanningSceneId() const 00725 { 00726 return planning_scene_id_; 00727 } 00728 00730 inline std::string getPlanningSceneName() const 00731 { 00732 return getPlanningSceneNameFromId(planning_scene_id_); 00733 } 00734 00736 inline std::set<unsigned int>& getTrajectories() 00737 { 00738 return trajectories_; 00739 } 00740 00741 unsigned int getNextTrajectoryId() const { 00742 if(trajectories_.empty()) return 0; 00743 return (*trajectories_.rbegin())+1; 00744 } 00745 00746 void addTrajectoryId(unsigned int id) { 00747 trajectories_.insert(id); 00748 } 00749 00750 void removeTrajectoryId(unsigned int id) { 00751 trajectories_.erase(id); 00752 } 00753 00754 bool hasTrajectoryId(unsigned int id) const { 00755 return (trajectories_.find(id) != trajectories_.end()); 00756 } 00757 00759 void updateCollisionMarkers(planning_environment::CollisionModels* cm_, 00760 ros::ServiceClient* distance_state_validity_service_client_); 00761 }; 00762 00763 00769 class TrajectoryData 00770 { 00771 public: 00772 00773 enum MarkerType { 00774 VISUAL, 00775 COLLISION, 00776 PADDED 00777 }; 00778 00779 protected: 00780 std::string name_; 00781 unsigned int id_; 00782 std::string source_; 00783 std::string group_name_; 00784 unsigned int planning_scene_id_; 00785 unsigned int motion_plan_request_Id_; 00786 trajectory_msgs::JointTrajectory trajectory_; 00787 bool is_visible_; 00788 MarkerType marker_type_; 00789 bool is_playing_; 00790 bool collisions_visible_; 00791 bool state_changed_; 00792 std_msgs::ColorRGBA color_; 00793 unsigned int current_trajectory_point_; 00794 unsigned int trajectory_bad_point_; 00795 planning_models::KinematicState* current_state_; 00796 ros::Duration duration_; 00797 bool should_refresh_colors_; 00798 bool has_refreshed_colors_; 00799 visualization_msgs::MarkerArray collision_markers_; 00800 RenderType render_type_; 00801 00802 public: 00803 00805 ros::Duration refresh_timer_; 00806 00808 arm_navigation_msgs::ArmNavigationErrorCodes trajectory_error_code_; 00809 00810 TrajectoryData(); 00811 TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name, 00812 const trajectory_msgs::JointTrajectory& trajectory); 00813 00816 void moveThroughTrajectory(int amount); 00817 00819 void updateCurrentState(); 00820 00822 inline bool hasStateChanged() const 00823 { 00824 return state_changed_; 00825 } 00826 00828 inline void setStateChanged(bool changed) 00829 { 00830 state_changed_ = changed; 00831 } 00832 00834 inline visualization_msgs::MarkerArray& getCollisionMarkers() 00835 { 00836 return collision_markers_; 00837 } 00838 00840 inline bool areCollisionsVisible() const 00841 { 00842 return collisions_visible_; 00843 } 00844 00846 inline void setCollisionsVisible(bool shown) 00847 { 00848 collisions_visible_ = shown; 00849 } 00850 00852 inline void showCollisions() 00853 { 00854 setCollisionsVisible(true); 00855 } 00856 00858 inline void hideCollisions() 00859 { 00860 setCollisionsVisible(false); 00861 } 00862 00864 inline size_t getTrajectorySize() const 00865 { 00866 return trajectory_.points.size(); 00867 } 00868 00870 inline bool shouldRefreshColors() const 00871 { 00872 return should_refresh_colors_; 00873 } 00874 00876 inline bool hasRefreshedColors() const 00877 { 00878 return has_refreshed_colors_; 00879 } 00880 00882 inline void setHasRefreshedColors(bool refresh) 00883 { 00884 has_refreshed_colors_ = refresh; 00885 00886 if(refresh) 00887 { 00888 should_refresh_colors_ = false; 00889 } 00890 } 00891 00894 inline void refreshColors() 00895 { 00896 should_refresh_colors_ = true; 00897 has_refreshed_colors_ = false; 00898 refresh_timer_ = ros::Duration(0.0); 00899 } 00900 00902 inline RenderType getRenderType() const 00903 { 00904 return render_type_; 00905 } 00906 00908 inline void setRenderType(RenderType renderType) 00909 { 00910 render_type_ = renderType; 00911 } 00912 00914 inline void reset() 00915 { 00916 00917 if(current_state_ != NULL) 00918 { 00919 delete current_state_; 00920 current_state_ = NULL; 00921 } 00922 00923 is_playing_ = false; 00924 is_visible_ = false; 00925 current_trajectory_point_ = 0; 00926 state_changed_ = false; 00927 } 00928 00931 inline planning_models::KinematicState* getCurrentState() 00932 { 00933 return current_state_; 00934 } 00935 00937 inline void setCurrentState(planning_models::KinematicState* state) 00938 { 00939 current_state_ = state; 00940 state_changed_ = true; 00941 } 00942 00944 inline void setMotionPlanRequestId(const unsigned int& Id) 00945 { 00946 motion_plan_request_Id_ = Id; 00947 } 00948 00950 inline const unsigned int& getMotionPlanRequestId() const 00951 { 00952 return motion_plan_request_Id_; 00953 } 00954 00955 void setPlanningSceneId(const unsigned int id) { 00956 planning_scene_id_ = id; 00957 } 00958 00959 unsigned int getPlanningScendId() const { 00960 return planning_scene_id_; 00961 } 00962 00964 inline void setCurrentPoint(unsigned int point) 00965 { 00966 current_trajectory_point_ = point; 00967 state_changed_ = true; 00968 } 00969 00971 inline unsigned int getCurrentPoint() const 00972 { 00973 return current_trajectory_point_; 00974 } 00975 00977 inline unsigned int getBadPoint() const 00978 { 00979 return trajectory_bad_point_; 00980 } 00981 00983 inline void setGroupname(const std::string& group_name) 00984 { 00985 group_name_ = group_name; 00986 } 00987 00989 inline bool isPlaying() const 00990 { 00991 return is_playing_; 00992 } 00993 00995 inline void setPlaying(bool playing) 00996 { 00997 is_playing_ = playing; 00998 } 00999 01001 inline void play() 01002 { 01003 is_playing_ = true; 01004 } 01005 01007 inline void stop() 01008 { 01009 is_playing_ = false; 01010 } 01011 01013 inline bool isVisible() const 01014 { 01015 return is_visible_; 01016 } 01017 01019 inline void setVisible(bool visible) 01020 { 01021 is_visible_ = visible; 01022 } 01023 01024 inline MarkerType getMarkerType() const 01025 { 01026 return marker_type_; 01027 } 01028 01030 inline void setMarkerType(MarkerType mt) 01031 { 01032 marker_type_ = mt; 01033 } 01034 01036 inline void show() 01037 { 01038 setVisible(true); 01039 } 01040 01042 inline void hide() 01043 { 01044 setVisible(false); 01045 } 01046 01049 inline const ros::Duration& getDuration() const 01050 { 01051 return duration_; 01052 } 01053 01055 inline void setDuration(const ros::Duration& duration) 01056 { 01057 duration_ = duration; 01058 } 01059 01061 inline const std_msgs::ColorRGBA& getColor() const 01062 { 01063 return color_; 01064 } 01065 01067 inline void setColor(const std_msgs::ColorRGBA& color) 01068 { 01069 color_ = color; 01070 } 01071 01073 inline std::string getSource() 01074 { 01075 return source_; 01076 } 01077 01079 inline trajectory_msgs::JointTrajectory& getTrajectory() 01080 { 01081 return trajectory_; 01082 } 01083 01085 inline void setSource(const std::string& source) 01086 { 01087 source_ = source; 01088 } 01089 01091 inline void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory) 01092 { 01093 trajectory_ = trajectory; 01094 } 01095 01097 inline unsigned int getId() const 01098 { 01099 return id_; 01100 } 01101 01103 inline void setId(const unsigned int& id) 01104 { 01105 id_ = id; 01106 name_ = getTrajectoryNameFromId(id_); 01107 } 01108 01109 const std::string& getName() const { 01110 return name_; 01111 } 01112 01114 inline const std::string& getGroupName() const 01115 { 01116 return group_name_; 01117 } 01118 01120 inline void setBadPoint(unsigned int point) 01121 { 01122 trajectory_bad_point_ = point; 01123 } 01124 01126 inline void setGroupName(std::string name) 01127 { 01128 group_name_ = name; 01129 } 01130 01133 void updateCollisionMarkers(planning_environment::CollisionModels* cm_, MotionPlanRequestData& motionPlanRequest, 01134 ros::ServiceClient* distance_state_validity_service_client_); 01135 }; 01136 01142 struct PlanningSceneParameters 01143 { 01144 std::string left_ik_name_; 01145 std::string right_ik_name_; 01146 std::string non_coll_left_ik_name_; 01147 std::string non_coll_right_ik_name_; 01148 std::string left_interpolate_service_name_; 01149 std::string right_interpolate_service_name_; 01150 std::string planner_service_name_; 01151 std::string proximity_space_service_name_; 01152 std::string proximity_space_validity_name_; 01153 std::string set_planning_scene_diff_name_; 01154 std::string trajectory_filter_service_name_; 01155 std::string proximity_space_planner_name_; 01156 std::string vis_topic_name_; 01157 std::string right_ik_link_; 01158 std::string left_ik_link_; 01159 std::string left_redundancy_; 01160 std::string right_redundancy_; 01161 std::string right_arm_group_; 01162 std::string left_arm_group_; 01163 std::string execute_left_trajectory_; 01164 std::string execute_right_trajectory_; 01165 std::string list_controllers_service_; 01166 std::string unload_controllers_service_; 01167 std::string load_controllers_service_; 01168 std::string switch_controllers_service_; 01169 std::string gazebo_model_name_; 01170 std::string robot_description_param_; 01171 bool use_robot_data_; 01172 bool sync_robot_state_with_gazebo_; 01173 }; 01174 01179 class PlanningSceneEditor 01180 { 01181 public: 01186 enum GeneratedShape 01187 { 01188 Box, Cylinder, Sphere 01189 }; 01190 protected: 01191 01200 enum MonitorStatus 01201 { 01202 idle, Executing, Done 01203 }; 01204 01213 struct StateRegistry 01214 { 01215 planning_models::KinematicState* state; 01216 std::string source; 01217 }; 01218 01224 struct SelectableObject 01225 { 01226 SelectableObject() { 01227 attach_ = false; 01228 detach_ = false; 01229 attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE; 01230 collision_object_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE; 01231 } 01232 01233 bool attach_; 01234 bool detach_; 01235 arm_navigation_msgs::AttachedCollisionObject attached_collision_object_; 01236 arm_navigation_msgs::CollisionObject collision_object_; 01237 visualization_msgs::InteractiveMarker selection_marker_; 01238 visualization_msgs::InteractiveMarker control_marker_; 01239 std_msgs::ColorRGBA color_; 01240 01241 std::string id_; 01242 }; 01243 01249 struct IKController 01250 { 01251 unsigned int motion_plan_id_; 01252 visualization_msgs::InteractiveMarker start_controller_; 01253 visualization_msgs::InteractiveMarker end_controller_; 01254 }; 01255 01260 virtual void updateState() 01261 { 01262 } 01263 01268 virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0; 01269 01274 virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0; 01275 01276 virtual void attachObjectCallback(const std::string& name) = 0; 01277 // virtual void detachObjectCallback(arm_navigation_msgs::CollisionObject& object) = 0; 01278 01279 void changeToAttached(const std::string& name); 01280 01281 boost::recursive_mutex lock_scene_; 01282 arm_navigation_msgs::ArmNavigationErrorCodes last_collision_set_error_code_; 01283 move_arm_warehouse::MoveArmWarehouseLoggerReader* move_arm_warehouse_logger_reader_; 01284 planning_environment::CollisionModels* cm_; 01285 planning_models::KinematicState* robot_state_; 01286 PlanningSceneParameters params_; 01287 ros::NodeHandle nh_; 01288 ros::Publisher clock_publisher_; 01289 ros::Publisher joint_state_publisher_; 01290 ros::Publisher vis_marker_array_publisher_; 01291 ros::Publisher vis_marker_publisher_; 01292 ros::Subscriber joint_state_subscriber_; 01293 ros::ServiceClient collision_proximity_planner_client_; 01294 ros::ServiceClient distance_aware_service_client_; 01295 ros::ServiceClient distance_state_validity_service_client_; 01296 ros::ServiceClient set_planning_scene_diff_client_; 01297 ros::ServiceClient left_ik_service_client_; 01298 ros::ServiceClient left_interpolate_service_client_; 01299 ros::ServiceClient non_coll_left_ik_service_client_; 01300 ros::ServiceClient non_coll_right_ik_service_client_; 01301 ros::ServiceClient planning_service_client_; 01302 ros::ServiceClient right_ik_service_client_; 01303 ros::ServiceClient right_interpolate_service_client_; 01304 ros::ServiceClient trajectory_filter_service_client_; 01305 ros::ServiceClient gazebo_joint_state_client_; 01306 ros::ServiceClient list_controllers_client_; 01307 ros::ServiceClient load_controllers_client_; 01308 ros::ServiceClient unload_controllers_client_; 01309 ros::ServiceClient switch_controllers_client_; 01310 ros::ServiceClient pause_gazebo_client_; 01311 ros::ServiceClient unpause_gazebo_client_; 01312 ros::ServiceClient set_link_properties_client_; 01313 ros::ServiceClient get_link_properties_client_; 01314 01315 std::map<std::string, double> robot_state_joint_values_; 01316 std::vector<ros::Time> last_creation_time_query_; 01317 tf::TransformBroadcaster transform_broadcaster_; 01318 tf::TransformListener transform_listener_; 01319 std::map<std::string, actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>*> arm_controller_map_; 01320 unsigned int max_trajectory_id_; 01321 unsigned int max_collision_object_id_; 01322 01323 bool warehouse_data_loaded_once_; 01324 bool use_interpolated_planner_; 01325 01326 trajectory_msgs::JointTrajectory logged_trajectory_; 01327 ros::Time logged_trajectory_start_time_; 01328 01329 bool send_collision_markers_; 01330 std::string collision_marker_state_; 01331 visualization_msgs::MarkerArray collision_markers_; 01332 planning_models::KinematicState* paused_collision_state_; 01333 std_msgs::ColorRGBA point_color_; 01334 std::vector<geometry_msgs::TransformStamped> robot_transforms_; 01335 01336 interactive_markers::MenuHandler::FeedbackCallback attached_collision_object_feedback_ptr_; 01337 interactive_markers::MenuHandler::FeedbackCallback collision_object_selection_feedback_ptr_; 01338 interactive_markers::MenuHandler::FeedbackCallback collision_object_movement_feedback_ptr_; 01339 interactive_markers::MenuHandler::FeedbackCallback ik_control_feedback_ptr_; 01340 interactive_markers::MenuHandler::FeedbackCallback joint_control_feedback_ptr_; 01341 01342 interactive_markers::InteractiveMarkerServer* interactive_marker_server_; 01343 01344 std::map<std::string, SelectableObject>* selectable_objects_; 01345 std::map<std::string, IKController>* ik_controllers_; 01346 01347 std::string current_planning_scene_name_; 01348 std::string selected_motion_plan_name_; 01349 std::string selected_trajectory_name_; 01350 std::string logged_group_name_; 01351 std::string logged_motion_plan_request_; 01352 std::map<string, MenuEntryMap> menu_entry_maps_; 01353 MenuHandlerMap menu_handler_map_; 01354 01355 std::map<string, ros::ServiceClient*>* collision_aware_ik_services_; 01356 std::map<string, ros::ServiceClient*>* non_collision_aware_ik_services_; 01357 std::map<string, ros::ServiceClient*>* interpolated_ik_services_; 01358 std::map<string, arm_navigation_msgs::ArmNavigationErrorCodes> error_map_; 01359 std::vector<StateRegistry> states_; 01360 01361 interactive_markers::MenuHandler::EntryHandle last_resize_handle_; 01362 01363 std_msgs::ColorRGBA last_collision_object_color_; 01364 std_msgs::ColorRGBA last_mesh_object_color_; 01365 01366 MonitorStatus monitor_status_; 01367 01368 ros::Time last_marker_start_time_; 01369 ros::Duration marker_dt_; 01370 01371 void attachCollisionObject(const std::string& name, 01372 const std::string& link_name, 01373 const std::vector<std::string>& touch_links); 01374 01378 void createSelectableMarkerFromCollisionObject(arm_navigation_msgs::CollisionObject& object, std::string name, 01379 std::string description, std_msgs::ColorRGBA color, bool insert_selectable=true); 01380 01381 01382 01383 public: 01384 static geometry_msgs::Pose toGeometryPose(btTransform transform) 01385 { 01386 geometry_msgs::Pose toReturn; 01387 toReturn.position.x = transform.getOrigin().x(); 01388 toReturn.position.y = transform.getOrigin().y(); 01389 toReturn.position.z = transform.getOrigin().z(); 01390 toReturn.orientation.x = transform.getRotation().x(); 01391 toReturn.orientation.y = transform.getRotation().y(); 01392 toReturn.orientation.z = transform.getRotation().z(); 01393 toReturn.orientation.w = transform.getRotation().w(); 01394 return toReturn; 01395 } 01396 01397 static btTransform toBulletTransform(geometry_msgs::Pose pose) 01398 { 01399 btQuaternion quat = 01400 btQuaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); 01401 btVector3 vec = btVector3(pose.position.x, pose.position.y, pose.position.z); 01402 return btTransform(quat, vec); 01403 } 01404 01406 std::map<std::string, PlanningSceneData> planning_scene_map_; 01407 01409 std::map<std::string, std::map<std::string, TrajectoryData> > trajectory_map_; 01410 01412 std::map<std::string, MotionPlanRequestData> motion_plan_map_; 01413 01415 std::map<std::string, bool> joint_clicked_map_; 01416 01418 std::map<std::string, btTransform> joint_prev_transform_map_; 01419 01420 PlanningSceneEditor(); 01421 PlanningSceneEditor(PlanningSceneParameters& params); 01422 ~PlanningSceneEditor(); 01423 01431 bool filterTrajectory(MotionPlanRequestData& requestData, TrajectoryData& trajectory, unsigned int& filter_id); 01432 01441 bool getAllAssociatedMotionPlanRequests(const unsigned int id, 01442 std::vector<unsigned int>& ids, 01443 std::vector<std::string>& stages, 01444 std::vector<arm_navigation_msgs::MotionPlanRequest>& requests); 01445 01452 bool getAllAssociatedPausedStates(const unsigned int id, 01453 std::vector<ros::Time>& paused_times); 01454 01461 bool getAllAssociatedTrajectorySources(const unsigned int planning_id, 01462 const unsigned int mpr_id, 01463 std::vector<unsigned int>& trajectory_ids, 01464 std::vector<std::string>& trajectory_sources); 01465 01471 bool getAllPlanningSceneTimes(std::vector<ros::Time>& planning_scene_times, 01472 vector<unsigned int>& planning_scene_ids); 01473 01474 01484 bool getMotionPlanRequest(const ros::Time& time, const std::string& stage, 01485 arm_navigation_msgs::MotionPlanRequest& mpr, std::string& id, 01486 std::string& planning_scene_id); 01487 01495 bool getPausedState(const unsigned int id, 01496 const ros::Time& paused_time, 01497 head_monitor_msgs::HeadMonitorFeedback& paused_state); 01498 01507 bool getPlanningSceneOutcomes(const unsigned int id, 01508 std::vector<std::string>& pipeline_stages, 01509 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes, 01510 std::map<std::string, arm_navigation_msgs::ArmNavigationErrorCodes>& error_map); 01511 01512 01519 bool loadPlanningScene(const ros::Time& time, 01520 const unsigned int id); 01521 01532 bool planToKinematicState(const planning_models::KinematicState& state, const std::string& group_name, 01533 const std::string& end_effector_name, const bool constrain, unsigned int& trajectoryid_Out, 01534 unsigned int& planning_scene_id); 01535 01542 bool planToRequest(MotionPlanRequestData& data, unsigned int& trajectoryid_Out); 01543 01550 bool planToRequest(const std::string& requestid, unsigned int& trajectoryid_Out); 01551 01558 bool playTrajectory(MotionPlanRequestData& requestData, TrajectoryData& data); 01559 01565 bool sendPlanningScene(PlanningSceneData& data); 01566 01577 bool solveIKForEndEffectorPose(MotionPlanRequestData& mpr, PositionType type, bool coll_aware = true, 01578 bool constrain_pitch_and_roll = false, double change_redundancy = 0.0); 01579 01580 01588 interactive_markers::MenuHandler::EntryHandle registerMenuEntry(std::string menu, std::string entryName, 01589 interactive_markers::MenuHandler::FeedbackCallback& callback); 01590 01591 01592 01601 interactive_markers::MenuHandler::EntryHandle registerSubMenuEntry(std::string menu, std::string name, 01602 std::string subMenu, 01603 interactive_markers::MenuHandler::FeedbackCallback& callback); 01604 01609 std::string createNewPlanningScene(); 01610 01617 virtual void onPlanningSceneLoaded(int scene, int numScenes) 01618 { 01619 } 01620 01625 void collisionObjectMovementCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 01626 01631 void collisionObjectSelectionCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 01632 01633 void attachedCollisionObjectInteractiveCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 01634 01645 std::string createCollisionObject(const std::string& name, geometry_msgs::Pose pose, GeneratedShape shape, float scaleX, float scaleY, 01646 float scaleZ, std_msgs::ColorRGBA color); 01647 01648 std::string createMeshObject(const std::string& name, 01649 geometry_msgs::Pose pose, 01650 const std::string& filename, 01651 const btVector3& scale, 01652 std_msgs::ColorRGBA color); 01659 void createIKController(MotionPlanRequestData& data, PositionType type, bool rePose = true); 01660 01667 void createIkControllersFromMotionPlanRequest(MotionPlanRequestData& data, bool rePose = true); 01668 01674 void createJointMarkers(MotionPlanRequestData& data, planning_scene_utils::PositionType position); 01675 01676 01688 void createMotionPlanRequest(const planning_models::KinematicState& start_state, 01689 const planning_models::KinematicState& end_state, 01690 const std::string& group_name, 01691 const std::string& end_effector_name, 01692 const bool constrain, 01693 const unsigned int& planning_scene_name, 01694 const bool fromRobotState, 01695 unsigned int& motionPlan_id_Out); 01696 01697 01705 void initMotionPlanRequestData(const unsigned int& planning_scene_id, 01706 const std::vector<unsigned int>& ids, 01707 const std::vector<std::string>& stages, 01708 const std::vector<arm_navigation_msgs::MotionPlanRequest>& requests); 01709 01710 01716 void deleteJointMarkers(MotionPlanRequestData& data, PositionType type); 01717 01722 void deleteKinematicStates(); 01723 01728 void deleteMotionPlanRequest(const unsigned int& id, 01729 std::vector<unsigned int>& erased_trajectories); 01730 01735 void deleteTrajectory(unsigned int mpr_id, unsigned int traj_id); 01736 01737 01745 void determinePitchRollConstraintsGivenState(const planning_models::KinematicState& state, 01746 const std::string& end_effector_link, 01747 arm_navigation_msgs::OrientationConstraint& goal_constraint, 01748 arm_navigation_msgs::OrientationConstraint& path_constraint); 01749 01754 void executeTrajectory(const std::string& mpr_name, const std::string& traj_name); 01755 01760 void executeTrajectory(TrajectoryData& data); 01761 01768 void getAllRobotStampedTransforms(const planning_models::KinematicState& state, 01769 vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp); 01770 01775 void getMotionPlanningMarkers(visualization_msgs::MarkerArray& arr); 01776 01781 void getTrajectoryMarkers(visualization_msgs::MarkerArray& arr); 01782 01787 void IKControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 01788 01793 void JointControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 01794 01800 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state, 01801 const control_msgs::FollowJointTrajectoryResultConstPtr& result); 01802 01807 void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state); 01808 01812 void loadAllWarehouseData(); 01813 01823 void makeInteractive1DOFRotationMarker(btTransform transform, btVector3 axis, string name, string description, 01824 float scale = 1.0f, float angle = 0.0f); 01825 01835 void makeInteractive1DOFTranslationMarker(btTransform transform, btVector3 axis, string name, string description, 01836 float scale = 1.0f, float value = 0.0f); 01837 01841 void printTrajectoryPoint(const std::vector<std::string>& joint_names, const std::vector<double>& joint_values); 01842 01848 void randomlyPerturb(MotionPlanRequestData& mpr, PositionType type); 01849 01854 void savePlanningScene(PlanningSceneData& data, bool copy = false); 01855 01859 void sendMarkers(); 01860 01864 void sendTransformsAndClock(); 01865 01866 void makeSelectableAttachedObjectFromPlanningScene(const arm_navigation_msgs::PlanningScene& scene, 01867 arm_navigation_msgs::AttachedCollisionObject& att); 01868 01875 void setCurrentPlanningScene(std::string id, bool loadRequests = true, bool loadTrajectories = true); 01876 01883 void setIKControlsVisible(std::string id, PositionType type, bool visible); 01884 01893 void setJointState(MotionPlanRequestData& data, PositionType position, std::string& jointName, btTransform value); 01894 01899 void updateJointStates(); 01900 01901 bool hasTrajectory(const std::string& mpr_name, 01902 const std::string& traj_name) const; 01903 01908 inline std::string generateNewCollisionObjectId() 01909 { 01910 std::stringstream stream; 01911 stream << "collision_object_"; 01912 max_collision_object_id_++; 01913 stream << max_collision_object_id_; 01914 return stream.str(); 01915 } 01916 01921 inline unsigned int generateNewPlanningSceneId() 01922 { 01923 return move_arm_warehouse_logger_reader_->determineNextPlanningSceneId(); 01924 } 01925 01926 01931 void deleteCollisionObject(std::string& name); 01932 01933 01934 inline void lockScene() 01935 { 01936 lock_scene_.lock(); 01937 } 01938 01939 inline void unlockScene() 01940 { 01941 lock_scene_.unlock(); 01942 } 01943 01944 inline planning_environment::CollisionModels* getCollisionModel() 01945 { 01946 return cm_; 01947 } 01948 01949 inline void setCollisionModel(planning_environment::CollisionModels* model, bool shouldDelete = false) 01950 { 01951 if(shouldDelete && cm_ != NULL) 01952 { 01953 delete cm_; 01954 cm_ = model; 01955 } 01956 cm_ = model; 01957 } 01958 01959 inline planning_models::KinematicState* getRobotState() 01960 { 01961 return robot_state_; 01962 } 01963 01964 inline void setRobotState(planning_models::KinematicState* robot_state, bool shouldDelete = true) 01965 { 01966 if(shouldDelete && robot_state_ != NULL) 01967 { 01968 delete robot_state_; 01969 robot_state_ = NULL; 01970 } 01971 01972 robot_state_ = robot_state; 01973 } 01974 01975 inline move_arm_warehouse::MoveArmWarehouseLoggerReader* getLoggerReader() 01976 { 01977 return move_arm_warehouse_logger_reader_; 01978 } 01979 01980 inline void setLoggerReader(move_arm_warehouse::MoveArmWarehouseLoggerReader* loggerReader, 01981 bool shouldDelete = true) 01982 { 01983 if(move_arm_warehouse_logger_reader_ != NULL) 01984 { 01985 delete move_arm_warehouse_logger_reader_; 01986 move_arm_warehouse_logger_reader_ = NULL; 01987 } 01988 01989 move_arm_warehouse_logger_reader_ = loggerReader; 01990 } 01991 }; 01992 01993 } 01994 #endif