├── launch ├── actions_nao.launch ├── actions_romeo.launch ├── actions_pepper.launch ├── actions_pepper_sim.launch └── actions_romeo_sim.launch ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── romeo_moveit_actions │ ├── toolsForObject.hpp │ ├── toolsForAction.hpp │ ├── postures.hpp │ ├── objprocessing.hpp │ ├── metablock.hpp │ ├── evaluation.hpp │ ├── simplepickplace.hpp │ ├── tools.hpp │ └── action.hpp ├── package.xml ├── src ├── toolsForObject.cpp ├── main.cpp ├── postures.cpp ├── metablock.cpp ├── objprocessing.cpp ├── evaluation.cpp ├── simplepickplace.cpp └── action.cpp └── README.rst /launch/actions_nao.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /launch/actions_romeo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /launch/actions_pepper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /launch/actions_pepper_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package romeo_moveit_actions 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.8 (2016-02-12) 6 | ------------------ 7 | * fixing the doc and description 8 | * Contributors: nlyubova 9 | 10 | 0.0.7 (2016-02-03) 11 | ------------------ 12 | * Merge pull request `#1 `_ from IanTheEngineer/remove_shape_tools 13 | Convert deprecated shape_tools dependency 14 | * Convert deprecated shape_tools dependency 15 | shape_tools functionality was merged into geometric_shapes: 16 | https://github.com/ros-planning/geometric_shapes/pull/32 17 | and removed from moveit_core 18 | https://github.com/ros-planning/moveit_core/pull/242 19 | which caused this issue. 20 | This commit updates the pick and place tutorial and adds 21 | geometric_shapes to the package.xml and CMakeLists.txt to 22 | prevent the ROS buildfarm from failing to build this package. 23 | * Contributors: Ian McMahon, Natalia Lyubova 24 | 25 | 0.0.6 (2015-12-30) 26 | ------------------ 27 | * fixing visualization of geometric shapes and cleaning 28 | * renaming the package to romeo_moveit_actions 29 | 30 | 0.0.5 (2015-12-14) 31 | ------------------ 32 | * adding changelogs 33 | 34 | 0.0.4 (2015-12-10) 35 | ------------------ 36 | * adding changelogs 37 | * fixing with changes in moveit visual tools 38 | 39 | 0.0.3 (2015-11-25 17:46) 40 | ------------------------ 41 | * 0.0.2 42 | * adding CHANGELOG.rst 43 | 44 | 0.0.2 (2015-11-25 17:09) 45 | ------------------------ 46 | * adding CHANGELOG.rst 47 | * adding a planning group (arm) name as an argument 48 | * taking into account object height when grasping 49 | * initial commit, new version of romeo_action package 50 | -------------------------------------------------------------------------------- /launch/actions_romeo_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | SET(PROJECTNAME romeo_moveit_actions) 3 | project(${PROJECTNAME}) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | moveit_ros_planning 7 | moveit_ros_planning_interface 8 | moveit_visual_tools 9 | moveit_simple_grasps 10 | std_msgs 11 | geometry_msgs 12 | geometric_shapes 13 | roscpp 14 | cmake_modules 15 | object_recognition_msgs 16 | actionlib 17 | ) 18 | 19 | find_package(Boost REQUIRED system thread date_time system filesystem) 20 | find_package(Boost QUIET COMPONENTS program_options) 21 | 22 | catkin_package( 23 | INCLUDE_DIRS include 24 | CATKIN_DEPENDS roscpp geometry_msgs geometric_shapes std_msgs moveit_ros_planning moveit_ros_planning_interface moveit_visual_tools moveit_simple_grasps object_recognition_msgs 25 | ) 26 | 27 | ########### 28 | ## Build ## 29 | ########### 30 | 31 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 32 | 33 | link_directories(${Boost_LIBRARY_DIRS}) 34 | 35 | set(SRC 36 | src/main.cpp 37 | src/simplepickplace.cpp 38 | src/metablock.cpp 39 | src/action.cpp 40 | src/postures.cpp 41 | src/objprocessing.cpp 42 | src/evaluation.cpp 43 | src/toolsForObject.cpp 44 | include/romeo_moveit_actions/simplepickplace.hpp 45 | include/romeo_moveit_actions/metablock.hpp 46 | include/romeo_moveit_actions/action.hpp 47 | include/romeo_moveit_actions/postures.hpp 48 | include/romeo_moveit_actions/tools.hpp 49 | include/romeo_moveit_actions/toolsForAction.hpp 50 | include/romeo_moveit_actions/toolsForObject.hpp 51 | include/romeo_moveit_actions/evaluation.hpp 52 | include/romeo_moveit_actions/objprocessing.hpp 53 | ) 54 | 55 | # Main executable 56 | add_executable(${PROJECTNAME} ${SRC}) 57 | 58 | target_link_libraries(${PROJECTNAME} 59 | ${catkin_LIBRARIES} 60 | ${Boost_LIBRARIES}) 61 | 62 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/toolsForObject.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef TOOLSFOROBJECT_H 18 | #define TOOLSFOROBJECT_H 19 | 20 | #include 21 | 22 | #include "romeo_moveit_actions/metablock.hpp" 23 | 24 | namespace moveit_simple_actions 25 | { 26 | //! @brief set the pose 27 | void setPose(geometry_msgs::Pose *pose, 28 | const double &x, 29 | const double &y, 30 | const double &z, 31 | const double &ox, 32 | const double &oy, 33 | const double &oz, 34 | const double &ow); 35 | 36 | //! @brief set the pose 37 | void setPose(geometry_msgs::Pose *pose, 38 | const double &x, 39 | const double &y, 40 | const double &z); 41 | 42 | //! @brief find the object by name 43 | int findObj(std::vector *blocks, 44 | const std::string name); 45 | 46 | //! @brief get a list of objects 47 | std::vector getObjectsList(const std::vector &blocks); 48 | 49 | //! @brief get a list of old objects 50 | std::vector getObjectsOldList(std::vector *objects); 51 | 52 | //! @brief swap poses 53 | void swapPoses(geometry_msgs::Pose *pose1, 54 | geometry_msgs::Pose *pose2); 55 | } 56 | 57 | #endif // TOOLSFOROBJECT_H 58 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | romeo_moveit_actions 3 | 0.0.8 4 | Interaction with objects using MoveIt! configured for Romeo, NAO, and Pepper robots 5 | 6 | Natalia Lyubova 7 | 8 | BSD 9 | 10 | https://github.com/ros-aldebaran/romeo_moveit_actions/issues 11 | https://github.com/ros-aldebaran/romeo_moveit_actions 12 | 13 | catkin 14 | 15 | moveit_core 16 | moveit_ros_planning 17 | moveit_ros_planning_interface 18 | moveit_simple_grasps 19 | moveit_visual_tools 20 | geometry_msgs 21 | geometric_shapes 22 | roscpp 23 | cmake_modules 24 | std_msgs 25 | object_recognition_msgs 26 | actionlib 27 | 28 | moveit_core 29 | moveit_ros 30 | moveit_ros_visualization 31 | moveit_ros_planning 32 | moveit_ros_planning_interface 33 | moveit_planners 34 | moveit_planners_ompl 35 | moveit_plugins 36 | moveit_simple_controller_manager 37 | moveit_simple_grasps 38 | moveit_visual_tools 39 | geometry_msgs 40 | geometric_shapes 41 | roscpp 42 | std_msgs 43 | object_recognition_msgs 44 | actionlib 45 | 46 | 47 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/toolsForAction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef TOOLSFORACTION_HPP 18 | #define TOOLSFORACTION_HPP 19 | 20 | // ROS 21 | #include 22 | 23 | namespace moveit_simple_actions 24 | { 25 | 26 | //! @brief wait for action 27 | template 28 | void waitForAction(const T &action, 29 | const ros::NodeHandle &node_handle, 30 | const ros::Duration &wait_for_server, 31 | const std::string &name) 32 | { 33 | ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str()); 34 | 35 | // in case ROS time is published, wait for the time data to arrive 36 | ros::Time start_time = ros::Time::now(); 37 | while (start_time == ros::Time::now()) 38 | { 39 | ros::WallDuration(0.01).sleep(); 40 | ros::spinOnce(); 41 | } 42 | 43 | // wait for the server (and spin as needed) 44 | if (wait_for_server == ros::Duration(0, 0)) 45 | { 46 | while (node_handle.ok() && !action->isServerConnected()) 47 | { 48 | ros::WallDuration(0.02).sleep(); 49 | ros::spinOnce(); 50 | } 51 | } 52 | else 53 | { 54 | ros::Time final_time = ros::Time::now() + wait_for_server; 55 | while (node_handle.ok() && !action->isServerConnected() 56 | && final_time > ros::Time::now()) 57 | { 58 | ros::WallDuration(0.02).sleep(); 59 | ros::spinOnce(); 60 | } 61 | } 62 | 63 | if (!action->isServerConnected()) 64 | throw std::runtime_error("Unable to connect to the action server within allotted time (2)"); 65 | else 66 | ROS_DEBUG("Connected to '%s'", name.c_str()); 67 | } 68 | } 69 | 70 | #endif // TOOLSFORACTION_HPP 71 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/postures.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef POSTURES_HPP 18 | #define POSTURES_HPP 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace moveit_simple_actions 25 | { 26 | //! @brief Class for the robot's posture processing. 27 | class Posture 28 | { 29 | public: 30 | //! @brief constructor 31 | Posture(const std::string robot_name, 32 | const std::string eef_name, 33 | const std::string group_name); 34 | 35 | //! @brief initialize the hand pose 36 | void initHandPose(const double &value, const int &pose); 37 | 38 | //! @brief define the head down pose 39 | bool poseHeadDown(); 40 | 41 | //! @brief define the head zero pose 42 | bool poseHeadZero(); 43 | 44 | //! @brief define the hand open pose 45 | bool poseHandOpen(const std::string &end_eff); 46 | 47 | //! @brief define the hand close pose 48 | bool poseHandClose(const std::string &end_eff); 49 | 50 | //! @brief define the hand pose 51 | bool poseHand(const std::string &end_eff, 52 | const std::string &group, 53 | const int &pose_id); 54 | 55 | private: 56 | //! @brief go to the pose 57 | bool goToPose(const std::string group_name, 58 | std::vector *pose); 59 | 60 | /** pre-defined head down pose */ 61 | std::vector pose_head_down_; 62 | 63 | /** pre-defined hand zero pose */ 64 | std::vector pose_head_zero_; 65 | 66 | /** pre-defined hand pose */ 67 | std::vector< std::vector > pose_hand_; 68 | 69 | /** pre-defined arm pose */ 70 | std::vector< std::vector > pose_arm_; 71 | }; 72 | } 73 | #endif // POSTURES_HPP 74 | -------------------------------------------------------------------------------- /src/toolsForObject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "romeo_moveit_actions/toolsForObject.hpp" 18 | 19 | namespace moveit_simple_actions 20 | { 21 | 22 | void setPose(geometry_msgs::Pose *pose, 23 | const double &x, 24 | const double &y, 25 | const double &z, 26 | const double &ox, 27 | const double &oy, 28 | const double &oz, 29 | const double &ow) 30 | { 31 | pose->position.x = x; 32 | pose->position.y = y; 33 | pose->position.z = z; 34 | pose->orientation.x = ox; 35 | pose->orientation.y = oy; 36 | pose->orientation.z = oz; 37 | pose->orientation.w = ow; 38 | } 39 | 40 | void setPose(geometry_msgs::Pose *pose, 41 | const double &x, 42 | const double &y, 43 | const double &z) 44 | { 45 | pose->position.x = x; 46 | pose->position.y = y; 47 | pose->position.z = z; 48 | } 49 | 50 | int findObj(std::vector *blocks, 51 | const std::string name) 52 | { 53 | int idx = -1; 54 | for (int i=0; isize(); ++i) 55 | if (blocks->at(i).name_ == name){ 56 | idx = i; 57 | return idx; 58 | } 59 | return idx; 60 | } 61 | 62 | std::vector getObjectsList(const std::vector &blocks) 63 | { 64 | std::vector res; 65 | res.resize(blocks.size()); 66 | if (blocks.size() > 0) 67 | { 68 | std::vector::const_iterator it=blocks.begin(); 69 | for (int i=0; it!=blocks.end(); ++it, ++i) 70 | res[i] = it->name_; 71 | } 72 | return res; 73 | } 74 | 75 | //clean the object list based on the timestamp 76 | std::vector getObjectsOldList(std::vector *objects) 77 | { 78 | std::vector objects_id; 79 | if (objects->size()>0) 80 | { 81 | ros::Time now = ros::Time::now() - ros::Duration(5); 82 | std::vector::iterator block=objects->begin(); 83 | for (; block != objects->end(); ++block) 84 | { 85 | if (block->timestamp_ < now) 86 | objects_id.push_back(block->name_); 87 | //resetBlock(&(*block)); 88 | } 89 | } 90 | return objects_id; 91 | } 92 | 93 | void swapPoses(geometry_msgs::Pose *pose1, 94 | geometry_msgs::Pose *pose2) 95 | { 96 | geometry_msgs::Pose temp = *pose1; 97 | pose1 = pose2; 98 | *pose2 = temp; 99 | } 100 | 101 | } 102 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/objprocessing.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef OBJPROCESSING_HPP 18 | #define OBJPROCESSING_HPP 19 | 20 | #include 21 | 22 | // ROS 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include "romeo_moveit_actions/metablock.hpp" 35 | #include "romeo_moveit_actions/evaluation.hpp" 36 | 37 | typedef object_recognition_msgs::RecognizedObjectArray RecognizedObjArray; 38 | typedef actionlib::SimpleActionClient ObjRecoActionClient; 39 | typedef object_recognition_msgs::GetObjectInformation GetObjInfo; 40 | 41 | namespace moveit_simple_actions 42 | { 43 | 44 | //! @brief Class for processing objects recognized by ORK. 45 | class ObjProcessor 46 | { 47 | public: 48 | //! @brief constructor 49 | ObjProcessor(ros::NodeHandle *nh, 50 | Evaluation *evaluation); 51 | 52 | //! @brief trigger object detection client 53 | bool triggerObjectDetection(); 54 | 55 | //! @brief convert object recognition messages into collision objects 56 | void getRecognizedObjects(const RecognizedObjArray::ConstPtr& msg); 57 | 58 | //! @brief get amount of objects 59 | int getAmountOfBlocks() 60 | { 61 | return blocks_.size(); 62 | } 63 | 64 | //! @brief get an object by id 65 | MetaBlock * getBlock(const int &id); 66 | 67 | //! @brief add a new object to the end 68 | void addBlock(const MetaBlock &block); 69 | 70 | //! @brief clean the list of objects based on the timestamp 71 | void cleanObjects(const bool list_erase=true); 72 | 73 | /** publisher of objects' poses */ 74 | ros::Publisher pub_obj_poses_; 75 | 76 | protected: 77 | //! @brief get the object's mesh from the DB 78 | bool getMeshFromDB(GetObjInfo &obj_info); 79 | 80 | //! @brief publish all collision objects in MoveIt 81 | void publishAllCollObj(std::vector *blocks); 82 | 83 | /** node handle */ 84 | ros::NodeHandle *nh_; 85 | 86 | /** evaluation of reaching/grasping */ 87 | Evaluation *evaluation_; 88 | 89 | /** object recognition action */ 90 | const std::string OBJECT_RECOGNITION_ACTION; 91 | 92 | /** final object frame */ 93 | std::string target_frame_; 94 | 95 | /** recognized object topic */ 96 | std::string object_topic_; 97 | 98 | /** if found object info server */ 99 | bool found_srv_obj_info_; 100 | 101 | /** Client for getting the mesh for a database object */ 102 | ros::ServiceClient get_model_mesh_srv_; 103 | 104 | /** object recognition client */ 105 | boost::scoped_ptr obj_reco_client_; 106 | 107 | /** if found object recognition server */ 108 | bool found_obj_reco_client_; 109 | 110 | /** subscriber to object recognition topic */ 111 | ros::Subscriber object_sub_; 112 | 113 | /** transform listener */ 114 | tf::TransformListener listener_; 115 | 116 | /** current MoveIt scene */ 117 | moveit::planning_interface::PlanningSceneInterface current_scene_; 118 | 119 | /** set of available objects */ 120 | std::vector blocks_; 121 | 122 | //! @brief all objects positions 123 | geometry_msgs::PoseArray msg_obj_poses_; 124 | }; 125 | } 126 | #endif // OBJPROCESSING_HPP 127 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/metablock.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef METABLOCK_H 18 | #define METABLOCK_H 19 | 20 | // ROS 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | typedef shape_msgs::SolidPrimitive sprimitive; 36 | typedef moveit_msgs::CollisionObject mcollobj; 37 | typedef moveit::planning_interface::PlanningSceneInterface mscene; 38 | 39 | namespace moveit_simple_actions 40 | { 41 | 42 | //! @brief Class for working with collision objects. 43 | class MetaBlock 44 | { 45 | public: 46 | //! @brief shape-based constructor 47 | MetaBlock(const std::string name, 48 | const geometry_msgs::Pose pose, 49 | const uint &shapeType, 50 | const float &size_x, 51 | const float &size_y, 52 | const float &size_z, 53 | ros::Time timestamp=ros::Time::now(), 54 | std::string base_frame="base_link"); 55 | 56 | //! @brief mesh-based constructor 57 | MetaBlock(const std::string name, 58 | const geometry_msgs::Pose pose, 59 | const shape_msgs::Mesh mesh, 60 | const object_recognition_msgs::ObjectType type, 61 | ros::Time timestamp=ros::Time::now(), 62 | std::string base_frame="odom"); 63 | 64 | //! @brief update the object pose 65 | void updatePose(const geometry_msgs::Pose &pose); 66 | 67 | //! @brief update the object's pose visually only 68 | void updatePoseVis(const geometry_msgs::Pose &pose); 69 | 70 | //! @brief update the pose and publish 71 | void updatePose(mscene *current_scene, 72 | const geometry_msgs::Pose &pose); 73 | 74 | //! @brief remove the collision object 75 | void removeBlock(mscene *current_scene); 76 | 77 | //! @brief publish the collision object 78 | void publishBlock(mscene *current_scene); 79 | 80 | //! @brief get the transform to base_link 81 | tf::Stamped getTransform(tf::TransformListener *listener, 82 | const std::string &frame); 83 | 84 | //! @brief get the transform to base_link 85 | geometry_msgs::PoseStamped getTransformed(tf::TransformListener *listener, 86 | const std::string &frame); 87 | 88 | /** object name */ 89 | std::string name_; 90 | 91 | /** the current position */ 92 | geometry_msgs::Pose pose_; 93 | 94 | /** the goal position */ 95 | geometry_msgs::Pose goal_pose_; 96 | 97 | /** corresponding collision object */ 98 | moveit_msgs::CollisionObject collObj_; 99 | 100 | /** x dimenssion */ 101 | double size_x_; 102 | 103 | /** y dimenssion */ 104 | double size_y_; 105 | 106 | /** z dimenssion */ 107 | double size_z_; 108 | 109 | /** timestamp of creation */ 110 | ros::Time timestamp_; 111 | 112 | /** the base frame */ 113 | std::string base_frame_; 114 | 115 | /** corresponding object type in DB */ 116 | object_recognition_msgs::ObjectType type_; 117 | 118 | protected: 119 | /** the object's shape */ 120 | shape_msgs::SolidPrimitive shape_; 121 | 122 | /** the object's mesh */ 123 | shape_msgs::Mesh mesh_; 124 | }; 125 | } 126 | 127 | #endif // METABLOCK_H 128 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | romeo_moveit_actions 2 | ==================== 3 | 4 | The package allows to perform simple actions with MoveIt! configured for Romeo, NAO, and Pepper robots. 5 | 6 | It offers a set of functionalities, like: 7 | * pick and place objects, 8 | * reach and grasp objects, 9 | * test a target space for reaching and grasping, 10 | * manage objects 11 | * virtual objects: add, move, remove, 12 | * detect objects with the robot's sensors: call ORK Linemod object recognition. 13 | 14 | Documentation 15 | ============= 16 | 17 | To compile the documentation, please run 18 | 19 | * rosdoc_lite 20 | 21 | Installation 22 | ============ 23 | 24 | * apt-get install ros-indigo-romeo-moveit-actions 25 | * or from source: git clone https://github.com/ros-aldebaran/romeo_moveit_actions 26 | 27 | In case of compiling from source, install the following dependencies: 28 | * MoveIt! 29 | * apt-get install ros-indigo-moveit-full 30 | * apt-get install ros-indigo-moveit-visual-tools 31 | * MoveIt! Grasp Generator 32 | * from source, the recommended branch: git clone https://github.com/nlyubova/moveit_simple_grasps -b romeo-dev 33 | 34 | Additionally, install a required robot description package and MoveIt! configuration, at minimum. 35 | 36 | Get a robot description: 37 | * for Romeo: 38 | * apt-get install ros-indigo-romeo-description 39 | * or from source https://github.com/ros-aldebaran/romeo_robot 40 | * for Nao: 41 | * apt-get install ros-indigo-nao-description ros-indigo-nao-meshes 42 | * or from source https://github.com/ros-naoqi/nao_robot, https://github.com/ros-naoqi/nao_meshes 43 | * for Pepper: 44 | * apt-get install ros-indigo-pepper-description ros-indigo-pepper-meshes 45 | * or from source https://github.com/ros-naoqi/pepper_robot, https://github.com/ros-naoqi/pepper_meshes 46 | 47 | Get a robot-specific Moveit! configuration: 48 | * for Romeo: 49 | * apt-get install ros-indigo-romeo-moveit-config 50 | * or from source https://github.com/ros-aldebaran/romeo_moveit_config 51 | * for Nao: 52 | * apt-get install ros-indigo-nao-moveit-config 53 | * or from source https://github.com/ros-naoqi/nao_moveit_config 54 | * for Pepper: 55 | * apt-get install ros-indigo-pepper-moveit-config 56 | * or from source https://github.com/ros-naoqi/pepper_moveit_config 57 | 58 | 59 | For a real robot (not a simulator) 60 | -------------------------------- 61 | If you are working with a real robot (not a simulator) then install the dcm-related package: 62 | * apt-get install ros-indigo-naoqi-dcm-driver 63 | * for Romeo: 64 | * apt-get install ros-indigo-romeo-control ros-indigo-romeo-dcm-bringup 65 | * from source https://github.com/ros-aldebaran/romeo_virtual, https://github.com/ros-aldebaran/romeo_dcm_robot 66 | * for Pepper: 67 | * apt-get install ros-indigo-pepper-control ros-indigo-pepper-dcm-bringup 68 | * from source https://github.com/ros-naoqi/pepper_virtual, https://github.com/ros-naoqi/pepper_dcm_robot 69 | * for Nao: 70 | * sudo-apt-get install ros-indigo-nao-control ros-indigo-nao-dcm-bringup 71 | * or from source https://github.com/ros-naoqi/nao_virtual, https://github.com/ros-aldebaran/nao_dcm_robot 72 | 73 | Compile all source packages. 74 | 75 | 76 | How it works 77 | ============ 78 | 79 | For a simulator (example for Romeo robot) 80 | ------------------- 81 | 82 | Launch MoveIt!: 83 | 84 | .. code-block:: bash 85 | 86 | roslaunch romeo_moveit_config demo.launch 87 | 88 | Wait until the robot model is loaded and launch moveit_simple_actions: 89 | 90 | .. code-block:: bash 91 | 92 | roslaunch romeo_moveit_actions actions_romeo_sim.launch 93 | 94 | Welcome to the world of simple actions! Now, you can add virtual objects or detect real objects and reach/grasp/place them ! 95 | 96 | 97 | For a real robot (example for Romeo robot) 98 | -------------------------------- 99 | 100 | Launch the dcm_bringup (providing the robot's IP): 101 | 102 | .. code-block:: bash 103 | 104 | roslaunch romeo_dcm_bringup romeo_dcm_bringup_remote.launch robot_ip:= 105 | 106 | Launch MoveIt!: 107 | 108 | .. code-block:: bash 109 | 110 | roslaunch romeo_moveit_config moveit_planner.launch 111 | 112 | Wait until the robot model is loaded and launch moveit_simple_actions: 113 | 114 | .. code-block:: bash 115 | 116 | roslaunch romeo_moveit_actions actions_romeo.launch 117 | 118 | Welcome to the world of simple actions! Now, you can add virtual objects or detect real objects and reach/grasp/place them ! 119 | 120 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/evaluation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef EVALUATION_HPP 18 | #define EVALUATION_HPP 19 | 20 | #include 21 | 22 | #include "romeo_moveit_actions/metablock.hpp" 23 | #include "romeo_moveit_actions/action.hpp" 24 | 25 | namespace moveit_simple_actions 26 | { 27 | 28 | //! @brief Class for evaluation the algorithm. 29 | class Evaluation 30 | { 31 | public: 32 | //! @brief constructor 33 | Evaluation(const bool &verbose, const std::string &base_frame); 34 | 35 | //! @brief initialization 36 | void init(const double &test_step, 37 | const double &block_size_x, 38 | const double &block_size_y, 39 | const double floor_to_base_height, 40 | const double &x_min, 41 | const double &x_max, 42 | const double &y_min, 43 | const double &y_max, 44 | const double &z_min, 45 | const double &z_max); 46 | 47 | //! @brief testing grasping or approximate grasping 48 | void testReach(ros::NodeHandle &nh, 49 | ros::Publisher *pub_obj_pose, 50 | ros::Publisher *pub_obj_poses, 51 | ros::Publisher *pub_obj_moveit, 52 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools, 53 | Action *action_left, 54 | Action *action_right, 55 | std::vector *blocks_surfaces, 56 | const bool pickVsReach, 57 | const bool test_poses_rnd=false); 58 | 59 | //! @brief printing the successfully reached positions 60 | void printStat(); 61 | 62 | //! @brief checking if the pose is within the working space (close enough) 63 | bool inWorkSpace(geometry_msgs::Pose pose, 64 | const bool x=true, 65 | const bool y=true, 66 | const bool z=true); 67 | 68 | protected: 69 | //! @brief generating test poses in a regular maner 70 | geometry_msgs::PoseArray generatePosesGrid(); 71 | 72 | //! @brief generating poses in a random maner 73 | geometry_msgs::PoseArray generatePosesRnd(const int poses_nbr); 74 | 75 | //! @brief testing a single hand 76 | int testReachWithGenSingleHand(Action *action, 77 | std::vector *blocks_surfaces, 78 | ros::Publisher *pub_obj_pose, 79 | ros::Publisher *pub_obj_poses, 80 | ros::Publisher *pub_obj_moveit, 81 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools, 82 | const bool pickVsReach, 83 | const int attempts_nbr, 84 | const double planning_time, 85 | geometry_msgs::PoseArray &msg_poses_validated); 86 | 87 | /** verbose or not */ 88 | bool verbose_; 89 | 90 | /** robot's base_frame */ 91 | std::string base_frame_; 92 | 93 | /** step for testing the working space */ 94 | double test_step_; 95 | 96 | /** size X of a default object */ 97 | double block_size_x_; 98 | 99 | /** size Y of a default object */ 100 | double block_size_y_; 101 | 102 | /** shift of the robot's base to the floor */ 103 | double floor_to_base_height_; 104 | 105 | /** working space in X dim min */ 106 | float x_min_; 107 | 108 | /** working space in X dim max */ 109 | float x_max_; 110 | 111 | /** working space in Y dim min */ 112 | float y_min_; 113 | 114 | /** working space in Y dim max */ 115 | float y_max_; 116 | 117 | /** working space in Z dim min */ 118 | float z_min_; 119 | 120 | /** working space in Z dim max */ 121 | float z_max_; 122 | 123 | /** default zero pose */ 124 | geometry_msgs::Pose pose_zero_; 125 | 126 | /** successfully reached target poses */ 127 | std::vector stat_poses_success_; 128 | 129 | /** default object to grasp */ 130 | MetaBlock *block_; 131 | 132 | /** default table to grasp on */ 133 | MetaBlock *table_; 134 | }; 135 | } 136 | #endif // EVALUATION_HPP 137 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/simplepickplace.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef SIMPLEACTIONS_HPP 18 | #define SIMPLEACTIONS_HPP 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "romeo_moveit_actions/metablock.hpp" 25 | #include "romeo_moveit_actions/action.hpp" 26 | #include "romeo_moveit_actions/objprocessing.hpp" 27 | #include "romeo_moveit_actions/evaluation.hpp" 28 | 29 | namespace moveit_simple_actions 30 | { 31 | 32 | //! @brief Class for running the main pipeline. 33 | class SimplePickPlace 34 | { 35 | public: 36 | //! @brief constructor 37 | SimplePickPlace(const std::string robot_name, 38 | const double test_step, 39 | const double x_min, 40 | const double x_max, 41 | const double y_min, 42 | const double y_max, 43 | const double z_min, 44 | const double z_max, 45 | const std::string left_arm_name, 46 | const std::string right_arm_name, 47 | const bool verbose); 48 | 49 | //! @brief main cycle 50 | void run(); 51 | 52 | protected: 53 | //! @brief create a table object 54 | MetaBlock createTable(); 55 | 56 | //! @brief switch between the left and right arms 57 | void switchArm(Action *action_now); 58 | 59 | //! @brief create and publish an object 60 | void createObj(const MetaBlock &block); 61 | 62 | //! @brief publish the object at new position 63 | void resetBlock(MetaBlock *block); 64 | 65 | //! @brief get collision objects from the topic /collision_object 66 | void getCollisionObjects(const moveit_msgs::CollisionObject::ConstPtr& msg); 67 | 68 | //! @brief clean the object list based on the timestamp 69 | void cleanObjects(std::vector *objects, 70 | const bool list_erase=true); 71 | 72 | //! @brief check if the block exists 73 | bool checkObj(int &block_id); 74 | 75 | /** node handle */ 76 | ros::NodeHandle nh_, nh_priv_; 77 | 78 | /** robot's name */ 79 | std::string robot_name_; 80 | 81 | /** verbosity */ 82 | const bool verbose_; 83 | 84 | /** robot's base_frame */ 85 | std::string base_frame_; 86 | 87 | /** dimenssion x of a default object */ 88 | double block_size_x_; 89 | 90 | /** dimenssion y of a default object */ 91 | double block_size_y_; 92 | 93 | /** shift of the robot's base to the floor */ 94 | double floor_to_base_height_; 95 | 96 | /** object processing */ 97 | ObjProcessor obj_proc_; 98 | 99 | /** evaluation of reaching/grasping */ 100 | Evaluation evaluation_; 101 | 102 | /** state of re-drawing the world */ 103 | bool env_shown_; 104 | 105 | //the working space of the robot 106 | double x_min_; 107 | double x_max_; 108 | double y_min_; 109 | double y_max_; 110 | double z_min_; 111 | double z_max_; 112 | 113 | /** name of the current support surface */ 114 | std::string support_surface_; 115 | 116 | /** instance of an Action class for the left arm */ 117 | Action *action_left_; 118 | 119 | /** instance of an Action class for the right arm */ 120 | Action *action_right_; 121 | 122 | /** visual tools pointer used for scene visualization */ 123 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 124 | 125 | /** current MoveIt scene */ 126 | moveit::planning_interface::PlanningSceneInterface current_scene_; 127 | 128 | /** set of available surfaces */ 129 | std::vector blocks_surfaces_; 130 | 131 | /** subscriber to get objects from /collision_object */ 132 | ros::Subscriber sub_obj_coll_; 133 | 134 | /** publisher of the current object pose */ 135 | ros::Publisher pub_obj_pose_; 136 | 137 | /** current object position */ 138 | geometry_msgs::PoseStamped msg_obj_pose_; 139 | 140 | /** default object pose for the left arm */ 141 | geometry_msgs::Pose pose_default_; 142 | 143 | /** default object pose for the right arm */ 144 | geometry_msgs::Pose pose_default_r_; 145 | 146 | /** default object pose at zero */ 147 | geometry_msgs::Pose pose_zero_; 148 | 149 | /** all successfully reached positions */ 150 | std::vector stat_poses_success_; 151 | 152 | /** publisher of collision objects to /collision_world */ 153 | ros::Publisher pub_obj_moveit_; 154 | 155 | /** processing rate */ 156 | ros::Rate rate_; 157 | }; 158 | } 159 | 160 | #endif // SIMPLEACTIONS_HPP 161 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/tools.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef TOOLS_H 18 | #define TOOLS_H 19 | 20 | #include 21 | 22 | namespace moveit_simple_actions 23 | { 24 | 25 | void printTutorial(const std::string robot_name) 26 | { 27 | std::cout << "\n \n" 28 | << "************************************************\n" 29 | << "Tutorial for simple actions with Romeo, NAO, and Pepper.\n" 30 | << "You have chosen -" << robot_name << "- robot. \n\n" 31 | 32 | << "In your scene, you should see:\n" 33 | << "- a robot\n" 34 | << "- an object\n" 35 | << "- [optionally] a table. \n\n" 36 | 37 | << "The scenario is\n" 38 | << "1) Try to grasp an object (for Romeo only) by pressing -g- + Enter.\n" 39 | << "2) If the grasp is successful, place the object by typing -place-\n" 40 | << " If not, try an approximative grasp by typing -u- + Enter.\n" 41 | << "3) Try to move the object right/left/top/down/farther/closer\n" 42 | << " by pressing -s- -f- -e- -x- -r- -c- keys and grasp again.\n" 43 | << "4) Try to grasp with another hand.\n" 44 | << "************************************************\n" 45 | << std::endl; 46 | } 47 | 48 | void printAllActions() 49 | { 50 | ROS_INFO_STREAM("Possible actions are: \n" 51 | << " g - pick an object, \n" 52 | << " p - place the object, \n" 53 | 54 | << " u - reach and grasp, \n" 55 | << " reachtop - reach the object from top, \n" 56 | 57 | //<< " plan - plan grasping the object, \n" 58 | << " a - plan all possible grasps, \n" 59 | //<< " execute - execute the planned action, \n" 60 | 61 | << " i - go to init pose (i1, i2, i3, i0), \n" 62 | << " get_pose - show the pre-grasp pose, \n" 63 | << " hand_open - open hand, \n" 64 | << " hand_close - close hand, \n" 65 | << " look_down - move the head to look down, \n" 66 | << " look_zero - move the head to the zero pose, \n" 67 | 68 | << " d - detect objects, \n" 69 | << " lc - add a cylinder on the left, \n" 70 | << " rc - add a cylinder on the right, \n" 71 | << " table - add/remove the table/scene, \n" 72 | << " set_table_height - set the table height, \n" 73 | //<< " compute_distance - compute the distance to the object, \n" 74 | << " next_obj - process the next object, \n" 75 | << " e/s/f/x/r/c - move the object top/left/right/down/closer/farther \n" 76 | 77 | << " switcharm - switch the active arm, \n" 78 | 79 | << " test_pick - test the goal space for picking, \n" 80 | << " test_reach - test the goal space for reaching, \n" 81 | //<< " stat - print statistics on successfull grasps, \n" 82 | 83 | << " q - exit, \n" 84 | << " So, what do you choose?" 85 | ); 86 | } 87 | 88 | int promptUserAction() 89 | { 90 | printAllActions(); 91 | char ascii; 92 | std::cin >> ascii; 93 | int in = (int) ascii; 94 | return in; 95 | } 96 | 97 | std::string promptUserQuestionString() 98 | { 99 | printAllActions(); 100 | std::string input; 101 | std::cin >> input; 102 | return input; 103 | } 104 | 105 | bool promptUserQuestion(const std::string command) 106 | { 107 | ROS_INFO_STREAM_NAMED("pick_place",command); 108 | char input; 109 | std::cin >> input; 110 | if( input == 'n' ) // used for yes/no 111 | return false; 112 | 113 | return true; 114 | } 115 | 116 | double promptUserValue(const std::string command) 117 | { 118 | ROS_INFO_STREAM_NAMED("pick_place",command); 119 | double input; 120 | std::cin >> input; 121 | return input; 122 | } 123 | 124 | double promptUserInt(const std::string command) 125 | { 126 | ROS_INFO_STREAM_NAMED("pick_place",command); 127 | double input; 128 | std::cin >> input; 129 | return input; 130 | } 131 | 132 | bool promptUser() 133 | { 134 | ROS_INFO_STREAM_NAMED("pick_place","Retry? (y/n)"); 135 | char input; // used for prompting yes/no 136 | std::cin >> input; 137 | if( input == 'n' ) 138 | return false; 139 | 140 | return true; 141 | } 142 | } 143 | #endif // TOOLS_H 144 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include "romeo_moveit_actions/simplepickplace.hpp" 20 | 21 | void parse_command_line(int argc, char ** argv, 22 | std::string &robot_name_, 23 | double &test_step_, 24 | double &x_min_, 25 | double &x_max_, 26 | double &y_min_, 27 | double &y_max_, 28 | double &z_min_, 29 | double &z_max_, 30 | std::string &left_arm_name_, 31 | std::string &right_arm_name_, 32 | bool &verbose_) 33 | { 34 | std::string robot_name; 35 | double test_step(0.0); 36 | double x_min(0.0); 37 | double x_max(0.0); 38 | double y_min(0.0); 39 | double y_max(0.0); 40 | double z_min(0.0); 41 | double z_max(0.0); 42 | std::string left_arm_name(""); 43 | std::string right_arm_name(""); 44 | bool verbose; 45 | boost::program_options::options_description desc("Configuration"); 46 | desc.add_options() 47 | ("help", "show this help message") 48 | ("robot_name", boost::program_options::value(&robot_name)->default_value(robot_name_), 49 | "robot_name") 50 | ("test_step", boost::program_options::value(&test_step)->default_value(test_step_), 51 | "test_step") 52 | ("x_min", boost::program_options::value(&x_min)->default_value(x_min_), 53 | "x_min") 54 | ("x_max", boost::program_options::value(&x_max)->default_value(x_max_), 55 | "x_max") 56 | ("y_min", boost::program_options::value(&y_min)->default_value(y_min_), 57 | "y_min") 58 | ("y_max", boost::program_options::value(&y_max)->default_value(y_max_), 59 | "y_max") 60 | ("z_min", boost::program_options::value(&z_min)->default_value(z_min_), 61 | "z_min") 62 | ("z_max", boost::program_options::value(&z_max)->default_value(z_max_), 63 | "z_max") 64 | ("left_arm_name", boost::program_options::value(&left_arm_name)->default_value(left_arm_name), 65 | "left_arm_name") 66 | ("right_arm_name", boost::program_options::value(&right_arm_name)->default_value(right_arm_name), 67 | "right_arm_name") 68 | ("verbose", boost::program_options::value(&verbose)->default_value(verbose_), 69 | "verbose") 70 | /*("depth_frame_id", boost::program_options::value(&depth_frame_id)->default_value(m_depth_frame_id), 71 | "depth_frame_id")*/ 72 | ; 73 | boost::program_options::variables_map vm; 74 | boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); 75 | boost::program_options::notify(vm); 76 | robot_name_ = vm["robot_name"].as(); 77 | test_step_ = vm["test_step"].as(); 78 | x_min_ = vm["x_min"].as(); 79 | x_max_ = vm["x_max"].as(); 80 | y_min_ = vm["y_min"].as(); 81 | y_max_ = vm["y_max"].as(); 82 | z_min_ = vm["z_min"].as(); 83 | z_max_ = vm["z_max"].as(); 84 | left_arm_name_ = vm["left_arm_name"].as(); 85 | right_arm_name_ = vm["right_arm_name"].as(); 86 | verbose_ = vm["verbose"].as(); 87 | 88 | ROS_INFO_STREAM("robot name = " << robot_name_); 89 | /*ROS_INFO_STREAM("test_step= " << test_step_); 90 | ROS_INFO_STREAM("x_min= " << x_min_); 91 | ROS_INFO_STREAM("x_max= " << x_max_); 92 | ROS_INFO_STREAM("y_min= " << y_min_); 93 | ROS_INFO_STREAM("y_max= " << y_max_); 94 | ROS_INFO_STREAM("z_min= " << z_min_); 95 | ROS_INFO_STREAM("z_max= " << z_max_); 96 | ROS_INFO_STREAM("left_arm_name = " << left_arm_name); 97 | ROS_INFO_STREAM("right_arm_name = " << right_arm_name);*/ 98 | 99 | if (vm.count("help")) { 100 | std::cout << desc << "\n"; 101 | return ; 102 | } 103 | } 104 | 105 | int main(int argc, char **argv) 106 | { 107 | ros::init (argc, argv, "moveit_simple_action"); 108 | ros::AsyncSpinner spinner(1); 109 | spinner.start(); 110 | 111 | // Check for verbose flag 112 | bool verbose = false; 113 | 114 | std::string robot_name("romeo"); 115 | double test_step(0.0); 116 | double x_min(0.0); 117 | double x_max(0.0); 118 | double y_min(0.0); 119 | double y_max(0.0); 120 | double z_min(0.0); 121 | double z_max(0.0); 122 | std::string left_arm_name("left"); 123 | std::string right_arm_name("right"); 124 | parse_command_line(argc, argv, robot_name, test_step, 125 | x_min, x_max, y_min, y_max, z_min, z_max, 126 | left_arm_name, right_arm_name, 127 | verbose); 128 | 129 | srand (time(NULL)); 130 | 131 | // Start the pick place node 132 | moveit_simple_actions::SimplePickPlace server_pickplace(robot_name, 133 | test_step, 134 | x_min, 135 | x_max, 136 | y_min, 137 | y_max, 138 | z_min, 139 | z_max, 140 | left_arm_name, 141 | right_arm_name, 142 | verbose); 143 | 144 | server_pickplace.run(); 145 | 146 | ROS_INFO_STREAM_NAMED("moveit_simple_action", "Shutting down."); 147 | spinner.stop(); 148 | ros::shutdown(); 149 | 150 | return 0; 151 | } 152 | -------------------------------------------------------------------------------- /src/postures.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | // MoveIt! 18 | #include 19 | 20 | #include "romeo_moveit_actions/postures.hpp" 21 | 22 | namespace moveit_simple_actions 23 | { 24 | Posture::Posture(const std::string robot_name, 25 | const std::string eef_name, 26 | const std::string group_name) 27 | { 28 | //set joint for the head 29 | moveit::planning_interface::MoveGroup move_group_head("head"); 30 | //get current 31 | move_group_head.getCurrentState()->copyJointGroupPositions(move_group_head.getCurrentState()->getRobotModel()->getJointModelGroup(move_group_head.getName()), pose_head_down_); 32 | if (pose_head_down_.size() > 1) 33 | if (robot_name == "nao") 34 | pose_head_down_[1] = 0.3; 35 | else 36 | pose_head_down_[1] = 0.35; 37 | pose_head_zero_.resize(pose_head_down_.size(), 0.0); 38 | 39 | //get the arm joints 40 | moveit::planning_interface::MoveGroup move_group_plan(group_name); 41 | std::vector pose_arm_init; 42 | move_group_plan.getCurrentState()->copyJointGroupPositions( 43 | move_group_plan.getCurrentState()->getRobotModel()->getJointModelGroup( 44 | move_group_plan.getName()), pose_arm_init); 45 | 46 | //initialize the left arm 47 | pose_arm_.resize(5); 48 | pose_arm_[0] = std::vector(pose_arm_init); 49 | for (int i=1; i= 5) 52 | { 53 | //define the zero pose 54 | pose_arm_[0].assign(pose_arm_[0].size(), 0.0); 55 | 56 | //define the initial pose (arms down) 57 | pose_arm_[1][0] = 1.56; 58 | pose_arm_[1][1] = 0.14; 59 | pose_arm_[1][2] = -1.22; 60 | pose_arm_[1][3] = -0.52; 61 | pose_arm_[1][4] = -0.02; 62 | if (pose_arm_[1].size() >= 6) 63 | { 64 | if (robot_name == "romeo") 65 | pose_arm_[1][5] = 0.17; 66 | else 67 | pose_arm_[1][5] = 0.02; 68 | } 69 | 70 | //define the initial grasp pose 71 | pose_arm_[2][0] = 1.74; 72 | pose_arm_[2][1] = 0.75; 73 | pose_arm_[2][2] = -2.08; 74 | pose_arm_[2][3] = -1.15; 75 | pose_arm_[2][4] = -0.43; 76 | if (pose_arm_[2].size() >= 6) 77 | { 78 | if (robot_name == "romeo") 79 | pose_arm_[2][5] = 0.17; 80 | else 81 | pose_arm_[1][5] = 0.02; 82 | } 83 | 84 | //define another initial grasp pose 85 | pose_arm_[3][0] = 1.08; 86 | pose_arm_[3][1] = 0.66; 87 | pose_arm_[3][2] = -0.84; 88 | pose_arm_[3][3] = -0.61; 89 | pose_arm_[3][4] = -0.67; 90 | if (pose_arm_[3].size() >= 6) 91 | { 92 | if (robot_name == "romeo") 93 | pose_arm_[3][5] = 0.025; 94 | else 95 | pose_arm_[1][5] = 0.02; 96 | } 97 | 98 | //define another initial grasp pose 99 | pose_arm_[4][0] = 1.87; 100 | pose_arm_[4][1] = 0.56; 101 | pose_arm_[4][2] = -1.99; 102 | pose_arm_[4][3] = -1.05; 103 | pose_arm_[4][4] = -0.67; 104 | if (pose_arm_[4].size() >= 6) 105 | pose_arm_[4][5] = 0.02; 106 | } 107 | 108 | //initialize the right arm 109 | if (group_name.find("right") != std::string::npos) 110 | for (int i=0; i= 3) 113 | { 114 | pose_arm_[i][1] *= -1; 115 | pose_arm_[i][2] *= -1; 116 | } 117 | if (pose_arm_[i].size() >= 5) 118 | { 119 | pose_arm_[i][3] *= -1; 120 | if (robot_name != "pepper") //not for Pepper 121 | pose_arm_[i][4] *= -1; 122 | } 123 | } 124 | 125 | //get the joint of the eef 126 | std::vector pose_hand; 127 | moveit::planning_interface::MoveGroup move_group_eef(eef_name); 128 | move_group_eef.getCurrentState()->copyJointGroupPositions(move_group_eef.getCurrentState()->getRobotModel()->getJointModelGroup(move_group_eef.getName()), pose_hand); 129 | pose_hand_.resize(2); 130 | pose_hand_[0].resize(pose_hand.size(), 0.0); 131 | pose_hand_[1].resize(pose_hand.size(), 0.8); 132 | } 133 | 134 | void Posture::initHandPose(const double &value, const int &pose) 135 | { 136 | if (pose <= pose_hand_.size()) 137 | { 138 | pose_hand_[pose].assign(pose_hand_[pose].size(), value); 139 | if (pose_hand_[pose].size() > 0) 140 | pose_hand_[pose][0] = 0.0; 141 | } 142 | } 143 | 144 | bool Posture::poseHeadZero() 145 | { 146 | goToPose("head", &pose_head_zero_); 147 | } 148 | 149 | bool Posture::poseHeadDown() 150 | { 151 | goToPose("head", &pose_head_down_); 152 | } 153 | 154 | bool Posture::poseHand(const std::string &end_eff, 155 | const std::string &group, 156 | const int &pose_id) 157 | { 158 | //set the end-effector first 159 | goToPose(end_eff, &pose_hand_[0]); 160 | bool res = false; 161 | if (pose_id < pose_arm_.size()) 162 | res = goToPose(group, &pose_arm_[pose_id]); 163 | return res; 164 | } 165 | 166 | bool Posture::poseHandOpen(const std::string &end_eff) 167 | { 168 | goToPose(end_eff, &pose_hand_[0]); 169 | } 170 | 171 | bool Posture::poseHandClose(const std::string &end_eff) 172 | { 173 | goToPose(end_eff, &pose_hand_[1]); 174 | } 175 | 176 | bool Posture::goToPose(const std::string group_name, 177 | std::vector *pose) 178 | { 179 | moveit::planning_interface::MoveGroup move_group(group_name); 180 | 181 | //get the current set of joint values for the group 182 | std::vector joints; 183 | move_group.getCurrentState()->copyJointGroupPositions( 184 | move_group.getCurrentState()->getRobotModel()-> 185 | getJointModelGroup(move_group.getName()), joints); 186 | 187 | //plan to the new joint space goal and visualize the plan 188 | if (joints.size() == pose->size()) 189 | { 190 | //move_group.setApproximateJointValueTarget(*pose); 191 | move_group.setJointValueTarget(*pose); 192 | 193 | moveit::planning_interface::MoveGroup::Plan plan; 194 | bool success = move_group.plan(plan); 195 | sleep(1.0); 196 | if (success) 197 | { 198 | ROS_INFO("Action with the move_group %s", success?"":"FAILED"); 199 | move_group.move(); 200 | sleep(1.0); 201 | 202 | /*std::vector joints_temp = move_group.getCurrentJointValues(); 203 | std::cout << "Joint values of " << group_name << " "; 204 | for (int i=0; i< joints_temp.size(); ++i) 205 | std::cout << joints_temp[i] << " "; 206 | std::cout << std::endl;*/ 207 | } 208 | return success; 209 | } 210 | else 211 | ROS_INFO_STREAM("Posture: Joint size is wrong for " << group_name 212 | << ": " << joints.size() << " != " << pose->size()); 213 | return false; 214 | } 215 | } 216 | -------------------------------------------------------------------------------- /src/metablock.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | #include "romeo_moveit_actions/metablock.hpp" 21 | #include "romeo_moveit_actions/toolsForObject.hpp" 22 | 23 | namespace moveit_simple_actions 24 | { 25 | 26 | MetaBlock::MetaBlock(const std::string name, 27 | const geometry_msgs::Pose pose, 28 | const uint &shapeType, 29 | const float &size_x, 30 | const float &size_y, 31 | const float &size_z, 32 | ros::Time timestamp, 33 | std::string base_frame): 34 | name_(name), 35 | pose_(pose), 36 | size_x_(size_x), 37 | size_y_(size_y), 38 | size_z_(size_z), 39 | timestamp_(timestamp), 40 | base_frame_(base_frame) 41 | { 42 | if (pose_.position.y < 0) 43 | pose_.orientation.y *= -1; 44 | 45 | goal_pose_ = pose_; 46 | if (pose_.position.y < 0) 47 | goal_pose_.position.y -= 0.2; 48 | else 49 | goal_pose_.position.y += 0.2; 50 | 51 | //setshape 52 | sprimitive solidPrimitive; 53 | if (shapeType == sprimitive::CYLINDER) 54 | { 55 | solidPrimitive.type = sprimitive::CYLINDER; 56 | solidPrimitive.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); 57 | solidPrimitive.dimensions[sprimitive::CYLINDER_HEIGHT] = size_y; 58 | solidPrimitive.dimensions[sprimitive::CYLINDER_RADIUS] = size_x; 59 | } 60 | else 61 | { 62 | solidPrimitive.type = sprimitive::BOX; 63 | solidPrimitive.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); 64 | solidPrimitive.dimensions[sprimitive::BOX_X] = size_x; 65 | solidPrimitive.dimensions[sprimitive::BOX_Y] = size_y; 66 | solidPrimitive.dimensions[sprimitive::BOX_Z] = size_z; 67 | } 68 | shape_ = solidPrimitive; 69 | 70 | //create collision object 71 | collObj_.header.stamp = ros::Time::now(); 72 | collObj_.header.frame_id = base_frame_; 73 | collObj_.id = name_; 74 | collObj_.operation = mcollobj::ADD; 75 | collObj_.primitives.resize(1); 76 | if (shape_.dimensions.size() > 0) 77 | collObj_.primitives[0] = shape_; 78 | collObj_.primitive_poses.resize(1); 79 | collObj_.primitive_poses[0] = pose_; 80 | } 81 | 82 | MetaBlock::MetaBlock(const std::string name, 83 | const geometry_msgs::Pose pose, 84 | const shape_msgs::Mesh mesh, 85 | const object_recognition_msgs::ObjectType type, 86 | ros::Time timestamp, 87 | std::string base_frame): 88 | name_(name), 89 | pose_(pose), 90 | size_x_(0.03), 91 | size_y_(0.115), 92 | size_z_(0.01), 93 | timestamp_(timestamp), 94 | base_frame_(base_frame) 95 | { 96 | pose_.orientation.x = -1.0; 97 | pose_.orientation.y = 0.0; 98 | pose_.orientation.z = 0.0; 99 | pose_.orientation.w = 0.0; 100 | 101 | goal_pose_ = pose_; 102 | goal_pose_.position.x = 0.4; 103 | goal_pose_.position.y = 0.3; 104 | goal_pose_.position.z = -0.05; 105 | if (pose_.position.y < 0) 106 | goal_pose_.position.y *= -1; 107 | 108 | mesh_ = mesh; 109 | type_ = type; 110 | 111 | //create collision object 112 | collObj_.header.stamp = ros::Time::now(); 113 | collObj_.header.frame_id = base_frame_; 114 | collObj_.id = name_; 115 | collObj_.operation = mcollobj::ADD; 116 | collObj_.meshes.resize(1); 117 | collObj_.meshes[0] = mesh; 118 | collObj_.mesh_poses.resize(1); 119 | collObj_.mesh_poses[0] = pose_; 120 | } 121 | 122 | void MetaBlock::updatePose(const geometry_msgs::Pose &pose) 123 | { 124 | pose_ = pose; 125 | if (collObj_.primitive_poses.size() > 0) 126 | collObj_.primitive_poses[0] = pose; 127 | } 128 | 129 | void MetaBlock::updatePoseVis(const geometry_msgs::Pose &pose) 130 | { 131 | if (collObj_.primitive_poses.size() > 0) 132 | collObj_.primitive_poses[0] = pose; 133 | } 134 | 135 | void MetaBlock::removeBlock(mscene *current_scene) 136 | { 137 | // Remove/Add collision object 138 | std::vector objects_id; 139 | objects_id.resize(1); 140 | objects_id[0] = name_; 141 | current_scene->removeCollisionObjects(objects_id); 142 | } 143 | 144 | void MetaBlock::publishBlock(mscene *current_scene) 145 | { 146 | std::vector coll_objects; 147 | coll_objects.push_back(collObj_); 148 | current_scene->addCollisionObjects(coll_objects); 149 | sleep(0.6); 150 | } 151 | 152 | void MetaBlock::updatePose(mscene *current_scene, 153 | const geometry_msgs::Pose &pose) 154 | { 155 | updatePose(pose); 156 | 157 | //pub_obj_moveit->publish(collObj_); 158 | std::vector coll_objects; 159 | coll_objects.push_back(collObj_); 160 | current_scene->addCollisionObjects(coll_objects); 161 | sleep(0.5); 162 | } 163 | 164 | tf::Stamped MetaBlock::getTransform(tf::TransformListener *listener, 165 | const std::string &frame) 166 | { 167 | tf::Stamped pose_to_robot; 168 | 169 | tf::Stamped tf_obj( 170 | tf::Pose( 171 | tf::Quaternion(pose_.orientation.x, 172 | pose_.orientation.y, 173 | pose_.orientation.z, 174 | pose_.orientation.w), 175 | tf::Vector3(pose_.position.x, 176 | pose_.position.y, 177 | pose_.position.z)), 178 | ros::Time(0), base_frame_); 179 | 180 | //transform the pose 181 | try 182 | { 183 | listener->transformPose(frame, tf_obj, pose_to_robot); 184 | } 185 | catch (tf::TransformException ex) 186 | { 187 | ROS_ERROR("%s",ex.what()); 188 | } 189 | 190 | /* ROS_INFO_STREAM("The pose to the object " 191 | << " " << pose_to_robot.getOrigin().x() 192 | << " " << pose_to_robot.getOrigin().y() 193 | << " " << pose_to_robot.getOrigin().z() 194 | );*/ 195 | return pose_to_robot; 196 | } 197 | 198 | geometry_msgs::PoseStamped MetaBlock::getTransformed(tf::TransformListener *listener, 199 | const std::string &frame) 200 | { 201 | geometry_msgs::PoseStamped pose_to_robot; 202 | 203 | geometry_msgs::PoseStamped pose_obj; 204 | pose_obj.header.stamp = ros::Time(0); 205 | pose_obj.header.frame_id = base_frame_; 206 | pose_obj.pose = pose_; 207 | 208 | //the the pose 209 | try 210 | { 211 | listener->transformPose(frame, pose_obj, pose_to_robot); 212 | } 213 | catch (tf::TransformException ex) 214 | { 215 | ROS_ERROR("%s",ex.what()); 216 | } 217 | 218 | /* ROS_INFO_STREAM("The pose to the object " 219 | << " " << pose_to_robot.getOrigin().x() 220 | << " " << pose_to_robot.getOrigin().y() 221 | << " " << pose_to_robot.getOrigin().z() 222 | );*/ 223 | return pose_to_robot; 224 | } 225 | } 226 | -------------------------------------------------------------------------------- /include/romeo_moveit_actions/action.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef ACTION_HPP 18 | #define ACTION_HPP 19 | 20 | // ROS 21 | #include 22 | 23 | // MoveIt! 24 | #include 25 | 26 | // Grasp generation 27 | #include 28 | #include 29 | 30 | //for showing grasps 31 | #include 32 | 33 | // Forward kinematics to have final pose of trajectory 34 | #include 35 | #include 36 | 37 | #include "romeo_moveit_actions/metablock.hpp" 38 | #include "romeo_moveit_actions/postures.hpp" 39 | 40 | #define FLAG_NO_MOVE 1 41 | #define FLAG_MOVE 2 42 | 43 | namespace moveit_simple_actions 44 | { 45 | 46 | //! @brief Class for motion planning based on move_group. 47 | class Action 48 | { 49 | public: 50 | //! @brief constructor 51 | Action(ros::NodeHandle *nh, 52 | const std::string &arm, 53 | const std::string &robot_name, 54 | const std::string &base_frame); 55 | 56 | //! @brief initialize the visual tools 57 | void initVisualTools(moveit_visual_tools::MoveItVisualToolsPtr &visual_tools); 58 | 59 | //! @brief pick an object with a grasp generator 60 | bool pickAction(MetaBlock *block, 61 | const std::string surface_name, 62 | const int attempts_nbr=0, 63 | const double planning_time=0.0); 64 | 65 | //! @brief pick an object 66 | bool placeAction(MetaBlock *block, 67 | const std::string surface_name); 68 | 69 | //! @brief pick an object without a grasp generator 70 | bool pickDefault(MetaBlock *block, 71 | const std::string surface_name); 72 | 73 | //! @brief plan a trajectory as computePlanButtonClicked in MoveIt 74 | bool graspPlan(MetaBlock *block, 75 | const std::string surface_name); 76 | 77 | //! @brief plan and show all possible motion trajectories 78 | bool graspPlanAllPossible(MetaBlock *block, 79 | const std::string surface_name); 80 | 81 | //! @brief execute the planned motion trajectory 82 | bool executeAction(); 83 | 84 | //! @brief reach default grasping pose 85 | bool reachGrasp(MetaBlock *block, 86 | const std::string surface_name, 87 | int attempts_nbr=0, 88 | float tolerance_min=0.0f, 89 | double planning_time=0.0); 90 | 91 | //! @brief reach the top of an object 92 | bool reachAction(geometry_msgs::PoseStamped pose_target, 93 | const std::string surface_name="", 94 | const int attempts_nbr=1); 95 | 96 | //! @brief go to the pose 97 | bool poseHand(const int pose_id); 98 | 99 | //! @brief get the current pose 100 | geometry_msgs::Pose getPose(); 101 | 102 | //! @brief go to the pose that opens hand 103 | void poseHandOpen(); 104 | 105 | //! @brief go to the pose that closes hand 106 | void poseHandClose(); 107 | 108 | //! @brief go to the pose that moves the head down 109 | bool poseHeadDown(); 110 | 111 | //! @brief go to the pose that moves the head to zero 112 | bool poseHeadZero(); 113 | 114 | //! @brief set the tolerance 115 | void setTolerance(const double value); 116 | 117 | //! @brief detach the collision object 118 | void detachObject(const std::string &block_name); 119 | 120 | //! @brief get the base_link 121 | std::string getBaseLink(); 122 | 123 | //! @brief compute the distance to the object 124 | float computeDistance(MetaBlock *block); 125 | 126 | //! @brief compute the distance to the pose 127 | float computeDistance(geometry_msgs::Pose goal); 128 | 129 | //! @brief get the grasping pose for the object pose 130 | geometry_msgs::PoseStamped getGraspPose(MetaBlock *block); 131 | 132 | //! @brief release the object 133 | void releaseObject(MetaBlock *block); 134 | 135 | /** name of the planning group */ 136 | const std::string plan_group_; 137 | 138 | private: 139 | //! @brief attach the collision object 140 | void attachObject(const std::string &block_name); 141 | 142 | //! @brief publish the planning info 143 | void publishPlanInfo(moveit::planning_interface::MoveGroup::Plan plan, 144 | geometry_msgs::Pose pose_target); 145 | 146 | //! @brief set the planning time 147 | void setPlanningTime(const double value); 148 | 149 | //! @brief set the tolerance step 150 | void setToleranceStep(const double value); 151 | 152 | //! @brief set the minimum tolerance 153 | void setToleranceMin(const double value); 154 | 155 | //! @brief set the maximum velocity scaling factor 156 | void setMaxVelocityScalingFactor(const double value); 157 | 158 | //! @brief set verbosity 159 | void setVerbose(bool verbose); 160 | 161 | //! @brief set the maximum number of attempts 162 | void setAttemptsMax(int value); 163 | 164 | //! @brief set the flag if action execution is allowed 165 | void setFlag(int flag); 166 | 167 | //! @brief generate all possible grasps 168 | std::vector generateGrasps(MetaBlock *block); 169 | 170 | //! @brief configure for planning 171 | std::vector configureForPlanning( 172 | const std::vector &grasps); 173 | 174 | //! @brief set allowed collision matrix 175 | bool setAllowedMoveItCollisionMatrix(moveit_msgs::AllowedCollisionMatrix& m); 176 | 177 | //! @brief get allowed collision matrix 178 | bool getCurrentMoveItAllowedCollisionMatrix(moveit_msgs::AllowedCollisionMatrix& matrix); 179 | 180 | //! @brief ensure that the entry exist in the collision matrix 181 | std::vector::iterator ensureExistsInACM(const std::string& name, 182 | moveit_msgs::AllowedCollisionMatrix& m, 183 | bool initFlag); 184 | 185 | //! @brief add an object to the collision matrix 186 | void expandMoveItCollisionMatrix(const std::string& name, 187 | moveit_msgs::AllowedCollisionMatrix& m, 188 | bool default_val); 189 | 190 | //! @brief update the collision matrix with the object 191 | void updateCollisionMatrix(const std::string& name); 192 | 193 | 194 | /** active end effector */ 195 | const std::string end_eff_; 196 | 197 | /** posture class */ 198 | Posture posture_; 199 | 200 | /** grasp configuration */ 201 | moveit_simple_grasps::GraspData grasp_data_; 202 | 203 | /** interface with MoveIt */ 204 | boost::scoped_ptr move_group_; 205 | 206 | /** grasp generator */ 207 | moveit_simple_grasps::SimpleGraspsPtr simple_grasps_; 208 | 209 | /** for planning actions */ 210 | boost::shared_ptr current_plan_; 211 | 212 | /** visual tools pointer used for scene visualization */ 213 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 214 | 215 | /** the current MoveIt scene */ 216 | moveit::planning_interface::PlanningSceneInterface current_scene_; 217 | 218 | /** publisher for a pre-grasp pose */ 219 | ros::Publisher pub_obj_pose_; 220 | 221 | /** publish final pose */ 222 | ros::Publisher pub_plan_pose_; 223 | 224 | /** publish final trajectory */ 225 | ros::Publisher pub_plan_traj_; 226 | 227 | /** client FK*/ 228 | ros::ServiceClient client_fk_; 229 | 230 | /** verbnosity level */ 231 | bool verbose_; 232 | 233 | /** maximum number of attempts to do action */ 234 | int attempts_max_; 235 | 236 | /** planning time */ 237 | double planning_time_; 238 | 239 | /** planning library */ 240 | std::string planner_id_; 241 | 242 | /** minimum tolerance to reach */ 243 | double tolerance_min_; 244 | 245 | /** the tolerance step to vary */ 246 | double tolerance_step_; 247 | 248 | /** maximum velocity factor */ 249 | double max_velocity_scaling_factor_; 250 | 251 | /** the the distance to object allowing to grasp it */ 252 | float dist_th_; 253 | 254 | /** flag to allow motion */ 255 | int flag_; 256 | 257 | /** the current attached object */ 258 | std::string object_attached_; 259 | 260 | /** the transform listener */ 261 | tf::TransformListener listener_; 262 | 263 | /** client get scene difference */ 264 | ros::Publisher planning_scene_publisher_; 265 | 266 | /** planning scene client */ 267 | ros::ServiceClient planning_scene_client_; 268 | 269 | /** allowed collision links */ 270 | std::vector allowedCollisionLinks_; 271 | 272 | /** base frame */ 273 | std::string base_frame_; 274 | }; 275 | } 276 | #endif // ACTION_HPP 277 | -------------------------------------------------------------------------------- /src/objprocessing.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "romeo_moveit_actions/objprocessing.hpp" 22 | #include "romeo_moveit_actions/toolsForAction.hpp" 23 | #include "romeo_moveit_actions/toolsForObject.hpp" 24 | 25 | namespace moveit_simple_actions 26 | { 27 | 28 | ObjProcessor::ObjProcessor(ros::NodeHandle *nh, 29 | Evaluation *evaluation): 30 | nh_(nh), 31 | evaluation_(evaluation), 32 | OBJECT_RECOGNITION_ACTION("/recognize_objects"), 33 | target_frame_("odom"), 34 | object_topic_("/recognized_object_array"), 35 | found_srv_obj_info_(false) 36 | { 37 | std::string mesh_srv_name("get_object_info"); 38 | found_srv_obj_info_ = true; 39 | found_obj_reco_client_ = false; 40 | 41 | ros::Time start_time = ros::Time::now(); 42 | while (!ros::service::waitForService(mesh_srv_name, ros::Duration(2.0))) 43 | { 44 | ROS_INFO("Waiting for %s service to come up", mesh_srv_name.c_str()); 45 | if (!nh_->ok() || ros::Time::now() - start_time >= ros::Duration(5.0)) 46 | { 47 | found_srv_obj_info_ = false; 48 | break; 49 | } 50 | } 51 | 52 | if (found_srv_obj_info_) 53 | { 54 | get_model_mesh_srv_ = nh_->serviceClient 55 | (mesh_srv_name, false); 56 | } 57 | 58 | object_sub_ = nh_->subscribe(object_topic_, 59 | 1, 60 | &ObjProcessor::getRecognizedObjects, 61 | this); 62 | 63 | msg_obj_poses_.header.frame_id = target_frame_; 64 | 65 | pub_obj_poses_ = nh_->advertise("/obj_poses", 1); 66 | } 67 | 68 | bool ObjProcessor::triggerObjectDetection() 69 | { 70 | if(!obj_reco_client_) 71 | { 72 | obj_reco_client_.reset(new ObjRecoActionClient(OBJECT_RECOGNITION_ACTION, false)); 73 | } 74 | if (!found_obj_reco_client_) 75 | { 76 | try 77 | { 78 | ROS_DEBUG("Waiting for the Object recognition client"); 79 | waitForAction(obj_reco_client_, 80 | *nh_, 81 | ros::Duration(5, 0), 82 | OBJECT_RECOGNITION_ACTION); 83 | ROS_DEBUG("Object recognition client is ready"); 84 | found_obj_reco_client_ = true; 85 | } 86 | catch(std::runtime_error &ex) 87 | { 88 | ROS_ERROR("Object recognition action: %s", ex.what()); 89 | return false; 90 | } 91 | } 92 | 93 | if (found_obj_reco_client_) 94 | { 95 | object_recognition_msgs::ObjectRecognitionGoal goal; 96 | obj_reco_client_->sendGoal(goal); 97 | if (!obj_reco_client_->waitForResult()) 98 | { 99 | ROS_DEBUG_STREAM("Object recognition client returned early"); 100 | return false; 101 | } 102 | if (obj_reco_client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED) 103 | { 104 | ROS_DEBUG_STREAM("Fail: " 105 | << obj_reco_client_->getState().toString() << ":" 106 | << obj_reco_client_->getState().getText()); 107 | return true; 108 | } 109 | } 110 | //ROS_INFO_STREAM("triggerObjectDetection finished"); 111 | return false; 112 | } 113 | 114 | bool ObjProcessor::getMeshFromDB(GetObjInfo &obj_info) 115 | { 116 | if (!found_srv_obj_info_) 117 | return false; 118 | 119 | if ( !get_model_mesh_srv_.call(obj_info) ) 120 | { 121 | ROS_ERROR("Get model mesh service service call failed altogether"); 122 | return false; 123 | } 124 | return true; 125 | } 126 | 127 | void ObjProcessor::getRecognizedObjects(const RecognizedObjArray::ConstPtr& msg) 128 | { 129 | if (msg->objects.empty()) 130 | return; 131 | 132 | std::vector coll_objects; 133 | 134 | float block_size = 0.03f; 135 | try 136 | { 137 | int obj_id = 0; 138 | RecognizedObjArray::_objects_type::const_iterator it; 139 | for (it = msg->objects.begin(); it != msg->objects.end(); ++it) 140 | { 141 | geometry_msgs::PoseStamped msg_obj_cam_, msg_obj_pose; 142 | 143 | msg_obj_cam_.header = msg->header; 144 | msg_obj_cam_.header.stamp = ros::Time(0); 145 | msg_obj_cam_.pose = it->pose.pose.pose; 146 | listener_.transformPose(target_frame_, msg_obj_cam_, msg_obj_pose); 147 | 148 | if (!evaluation_->inWorkSpace(msg_obj_pose.pose, 0, 0, 1)) 149 | continue; 150 | 151 | ROS_INFO_STREAM("xyz: " << msg_obj_pose.pose.position.x << " " 152 | << msg_obj_pose.pose.position.y << " " 153 | << msg_obj_pose.pose.position.z); 154 | 155 | GetObjInfo obj_info; 156 | obj_info.request.type = it->type; 157 | 158 | //check if exists 159 | std::stringstream obj_id_str; 160 | obj_id_str << obj_id << "_" << it->type.key; 161 | 162 | int idx = findObj(&blocks_, obj_id_str.str()); 163 | //msg_obj_pose.pose.position.z -= 0.01; 164 | if (idx == -1) //if not found, create a new one 165 | { 166 | if (getMeshFromDB(obj_info)) 167 | { 168 | blocks_.push_back(MetaBlock(obj_id_str.str(), 169 | msg_obj_pose.pose, 170 | obj_info.response.information.ground_truth_mesh, 171 | it->type, 172 | msg->header.stamp, 173 | target_frame_)); 174 | msg_obj_poses_.poses.push_back(msg_obj_pose.pose); 175 | } 176 | else 177 | { 178 | blocks_.push_back(MetaBlock(obj_id_str.str(), 179 | msg_obj_pose.pose, 180 | sprimitive::CYLINDER, 181 | block_size, 182 | block_size*3.0f, 183 | 0.0f, 184 | msg->header.stamp, 185 | target_frame_)); 186 | msg_obj_poses_.poses.push_back(msg_obj_pose.pose); 187 | } 188 | 189 | coll_objects.push_back(blocks_.back().collObj_); 190 | //ROS_INFO_STREAM("adding an object " << obj_id_str.str()); 191 | } 192 | else if ((idx >= 0) && (idx < blocks_.size())) 193 | { 194 | blocks_.at(idx).updatePose(msg_obj_pose.pose); 195 | if (idx >= msg_obj_poses_.poses.size()) 196 | msg_obj_poses_.poses.resize(idx+1); 197 | msg_obj_poses_.poses[idx] = msg_obj_pose.pose; 198 | 199 | coll_objects.push_back(blocks_.at(idx).collObj_); 200 | //ROS_INFO_STREAM("updating the object " << obj_id_str.str()); 201 | } 202 | ++obj_id; 203 | } 204 | } 205 | catch (tf::TransformException ex) 206 | { 207 | ROS_ERROR("%s",ex.what()); 208 | ros::Duration(1.0).sleep(); 209 | } 210 | 211 | if (blocks_.size() == 0) 212 | return; 213 | 214 | // publish all objects as collision blocks 215 | if (coll_objects.size() > 0) 216 | current_scene_.addCollisionObjects(coll_objects); 217 | //publishAllCollObj(blocks_); 218 | 219 | ROS_INFO_STREAM("The detected object: " 220 | << blocks_.front().pose_.position.x << " " 221 | << blocks_.front().pose_.position.y << " " 222 | << blocks_.front().pose_.position.z 223 | << " out of " << coll_objects.size() ); 224 | 225 | //ROS_INFO_STREAM("blocks_.size() " << blocks_.size() << " " << msg_obj_poses_.poses.size()); 226 | } 227 | 228 | void ObjProcessor::publishAllCollObj(std::vector *blocks) 229 | { 230 | if (blocks->empty()) 231 | return; 232 | 233 | std::vector coll_objects; 234 | 235 | std::vector::iterator block=blocks->begin(); 236 | for (; block != blocks->end(); ++block) 237 | coll_objects.push_back(block->collObj_); 238 | 239 | if (coll_objects.size() > 0) 240 | current_scene_.addCollisionObjects(coll_objects); 241 | } 242 | 243 | MetaBlock * ObjProcessor::getBlock(const int &id) 244 | { 245 | MetaBlock * block; 246 | if (id < blocks_.size()) 247 | block = &blocks_[id]; 248 | return block; 249 | } 250 | 251 | void ObjProcessor::addBlock(const MetaBlock &block) 252 | { 253 | blocks_.push_back(block); 254 | 255 | blocks_.back().publishBlock(¤t_scene_); 256 | } 257 | 258 | //clean the object list based on the timestamp 259 | void ObjProcessor::cleanObjects(const bool list_erase) 260 | { 261 | std::vector objects_list = getObjectsOldList(&blocks_); 262 | current_scene_.removeCollisionObjects(objects_list); 263 | 264 | //remove from the memory 265 | if (list_erase) 266 | { 267 | blocks_.clear(); 268 | msg_obj_poses_.poses.clear(); //TOCHECK 269 | pub_obj_poses_.publish(msg_obj_poses_); 270 | } 271 | } 272 | } 273 | -------------------------------------------------------------------------------- /src/evaluation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "romeo_moveit_actions/evaluation.hpp" 18 | #include "romeo_moveit_actions/toolsForObject.hpp" 19 | 20 | namespace moveit_simple_actions 21 | { 22 | 23 | Evaluation::Evaluation(const bool &verbose, const std::string &base_frame): 24 | verbose_(verbose), 25 | base_frame_(base_frame) 26 | { 27 | pose_zero_.position.x = 0.0; 28 | pose_zero_.position.y = 0.0; 29 | pose_zero_.position.z = 0.0; 30 | pose_zero_.orientation.x = -1.0; 31 | pose_zero_.orientation.y = 0.0; 32 | pose_zero_.orientation.z = 0.0; 33 | pose_zero_.orientation.w = 0.0; 34 | } 35 | 36 | void Evaluation::init(const double &test_step, 37 | const double &block_size_x, 38 | const double &block_size_y, 39 | const double floor_to_base_height, 40 | const double &x_min, 41 | const double &x_max, 42 | const double &y_min, 43 | const double &y_max, 44 | const double &z_min, 45 | const double &z_max) 46 | { 47 | test_step_ = test_step; 48 | block_size_x_ = block_size_x; 49 | block_size_y_ = block_size_y; 50 | floor_to_base_height_ = floor_to_base_height; 51 | x_min_ = x_min; 52 | x_max_ = x_max; 53 | y_min_ = y_min; 54 | y_max_ = y_max; 55 | z_min_ = z_min; 56 | z_max_ = z_max; 57 | 58 | //create a default block 59 | block_ = new MetaBlock("Virtual1", 60 | pose_zero_, 61 | sprimitive::CYLINDER, 62 | block_size_x_, 63 | block_size_y_, 64 | 0.0); 65 | 66 | //create a default table 67 | double height = -floor_to_base_height_ + z_min_; 68 | double width = y_max_*2.0; 69 | double depth = 0.35; 70 | geometry_msgs::Pose pose; 71 | setPose(&pose, 72 | x_min_ + depth/2.0, 73 | 0.0, 74 | floor_to_base_height_ + height/2.0); 75 | 76 | table_ = new MetaBlock("table", pose, sprimitive::BOX, depth, width, height); 77 | } 78 | 79 | geometry_msgs::PoseArray Evaluation::generatePosesGrid() 80 | { 81 | std::vector blocks_test; 82 | geometry_msgs::PoseArray poses; 83 | poses.header.frame_id = base_frame_; 84 | 85 | /*//detect objects to get any mesh 86 | ROS_INFO_STREAM(" Looking for objects"); 87 | ros::Time start_time = ros::Time::now(); 88 | while ((blocks_.size() <= 0) && (ros::Time::now() - start_time < ros::Duration(1.0))) 89 | { 90 | objproc.triggerObjectDetection(); 91 | }*/ 92 | 93 | if (verbose_) 94 | ROS_INFO_STREAM(" Generating goals in the target space"); 95 | 96 | MetaBlock block(*block_); 97 | 98 | double y_step = test_step_*1.5; 99 | double y_min(y_min_), y_max(y_max_); 100 | 101 | int count = 0; 102 | for (double y=y_min; y<=y_max; y+=y_step) 103 | for (double z=z_min_; z<=z_max_; z+=test_step_) 104 | for (double x=x_min_; x<=x_max_; x+=test_step_) 105 | { 106 | block.pose_.position.x = x; 107 | block.pose_.position.y = y; 108 | block.pose_.position.z = z; 109 | blocks_test.push_back(block); 110 | 111 | poses.poses.push_back(block.pose_); 112 | std::cout << x << " " << y << " " << z << std::endl; 113 | ++count; 114 | } 115 | 116 | y_min = -y_max_; 117 | y_max = -y_min_; 118 | for (double y=y_min; y<=y_max; y+=y_step) 119 | for (double z=z_min_; z<=z_max_; z+=test_step_) 120 | for (double x=x_min_; x<=x_max_; x+=test_step_) 121 | { 122 | block.pose_.position.x = x; 123 | block.pose_.position.y = y; 124 | block.pose_.position.z = z; 125 | blocks_test.push_back(block); 126 | 127 | poses.poses.push_back(block.pose_); 128 | std::cout << x << " " << y << " " << z << std::endl; 129 | ++count; 130 | } 131 | 132 | if (verbose_) 133 | ROS_INFO_STREAM("Total number of generated poses=" << count); 134 | return poses; 135 | } 136 | 137 | geometry_msgs::PoseArray Evaluation::generatePosesRnd(const int poses_nbr) 138 | { 139 | std::vector blocks_test; 140 | geometry_msgs::PoseArray poses; 141 | poses.header.frame_id = "base_link"; 142 | 143 | int count = 0; 144 | while (count < poses_nbr){ 145 | geometry_msgs::Pose pose(pose_zero_); 146 | pose.position.x = 0.35f + float(rand() % 150)/1000.0f; 147 | pose.position.y = float(rand() % 90)/100.0f - 0.45; 148 | pose.position.z = -0.23f + (float(rand() % 230)/1000.0f); 149 | blocks_test.push_back(MetaBlock("BlockTest", pose, sprimitive::CYLINDER, block_size_x_, block_size_y_, 0.0)); 150 | poses.poses.push_back(blocks_test.back().pose_); 151 | ++count; 152 | } 153 | return poses; 154 | } 155 | 156 | void Evaluation::testReach(ros::NodeHandle &nh, 157 | ros::Publisher *pub_obj_pose, 158 | ros::Publisher *pub_obj_poses, 159 | ros::Publisher *pub_obj_moveit, 160 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools, 161 | Action *action_left, 162 | Action *action_right, 163 | std::vector *blocks_surfaces, 164 | const bool pickVsReach, 165 | const bool test_poses_rnd) 166 | { 167 | geometry_msgs::PoseArray poses_test; 168 | if (test_poses_rnd) 169 | poses_test = generatePosesRnd(200); 170 | else 171 | poses_test = generatePosesGrid(); 172 | 173 | //visualize all generated samples of the goal space 174 | pub_obj_poses->publish(poses_test); 175 | sleep(0.05); 176 | 177 | ros::Publisher pub_obj_poses_left = 178 | nh.advertise("/poses_reachable_left", 100); 179 | ros::Publisher pub_obj_poses_right = 180 | nh.advertise("/poses_reachable_right", 100); 181 | 182 | int targets_nbr = 0; 183 | int attempts_nbr = 1; 184 | double planning_time = 30.0; 185 | geometry_msgs::PoseArray msg_poses_validated; 186 | msg_poses_validated.header.frame_id = action_right->getBaseLink(); 187 | targets_nbr += testReachWithGenSingleHand(action_left, 188 | blocks_surfaces, 189 | pub_obj_pose, 190 | &pub_obj_poses_left, 191 | pub_obj_moveit, 192 | visual_tools, 193 | pickVsReach, 194 | attempts_nbr, planning_time, msg_poses_validated); 195 | 196 | targets_nbr += testReachWithGenSingleHand(action_right, 197 | blocks_surfaces, 198 | pub_obj_pose, 199 | &pub_obj_poses_right, 200 | pub_obj_moveit, 201 | visual_tools, 202 | pickVsReach, 203 | attempts_nbr, planning_time, msg_poses_validated); 204 | //print all reachable poses 205 | if (verbose_) 206 | { 207 | ROS_INFO_STREAM("Exploration of reachable space \n" 208 | << "Successfull/total grasps = " << msg_poses_validated.poses.size() << "/" << targets_nbr << "\n" 209 | << "Test params: \n" 210 | << "Max attempts number = " << attempts_nbr << "\n planning time = " << planning_time << "\n" 211 | << "The tested space: step = " << test_step_ << " the zone: \n" 212 | << "x: [ " << x_min_ << " " << x_max_ << " ]\n" 213 | << "y: [ " << y_min_ << " " << y_max_ << " ]\n" 214 | << "z: [ " << z_min_ << " " << z_max_ << " ]"); 215 | 216 | for (int i =0; i < msg_poses_validated.poses.size(); ++i) 217 | std::cout << msg_poses_validated.poses[i].position.x << " " << msg_poses_validated.poses[i].position.y << " " << msg_poses_validated.poses[i].position.z << std::endl; 218 | } 219 | } 220 | 221 | int Evaluation::testReachWithGenSingleHand(Action *action, 222 | std::vector *blocks_surfaces, 223 | ros::Publisher *pub_obj_pose, 224 | ros::Publisher *pub_obj_poses, 225 | ros::Publisher *pub_obj_moveit, 226 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools, 227 | const bool pickVsReach, 228 | const int attempts_nbr, 229 | const double planning_time, 230 | geometry_msgs::PoseArray &msg_poses_validated) 231 | { 232 | moveit::planning_interface::PlanningSceneInterface current_scene; 233 | 234 | //Set the test space params 235 | double y_step = test_step_*1.5; 236 | double y_min(y_min_), y_max(y_max_); 237 | if (action->plan_group_.find("right") != std::string::npos) 238 | { 239 | y_min = -y_max_; 240 | y_max = -y_min_; 241 | } 242 | 243 | MetaBlock block(*block_); 244 | 245 | int count_total = 0; 246 | for (double z=z_min_; z<=z_max_; z+=test_step_) 247 | { 248 | //update the table height 249 | table_->size_z_ = -floor_to_base_height_ + (z-block_size_y_/2.0); 250 | table_->pose_.position.z = floor_to_base_height_ + table_->size_z_/2.0; 251 | 252 | for (double y=y_min; y<=y_max; y+=y_step) 253 | for (double x=x_min_; x<=x_max_; x+=test_step_) 254 | { 255 | ++count_total; 256 | 257 | table_->pose_.position.x = x - block_size_x_/2.0 + table_->size_x_/2.0, 258 | table_->updatePose(table_->pose_); 259 | std::vector objects; 260 | objects.push_back(table_->name_); 261 | current_scene.removeCollisionObjects(objects); 262 | sleep(1.5); 263 | pub_obj_moveit->publish(table_->collObj_); 264 | 265 | // publish the pose 266 | geometry_msgs::PoseStamped msg_obj_pose; 267 | msg_obj_pose.header.frame_id = action->getBaseLink(); 268 | msg_obj_pose.pose.position.x = x; 269 | msg_obj_pose.pose.position.y = y; 270 | msg_obj_pose.pose.position.z = z; 271 | pub_obj_pose->publish(msg_obj_pose); 272 | 273 | // publish the collision object 274 | block.updatePose(msg_obj_pose.pose); 275 | pub_obj_moveit->publish(block.collObj_); 276 | 277 | bool success(false); 278 | if (pickVsReach) 279 | { 280 | success = action->pickAction(&block, 281 | table_->name_, 282 | attempts_nbr, 283 | planning_time); 284 | } 285 | else 286 | { 287 | success = action->reachGrasp(&block, 288 | table_->name_, 289 | attempts_nbr, 290 | planning_time); 291 | } 292 | if (success) 293 | { 294 | //reset object, at first detach it 295 | action->detachObject(block.name_); 296 | 297 | msg_poses_validated.poses.push_back(block.pose_); 298 | pub_obj_poses->publish(msg_poses_validated); 299 | stat_poses_success_.push_back(block.pose_); 300 | } 301 | //return the hand 302 | action->poseHand(1); 303 | sleep(1.5); 304 | 305 | //remove collision object 306 | block.removeBlock(¤t_scene); 307 | } 308 | } 309 | 310 | //return the hand 311 | action->poseHand(1); 312 | sleep(1.5); 313 | return count_total; 314 | } 315 | 316 | void Evaluation::printStat() 317 | { 318 | ROS_INFO_STREAM("Successfully grasped objects at the following locations: "); 319 | for (std::vector ::iterator it = stat_poses_success_.begin(); it != stat_poses_success_.end(); ++it) 320 | { 321 | ROS_INFO_STREAM(" [" << it-> position.x << " " << it->position.y << " " << it->position.z << "] [" 322 | << it->orientation.x << " " << it->orientation.y << " " << it->orientation.z << " " << it->orientation.w << "]"); 323 | } 324 | } 325 | 326 | bool Evaluation::inWorkSpace(geometry_msgs::Pose pose, 327 | const bool x, 328 | const bool y, 329 | const bool z) 330 | { 331 | bool res(false); 332 | if (x && y && z) 333 | { 334 | if ((pose.position.x < x_max_) && (pose.position.x > x_min_) 335 | && (pose.position.y < y_max_) && (pose.position.y > y_min_) 336 | && (pose.position.z < z_max_) && (pose.position.z > z_min_)) 337 | res = true; 338 | } 339 | if (!x && !y && z) 340 | { 341 | if ((pose.position.z < z_max_) && (pose.position.z > z_min_)) 342 | res = true; 343 | } 344 | 345 | return res; 346 | } 347 | 348 | } 349 | -------------------------------------------------------------------------------- /src/simplepickplace.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | //publish messages with objects poses 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "romeo_moveit_actions/simplepickplace.hpp" 25 | #include "romeo_moveit_actions/tools.hpp" 26 | #include "romeo_moveit_actions/toolsForObject.hpp" 27 | 28 | namespace moveit_simple_actions 29 | { 30 | MetaBlock SimplePickPlace::createTable() 31 | { 32 | //create a table 33 | double height = -floor_to_base_height_ + (pose_default_.position.z-block_size_y_/2.0); 34 | double width = std::fabs(pose_default_r_.position.y*2.0) + block_size_x_/2.0; 35 | double depth = 0.35; 36 | geometry_msgs::Pose pose; 37 | setPose(&pose, 38 | pose_default_.position.x - block_size_x_/2.0 + depth/2.0, 39 | 0.0, 40 | floor_to_base_height_ + height/2.0); 41 | 42 | MetaBlock table("table", 43 | pose, 44 | shape_msgs::SolidPrimitive::BOX, 45 | depth, 46 | width, 47 | height); 48 | return table; 49 | } 50 | 51 | SimplePickPlace::SimplePickPlace(const std::string robot_name, 52 | double test_step, 53 | const double x_min, 54 | const double x_max, 55 | const double y_min, 56 | const double y_max, 57 | const double z_min, 58 | const double z_max, 59 | const std::string left_arm_name, 60 | const std::string right_arm_name, 61 | const bool verbose): 62 | nh_("~"), 63 | nh_priv_(""), 64 | robot_name_(robot_name), 65 | verbose_(verbose), 66 | base_frame_("base_link"), 67 | block_size_x_(0.03), 68 | block_size_y_(0.13), 69 | floor_to_base_height_(-1.0), 70 | env_shown_(false), 71 | evaluation_(verbose_, base_frame_), 72 | obj_proc_(&nh_priv_, &evaluation_), 73 | rate_(12.0) 74 | { 75 | setPose(&pose_zero_, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0); 76 | pose_default_ = pose_zero_; 77 | pose_default_r_ = pose_default_; 78 | if (robot_name_ == "nao") 79 | { 80 | block_size_x_ = 0.01; 81 | setPose(&pose_default_, 0.2, 0.1, 0.0); 82 | setPose(&pose_default_r_, 0.2, -0.1, 0.0); 83 | test_step = (test_step==0.0)?0.03:test_step; 84 | x_min_ = (x_min==0.0)?0.1:x_min; 85 | x_max_ = (x_max==0.0)?0.21:x_max; 86 | y_min_ = (y_min==0.0)?0.12:y_min; 87 | y_max_ = (y_max==0.0)?0.24:y_max; 88 | z_min_ = (z_min==0.0)?-0.07:z_min; 89 | z_max_ = (z_max==0.0)?0.05:z_max; 90 | } 91 | else if (robot_name == "pepper") 92 | { 93 | block_size_x_ = 0.02; 94 | setPose(&pose_default_, 0.25, 0.2, -0.1); 95 | setPose(&pose_default_r_, 0.25, -0.2, -0.1); 96 | test_step = (test_step==0.0)?0.04:test_step; 97 | x_min_ = (x_min==0.0)?0.2:x_min; 98 | x_max_ = (x_max==0.0)?0.4:x_max; 99 | y_min_ = (y_min==0.0)?0.12:y_min; 100 | y_max_ = (y_max==0.0)?0.24:y_max; 101 | z_min_ = (z_min==0.0)?-0.13:z_min; 102 | z_max_ = (z_max==0.0)?0.0:z_max; 103 | } 104 | else if (robot_name_ == "romeo") 105 | { 106 | setPose(&pose_default_, 0.44, 0.15, -0.1); 107 | setPose(&pose_default_r_, 0.49, -0.25, -0.1); 108 | test_step = (test_step==0.0)?0.02:test_step; 109 | x_min_ = (x_min==0.0)?0.38:x_min; 110 | x_max_ = (x_max==0.0)?0.5:x_max; 111 | y_min_ = (y_min==0.0)?0.12:y_min; 112 | y_max_ = (y_max==0.0)?0.24:y_max; 113 | z_min_ = (z_min==0.0)?-0.17:z_min; 114 | z_max_ = (z_max==0.0)?-0.08:z_max; 115 | } 116 | 117 | // objects related initialization 118 | /*sub_obj_coll_ = nh_.subscribe( 119 | "/collision_object", 100, &SimplePickPlace::getCollisionObjects, this);*/ 120 | 121 | pub_obj_pose_ = nh_.advertise("/pose_current", 1); 122 | 123 | pub_obj_moveit_ = nh_.advertise("/collision_object", 10); 124 | 125 | // Load the Robot Viz Tools for publishing to rviz 126 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("odom")); 127 | visual_tools_->setManualSceneUpdating(false); 128 | visual_tools_->setFloorToBaseHeight(floor_to_base_height_); 129 | visual_tools_->deleteAllMarkers(); 130 | visual_tools_->removeAllCollisionObjects(); 131 | ros::Duration(1.0).sleep(); 132 | 133 | action_left_ = new Action(&nh_, left_arm_name, robot_name_, base_frame_); 134 | action_right_ = new Action(&nh_, right_arm_name, robot_name_, base_frame_); 135 | action_left_->initVisualTools(visual_tools_); 136 | action_right_->initVisualTools(visual_tools_); 137 | 138 | msg_obj_pose_.header.frame_id = action_left_->getBaseLink(); 139 | 140 | //move the robots parts to init positions 141 | //if (promptUserQuestion("Should I move the head to look down?")) 142 | //action_left_->poseHeadDown(); 143 | 144 | //move arms to the initial position 145 | //if (promptUserQuestion(("Should I move the "+action_left_->end_eff+" to the initial pose?").c_str())) 146 | action_left_->poseHand(1); 147 | 148 | //if (promptUserQuestion(("Should I move the "+action_right_->plan_group+" to the initial pose?").c_str())) 149 | action_right_->poseHand(1); 150 | 151 | ros::Duration(1.0).sleep(); 152 | 153 | //create a possible table 154 | blocks_surfaces_.push_back(createTable()); 155 | support_surface_ = blocks_surfaces_.back().name_; 156 | //remove surfaces and publish again 157 | if (robot_name_ == "romeo") 158 | { 159 | std::vector objects = getObjectsOldList(&blocks_surfaces_); 160 | current_scene_.removeCollisionObjects(objects); 161 | sleep(1.0); 162 | //add a collision block 163 | if (blocks_surfaces_.size() > 0) 164 | { 165 | blocks_surfaces_.back().publishBlock(¤t_scene_); 166 | env_shown_ = true; 167 | } 168 | } 169 | 170 | evaluation_.init(test_step, 171 | block_size_x_, 172 | block_size_y_, 173 | floor_to_base_height_, 174 | x_min_, 175 | x_max_, 176 | y_min_, 177 | y_max_, 178 | z_min_, 179 | z_max_); 180 | 181 | printTutorial(robot_name); 182 | } 183 | 184 | 185 | void SimplePickPlace::switchArm(Action *action) 186 | { 187 | if (action->plan_group_.find("left") != std::string::npos) 188 | action = action_right_; 189 | else 190 | action = action_left_; 191 | 192 | ROS_INFO_STREAM("Switching the active arm to " << action->plan_group_); 193 | sleep(2.0); 194 | } 195 | 196 | void SimplePickPlace::createObj(const MetaBlock &block) 197 | { 198 | obj_proc_.addBlock(block); 199 | msg_obj_pose_.header.frame_id = block.base_frame_; 200 | msg_obj_pose_.pose = block.pose_; 201 | pub_obj_pose_.publish(msg_obj_pose_); 202 | 203 | //publish the collision object 204 | //obj_proc_.blocks_->back().publishBlock(¤t_scene_); 205 | } 206 | 207 | bool SimplePickPlace::checkObj(int &block_id) 208 | { 209 | if ((block_id >= 0) && (block_id < obj_proc_.getAmountOfBlocks())) 210 | return true; 211 | else 212 | false; 213 | } 214 | 215 | void SimplePickPlace::run() 216 | { 217 | int block_id = -1; 218 | int hand_id = 0; //0: any, 1: left, 2:right 219 | Action *action = action_left_; 220 | 221 | //create a virtual object 222 | MetaBlock block_l("Virtual1", 223 | pose_default_, 224 | shape_msgs::SolidPrimitive::CYLINDER, 225 | block_size_x_, 226 | block_size_y_, 227 | 0.0); 228 | 229 | createObj(block_l); 230 | block_id = 0; 231 | 232 | std::string actionName = ""; 233 | MetaBlock *block; 234 | 235 | // Release the object if any 236 | if (checkObj(block_id)) 237 | { 238 | block = obj_proc_.getBlock(block_id); 239 | action->releaseObject(block); 240 | } 241 | 242 | //the main loop 243 | while(ros::ok()) 244 | { 245 | //if there are some objects, take the first 246 | if ((block_id == -1) && (obj_proc_.getAmountOfBlocks() > 0)) 247 | block_id = 0; 248 | //if the object does not exist 249 | else if (block_id >= obj_proc_.getAmountOfBlocks()) 250 | { 251 | ROS_WARN_STREAM("The object " << block_id << " does not exist"); 252 | block_id = -1; 253 | obj_proc_.cleanObjects(true); 254 | } 255 | 256 | if (block_id >= 0) 257 | { 258 | block = obj_proc_.getBlock(block_id); 259 | if (block == NULL) 260 | { 261 | ROS_INFO_STREAM("the object " << block_id << " does not exist"); 262 | block_id = -1; 263 | continue; 264 | } 265 | 266 | //update the object's pose 267 | msg_obj_pose_.header.frame_id = block->base_frame_; 268 | msg_obj_pose_.pose = block->pose_; 269 | pub_obj_pose_.publish(msg_obj_pose_); 270 | ROS_INFO_STREAM("The current active object is " 271 | << block->name_ 272 | << " out of " << obj_proc_.getAmountOfBlocks()); 273 | //obj_proc_.publishAllCollObj(&blocks_); 274 | } 275 | 276 | //ROS_INFO_STREAM("What do you want me to do ?"); 277 | actionName = promptUserQuestionString(); 278 | ROS_INFO_STREAM("Action chosen '" << actionName 279 | << "' object_id=" << block_id 280 | << " the arm active=" << action->plan_group_); 281 | // Pick the object with a grasp generator 282 | if ((checkObj(block_id)) && (actionName == "g")) 283 | { 284 | bool success = action->pickAction(block, support_surface_); 285 | //if not succeded then try with another arm 286 | if(!success) 287 | { 288 | switchArm(action); 289 | success = action->pickAction(block, support_surface_); 290 | } 291 | 292 | if(success) 293 | stat_poses_success_.push_back(block->pose_); 294 | } 295 | // Place the object with a default function 296 | else if ((checkObj(block_id)) && (actionName == "p")) 297 | { 298 | if(action->placeAction(block, support_surface_)) 299 | { 300 | /* swap this block's start and the end pose 301 | * so that we can then move them back to position */ 302 | swapPoses(&block->pose_, &block->goal_pose_); 303 | resetBlock(block); 304 | } 305 | } 306 | //return the hand to the initial pose and relese the object 307 | else if (actionName == "i") 308 | { 309 | // Remove the attached object and the collision object 310 | if (checkObj(block_id)) 311 | action->releaseObject(block); 312 | else 313 | action->poseHand(2); 314 | } 315 | //return the hand to the initial pose 316 | else if ((actionName.length() == 3) || (actionName.compare(0,1,"i") == 0)) 317 | { 318 | //remove the attached object and the collision object 319 | if (checkObj(block_id)) 320 | resetBlock(block); 321 | //go to the required pose 322 | action->poseHand(actionName.at(1)); 323 | } 324 | //exit the application 325 | else if (actionName == "q") 326 | break; 327 | //clean a virtual box on the left arm side 328 | else if (actionName == "lb") 329 | { 330 | obj_proc_.cleanObjects(); 331 | createObj(MetaBlock("Virtual1", pose_default_, shape_msgs::SolidPrimitive::BOX, block_size_x_, block_size_y_, 0.0)); 332 | } 333 | //create a virtual cylinder on the left hand side 334 | else if (actionName == "lc") 335 | { 336 | obj_proc_.cleanObjects(); 337 | createObj(MetaBlock("Virtual1", pose_default_, shape_msgs::SolidPrimitive::CYLINDER, block_size_x_, block_size_y_, 0.0)); 338 | } 339 | //create a virtual box on the right hand side 340 | else if (actionName == "rb") 341 | { 342 | obj_proc_.cleanObjects(); 343 | createObj(MetaBlock("Virtual1", pose_default_r_, shape_msgs::SolidPrimitive::BOX, block_size_x_, block_size_y_, 0.0)); 344 | } 345 | //create a virtual cylinder on the right hand side 346 | else if (actionName == "rc") 347 | { 348 | obj_proc_.cleanObjects(); 349 | createObj(MetaBlock("Virtual1", pose_default_r_, shape_msgs::SolidPrimitive::CYLINDER, block_size_x_, block_size_y_, 0.0)); 350 | } 351 | //detect objects 352 | else if (actionName == "d") 353 | { 354 | obj_proc_.cleanObjects(); 355 | 356 | obj_proc_.triggerObjectDetection(); 357 | } 358 | //continuous object detection 359 | else if (actionName == "dd") 360 | { 361 | obj_proc_.cleanObjects(true); 362 | 363 | ROS_INFO_STREAM("Object detection is running..."); 364 | ros::Time start_time = ros::Time::now(); 365 | while ((obj_proc_.getAmountOfBlocks() <= 0) 366 | && (ros::Time::now()-start_time < ros::Duration(10.0))) 367 | { 368 | obj_proc_.triggerObjectDetection(); 369 | rate_.sleep(); 370 | } 371 | if (verbose_) 372 | ROS_INFO_STREAM(obj_proc_.getAmountOfBlocks() << " objects detected"); 373 | } 374 | //plan possible movement 375 | else if ((checkObj(block_id)) && (actionName == "plan")) 376 | { 377 | //TODO: do not remove an object but allow a collision to it 378 | visual_tools_->cleanupCO(block->name_); 379 | action->graspPlan(block, support_surface_); 380 | resetBlock(block); 381 | } 382 | //plan all possible movements 383 | else if ((checkObj(block_id)) && (actionName == "a")) 384 | { 385 | //TODO: do not remove an object but allow a collision to it 386 | visual_tools_->cleanupCO(block->name_); 387 | action->graspPlanAllPossible(block, support_surface_); 388 | resetBlock(block); 389 | } 390 | //reaching based on default pose and grasp 391 | else if ((checkObj(block_id)) && (actionName == "u")) 392 | { 393 | action->reachGrasp(block, support_surface_, 1, 0.015, 50.0); 394 | } 395 | //reach from top 396 | else if ((checkObj(block_id)) && (actionName == "reachtop")) 397 | { 398 | //TODO: do not remove an object but allow a collision to it 399 | visual_tools_->cleanupCO(block->name_); 400 | geometry_msgs::PoseStamped pose; 401 | pose.header.frame_id = block->base_frame_; 402 | pose.header.stamp = ros::Time::now(); 403 | pose.pose = block->pose_; 404 | pose.pose.orientation.x = 0; 405 | pose.pose.position.z += block->size_x_*1.5; 406 | action->reachAction(pose, support_surface_); 407 | resetBlock(block); 408 | } 409 | //pick an object without a grasp generator 410 | else if ((checkObj(block_id)) && (actionName == "b")) 411 | { 412 | action->pickDefault(block, support_surface_); 413 | } 414 | //execute the planned movement 415 | else if ((checkObj(block_id)) && (actionName == "execute")) 416 | { 417 | action->executeAction(); 418 | } 419 | //print the current pose 420 | else if (actionName == "get_pose") 421 | { 422 | action->getPose(); 423 | } 424 | //open hand 425 | else if (actionName == "hand_open") 426 | { 427 | action->poseHandOpen(); 428 | } 429 | //close hand 430 | else if (actionName == "hand_close") 431 | { 432 | action->poseHandClose(); 433 | } 434 | //process the next object 435 | else if (actionName == "next_obj") 436 | ++block_id; 437 | //test the goal space for picking 438 | else if (actionName == "test_pick") 439 | { 440 | cleanObjects(&blocks_surfaces_, false); 441 | obj_proc_.cleanObjects(); 442 | evaluation_.testReach(nh_, 443 | &pub_obj_pose_, 444 | &obj_proc_.pub_obj_poses_, 445 | &pub_obj_moveit_, 446 | visual_tools_, 447 | action_left_, 448 | action_right_, 449 | &blocks_surfaces_, 450 | true); 451 | } 452 | //test the goal space for reaching 453 | else if (actionName == "test_reach") 454 | { 455 | obj_proc_.cleanObjects(); 456 | evaluation_.testReach(nh_, 457 | &pub_obj_pose_, 458 | &obj_proc_.pub_obj_poses_, 459 | &pub_obj_moveit_, 460 | visual_tools_, 461 | action_left_, 462 | action_right_, 463 | &blocks_surfaces_, 464 | false); 465 | } 466 | //set the table height 467 | else if (actionName == "set_table_height") 468 | { 469 | //TODO make a function 470 | if (blocks_surfaces_.size() > 0) 471 | { 472 | blocks_surfaces_.back().size_z_ = promptUserValue("Set the table height to"); 473 | blocks_surfaces_.back().pose_.position.z = 474 | floor_to_base_height_ + blocks_surfaces_.back().size_z_/2.0; 475 | blocks_surfaces_.back().publishBlock(¤t_scene_); 476 | } 477 | } 478 | //clean the scene 479 | else if (actionName == "table") 480 | { 481 | if (env_shown_) 482 | { 483 | //cleanObjects(&blocks_surfaces_, false); 484 | //cleanObjects(&blocks_); 485 | //env_shown_ = false; 486 | std::vector objects = getObjectsList(blocks_surfaces_); 487 | current_scene_.removeCollisionObjects(objects); 488 | env_shown_ = false; 489 | } 490 | else 491 | { 492 | //pub_obj_moveit_.publish(blocks_surfaces_.front().collObj_); 493 | //env_shown_ = true; 494 | if (blocks_surfaces_.size() > 0) 495 | { 496 | blocks_surfaces_.back().publishBlock(¤t_scene_); 497 | env_shown_ = true; 498 | } 499 | } 500 | } 501 | //moving the virtual object down 502 | else if (checkObj(block_id) && ((actionName == "x") || (actionName == "down"))) 503 | { 504 | geometry_msgs::Pose pose(block->pose_); 505 | pose.position.z -= 0.05; 506 | block->updatePose(¤t_scene_, pose); 507 | } 508 | //move the virtual object left 509 | else if (checkObj(block_id) && ((actionName == "s") || (actionName == "left"))) 510 | { 511 | geometry_msgs::Pose pose(block->pose_); 512 | pose.position.y -= 0.05; 513 | block->updatePose(¤t_scene_, pose); 514 | } 515 | //move the virtual object up 516 | else if (checkObj(block_id) && ((actionName == "e") || (actionName == "up"))) 517 | { 518 | geometry_msgs::Pose pose(block->pose_); 519 | pose.position.z += 0.05; 520 | block->updatePose(¤t_scene_, pose); 521 | } 522 | //move the virtual object right 523 | else if (checkObj(block_id) && ((actionName == "f") || (actionName == "right"))) 524 | { 525 | geometry_msgs::Pose pose(block->pose_); 526 | pose.position.y += 0.05; 527 | block->updatePose(¤t_scene_, pose); 528 | } 529 | //move the virtual object farther 530 | else if (checkObj(block_id) && ((actionName == "c") || (actionName == "farther"))) 531 | { 532 | geometry_msgs::Pose pose(block->pose_); 533 | pose.position.x += 0.05; 534 | block->updatePose(¤t_scene_, pose); 535 | } 536 | //move the virtual object closer 537 | else if (checkObj(block_id) && ((actionName == "r") || (actionName == "closer"))) 538 | { 539 | geometry_msgs::Pose pose(block->pose_); 540 | pose.position.x -= 0.05; 541 | block->updatePose(¤t_scene_, pose); 542 | } 543 | //set the tolerance 544 | else if (actionName == "j") 545 | action->setTolerance(promptUserValue("Set the value: ")); 546 | //switch the active arm 547 | else if (actionName == "switcharm") 548 | switchArm(action); 549 | //move the head down 550 | else if (actionName == "look_down") 551 | action->poseHeadDown(); 552 | //move the head to zero 553 | else if (actionName == "look_zero") 554 | action->poseHeadZero(); 555 | //print the statistics on grasps 556 | else if (actionName == "stat") 557 | { 558 | ROS_INFO_STREAM("Successfully grasped objects at the locations: "); 559 | std::vector ::iterator it = stat_poses_success_.begin(); 560 | for (; it != stat_poses_success_.end(); ++it) 561 | { 562 | ROS_INFO_STREAM(" [" << it->position.x << " " 563 | << it->position.y << " " 564 | << it->position.z << "] [" 565 | << it->orientation.x << " " 566 | << it->orientation.y << " " 567 | << it->orientation.z << " " 568 | << it->orientation.w << "]"); 569 | } 570 | } 571 | //print the statistics on grasps 572 | else if (actionName == "stat_evaluation") 573 | { 574 | evaluation_.printStat(); 575 | } 576 | 577 | if (actionName == "q") 578 | break; 579 | } 580 | } 581 | 582 | //clean the object list based on the timestamp 583 | void SimplePickPlace::cleanObjects(std::vector *objects, 584 | const bool list_erase) 585 | { 586 | std::vector objects_list = getObjectsOldList(objects); 587 | current_scene_.removeCollisionObjects(objects_list); 588 | 589 | //remove from the memory 590 | if (list_erase) 591 | objects->clear(); 592 | } 593 | 594 | void SimplePickPlace::resetBlock(MetaBlock *block) 595 | { 596 | // Remove attached object 597 | if(block->name_ != support_surface_) 598 | { 599 | action_left_->detachObject(block->name_); 600 | action_right_->detachObject(block->name_); 601 | } 602 | sleep(0.2); 603 | 604 | // Remove/Add collision object 605 | //visual_tools_->cleanupCO(block->name_); 606 | //visual_tools_->processCollisionObjectMsg(block->wrapToCollisionObject); 607 | 608 | block->publishBlock(¤t_scene_); 609 | } 610 | 611 | } //namespace 612 | -------------------------------------------------------------------------------- /src/action.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 SoftBank Robotics Europe 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "romeo_moveit_actions/action.hpp" 25 | 26 | namespace moveit_simple_actions 27 | { 28 | 29 | Action::Action(ros::NodeHandle *nh, 30 | const std::string &arm, 31 | const std::string &robot_name, 32 | const std::string &base_frame): 33 | verbose_(false), 34 | attempts_max_(3), 35 | planning_time_(20.0), //=5 in GUI 36 | planner_id_("RRTConnectkConfigDefault"), //RRTConnect 37 | tolerance_min_(0.03), //tested on Pepper, works from 0.03 38 | tolerance_step_(0.02), 39 | max_velocity_scaling_factor_(1.0), //=1.0 in GUI 40 | dist_th_(0.08f), 41 | flag_(FLAG_MOVE), 42 | end_eff_(arm+"_hand"), 43 | plan_group_(arm+"_arm"), 44 | posture_(robot_name, end_eff_, plan_group_), 45 | base_frame_(base_frame) 46 | { 47 | /* ROS_INFO_STREAM(<< "Planning group: " << plan_group_ 48 | ROS_INFO_STREAM("End Effector: " << end_eff_); 49 | ROS_INFO_STREAM("Planning Group: " << plan_group_);*/ 50 | 51 | nh->getParam("tolerance_min", tolerance_min_); 52 | 53 | if (robot_name == "nao") 54 | planning_time_ = 30.0; 55 | 56 | // Create MoveGroup for the planning group 57 | move_group_.reset(new move_group_interface::MoveGroup(plan_group_)); 58 | move_group_->setGoalTolerance(tolerance_min_); 59 | move_group_->setPlanningTime(planning_time_); 60 | move_group_->setPlannerId(planner_id_); 61 | move_group_->setNumPlanningAttempts(4); 62 | /*move_group_->setGoalPositionTolerance(0.1); //0.0001 63 | move_group_->setGoalOrientationTolerance(0.1); //0.001*/ 64 | 65 | // Load grasp generator 66 | if (!grasp_data_.loadRobotGraspData(*nh, end_eff_)) 67 | { 68 | ROS_ERROR("The grasp data cannot be loaded"); 69 | ros::shutdown(); 70 | } 71 | 72 | //generate grasps at PI/angle_resolution increments 73 | grasp_data_.angle_resolution_ = 30; 74 | /* grasp depth, when <= 0 the grasps is from other side 75 | * of the object or from bottom */ 76 | grasp_data_.grasp_depth_ = 0.01; 77 | /*grasp_data_.approach_retreat_desired_dist_ = 0.3; //0.2 //as bigger better grasp 78 | grasp_data_.approach_retreat_min_dist_ = 0.05; //0.06*/ 79 | /*std::cout << "grasp_data.approach_retreat_desired_dist_=" << grasp_data_.approach_retreat_desired_dist_ << std::endl 80 | << " grasp_data.approach_retreat_min_dist_=" << grasp_data_.approach_retreat_min_dist_ << std::endl 81 | << " grasp_data_.grasp_depth_= " << grasp_data_.grasp_depth_ << std::endl;*/ 82 | 83 | if (grasp_data_.pre_grasp_posture_.points.size() > 0) 84 | for (int i=0; i 0) 89 | posture_.initHandPose(grasp_data_.pre_grasp_posture_.points.at(0).positions[i], 0); 90 | } 91 | if (grasp_data_.grasp_posture_.joint_names[i].find("Hand") != std::string::npos) 92 | { 93 | if (grasp_data_.grasp_posture_.points.size() > 0) 94 | posture_.initHandPose(grasp_data_.grasp_posture_.points.at(0).positions[i], 1); 95 | } 96 | } 97 | 98 | // Load Grasp generator 99 | simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(moveit_visual_tools::MoveItVisualToolsPtr())); 100 | 101 | pub_obj_pose_ = nh->advertise("/pose_target", 10); 102 | 103 | pub_plan_pose_ = nh->advertise("/pose_plan", 10); 104 | pub_plan_traj_ = nh->advertise("/trajectory", 10); 105 | client_fk_ = nh->serviceClient("/compute_fk"); 106 | 107 | planning_scene_publisher_ = nh->advertise("/planning_scene", 1); 108 | 109 | planning_scene_client_ = nh->serviceClient("/get_planning_scene"); 110 | 111 | allowedCollisionLinks_ = move_group_->getRobotModel()->getJointModelGroup(grasp_data_.ee_group_)->getLinkModelNames(); 112 | allowedCollisionLinks_.push_back(grasp_data_.ee_parent_link_); 113 | allowedCollisionLinks_.push_back("l_wrist"); 114 | allowedCollisionLinks_.push_back("r_wrist"); 115 | for(int i=0; i grasps(1); 133 | 134 | moveit_msgs::Grasp g; 135 | g.grasp_pose.header.frame_id = block->name_; 136 | g.grasp_pose.pose = block->pose_; 137 | g.grasp_pose.pose = grasp_data_.grasp_pose_to_eef_pose_; 138 | 139 | g.pre_grasp_approach.direction.header.frame_id = grasp_data_.ee_parent_link_; 140 | g.pre_grasp_approach.direction.vector.x = 0; 141 | g.pre_grasp_approach.direction.vector.y = 0; 142 | g.pre_grasp_approach.direction.vector.z = -1; 143 | g.pre_grasp_approach.min_distance = 0.06; //0.01; 144 | g.pre_grasp_approach.desired_distance = 0.2; 145 | 146 | g.post_grasp_retreat.direction.header.frame_id = grasp_data_.ee_parent_link_; 147 | g.post_grasp_retreat.direction.vector.x = 0; 148 | g.post_grasp_retreat.direction.vector.y = 0; 149 | g.post_grasp_retreat.direction.vector.z = 1; // Retreat direction (pos z axis) 150 | g.post_grasp_retreat.min_distance = 0.06; 151 | g.post_grasp_retreat.desired_distance = 0.2; 152 | 153 | g.pre_grasp_posture.header.frame_id = grasp_data_.ee_parent_link_; 154 | g.grasp_posture.header.frame_id = grasp_data_.ee_parent_link_; 155 | if (grasp_data_.grasp_posture_.joint_names.size() > 0) 156 | { 157 | g.pre_grasp_posture.joint_names.resize(1); 158 | g.pre_grasp_posture.joint_names[0] = grasp_data_.grasp_posture_.joint_names[0]; 159 | g.pre_grasp_posture.points.resize(1); 160 | g.pre_grasp_posture.points[0].positions.resize(1); 161 | g.pre_grasp_posture.points[0].positions[0] = 0.0; 162 | 163 | g.grasp_posture.joint_names.resize(1); 164 | g.grasp_posture.joint_names[0] = g.pre_grasp_posture.joint_names[0]; 165 | g.grasp_posture.points.resize(1); 166 | g.grasp_posture.points[0].positions.resize(1); 167 | g.grasp_posture.points[0].positions[0] = 1.0; 168 | } 169 | std::cout << "-- pickDefault g " << g << std::endl; 170 | 171 | grasps[0] = g; 172 | 173 | // Prevent collision with table 174 | move_group_->setSupportSurfaceName(surface_name); 175 | 176 | /* an optional list of obstacles that 177 | * can be touched/pushed/moved in the course of grasping */ 178 | std::vector allowed_touch_objects; 179 | allowed_touch_objects.push_back(block->name_); 180 | 181 | // Add this list to all grasps 182 | for (std::size_t i = 0; i < grasps.size(); ++i) 183 | grasps[i].allowed_touch_objects = allowed_touch_objects; 184 | 185 | if (move_group_->pick(block->name_, grasps)){ 186 | done = true; 187 | 188 | return true; 189 | } 190 | sleep(0.7); 191 | //} 192 | } 193 | 194 | bool Action::executeAction() 195 | { 196 | bool success(false); 197 | 198 | if (verbose_) 199 | ROS_INFO_STREAM("Execution of the planned action"); 200 | 201 | if (!move_group_) 202 | return false; 203 | 204 | if (current_plan_ && flag_ == FLAG_MOVE) 205 | success = move_group_->execute(*current_plan_); 206 | 207 | if (verbose_ && success) 208 | ROS_INFO_STREAM("Execute success! \n\n"); 209 | 210 | // If the function is called and FLAG_NO_MOVE return true 211 | // because only call executeAction if plan is success 212 | return success || flag_ == FLAG_NO_MOVE; 213 | } 214 | 215 | bool Action::graspPlan(MetaBlock *block, const std::string surface_name) 216 | { 217 | bool success(false); 218 | 219 | if (verbose_) 220 | ROS_INFO_STREAM("Planning " << block->name_ 221 | << " at pose " << block->pose_); 222 | 223 | double tolerance_cur = move_group_->getGoalPositionTolerance(); 224 | move_group_->setGoalTolerance(0.1);//0.05 225 | 226 | // Prevent collision with table 227 | if (!surface_name.empty()) 228 | move_group_->setSupportSurfaceName(surface_name); 229 | 230 | if (!move_group_) 231 | return false; 232 | 233 | //move_group_->setApproximateJointValueTargets(target, move_group_->getEndEffectorLink().c_str()); 234 | move_group_->setPoseTargets(configureForPlanning(generateGrasps(block)), 235 | move_group_->getEndEffectorLink().c_str()); 236 | 237 | current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan()); 238 | success = move_group_->plan(*current_plan_); 239 | if (!success) 240 | current_plan_.reset(); 241 | else 242 | publishPlanInfo(*current_plan_, block->pose_); 243 | 244 | if (verbose_ && success) 245 | ROS_INFO_STREAM("Grasp planning success! \n\n"); 246 | 247 | move_group_->setGoalTolerance(tolerance_cur); 248 | 249 | return success; 250 | } 251 | 252 | geometry_msgs::PoseStamped Action::getGraspPose(MetaBlock *block) 253 | { 254 | geometry_msgs::PoseStamped goal; 255 | goal.header.stamp = ros::Time::now(); 256 | 257 | if (block->base_frame_ == base_frame_) 258 | { 259 | goal.header.frame_id = block->base_frame_; 260 | goal.pose = block->pose_; 261 | goal.pose.position.x += grasp_data_.grasp_pose_to_eef_pose_.position.x; 262 | goal.pose.position.y += grasp_data_.grasp_pose_to_eef_pose_.position.y; 263 | goal.pose.position.z -= grasp_data_.grasp_pose_to_eef_pose_.position.z; 264 | 265 | goal.pose.orientation = grasp_data_.grasp_pose_to_eef_pose_.orientation; 266 | } 267 | else 268 | { 269 | geometry_msgs::PoseStamped pose_transform = block->getTransformed(&listener_, base_frame_); 270 | goal.header.frame_id = base_frame_; 271 | 272 | goal.pose = pose_transform.pose; 273 | goal.pose.position.x += grasp_data_.grasp_pose_to_eef_pose_.position.x; 274 | goal.pose.position.y += grasp_data_.grasp_pose_to_eef_pose_.position.y; 275 | goal.pose.position.z += grasp_data_.grasp_pose_to_eef_pose_.position.z; 276 | } 277 | return goal; 278 | } 279 | 280 | float Action::computeDistance(MetaBlock *block) 281 | { 282 | geometry_msgs::PoseStamped goal = getGraspPose(block); 283 | 284 | float dist = computeDistance(goal.pose); 285 | 286 | return dist; 287 | } 288 | 289 | float Action::computeDistance(geometry_msgs::Pose goal) 290 | { 291 | geometry_msgs::Pose current(move_group_->getCurrentPose().pose); 292 | float dist = sqrt((goal.position.x - current.position.x) 293 | * (goal.position.x - current.position.x) 294 | + (goal.position.y - current.position.y) 295 | * (goal.position.y - current.position.y) 296 | + (goal.position.z - current.position.z) 297 | * (goal.position.z - current.position.z)); 298 | return dist; 299 | } 300 | 301 | bool Action::poseHeadZero() 302 | { 303 | return posture_.poseHeadZero(); 304 | } 305 | 306 | bool Action::poseHeadDown() 307 | { 308 | return posture_.poseHeadDown(); 309 | } 310 | 311 | bool Action::poseHand(const int pose_id) 312 | { 313 | double tolerance_cur = move_group_->getGoalPositionTolerance(); 314 | move_group_->setGoalTolerance(0.05); 315 | bool res = posture_.poseHand(end_eff_, plan_group_, pose_id); 316 | move_group_->setGoalTolerance(tolerance_cur); 317 | return res; 318 | } 319 | 320 | void Action::poseHandOpen() 321 | { 322 | posture_.poseHandOpen(end_eff_); 323 | } 324 | 325 | void Action::poseHandClose() 326 | { 327 | posture_.poseHandClose(end_eff_); 328 | } 329 | 330 | geometry_msgs::Pose Action::getPose() 331 | { 332 | geometry_msgs::PoseStamped pose_now; 333 | pose_now.header.stamp = ros::Time::now(); 334 | pose_now.header.frame_id = grasp_data_.base_link_; 335 | 336 | pose_now.pose = move_group_->getCurrentPose().pose; 337 | 338 | pose_now.pose.position.x -= grasp_data_.grasp_pose_to_eef_pose_.position.x; 339 | pose_now.pose.position.y -= grasp_data_.grasp_pose_to_eef_pose_.position.y; 340 | pose_now.pose.position.z -= grasp_data_.grasp_pose_to_eef_pose_.position.z; 341 | pose_now.pose.orientation = grasp_data_.grasp_pose_to_eef_pose_.orientation; 342 | //ROS_INFO_STREAM("current pose is " << pose_now); 343 | 344 | pub_obj_pose_.publish(pose_now); 345 | 346 | return pose_now.pose; 347 | } 348 | 349 | void Action::setTolerance(const double value) 350 | { 351 | move_group_->setGoalTolerance(value); 352 | if (verbose_) 353 | ROS_INFO_STREAM("The Goal Position Tolerance = " 354 | << move_group_->getGoalPositionTolerance()); 355 | } 356 | 357 | void Action::releaseObject(MetaBlock *block) 358 | { 359 | move_group_->detachObject(block->name_); 360 | 361 | //remove the collision temporally 362 | std::vector objects; 363 | objects.push_back(block->name_); 364 | current_scene_.removeCollisionObjects(objects); 365 | 366 | //go to the required pose 367 | poseHand(2); 368 | ros::Duration(1.0).sleep(); 369 | poseHandOpen(); 370 | 371 | //add the collision object back 372 | std::vector coll_objects; 373 | coll_objects.push_back(block->collObj_); 374 | current_scene_.addCollisionObjects(coll_objects); 375 | } 376 | 377 | bool Action::setAllowedMoveItCollisionMatrix(moveit_msgs::AllowedCollisionMatrix& m) 378 | { 379 | moveit_msgs::PlanningScene planning_scene; 380 | 381 | if (planning_scene_publisher_.getNumSubscribers() < 1) 382 | { 383 | ROS_ERROR("Setting collision matrix won't have any effect!"); 384 | return false; 385 | } 386 | planning_scene.is_diff = true; 387 | planning_scene.allowed_collision_matrix = m; 388 | planning_scene_publisher_.publish(planning_scene); 389 | return true; 390 | } 391 | 392 | bool Action::getCurrentMoveItAllowedCollisionMatrix(moveit_msgs::AllowedCollisionMatrix& matrix) 393 | { 394 | moveit_msgs::GetPlanningScene srv; 395 | 396 | srv.request.components.components = moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX; 397 | 398 | if (!planning_scene_client_.call(srv)) 399 | { 400 | ROS_ERROR("Can't obtain planning scene"); 401 | return false; 402 | } 403 | 404 | matrix = srv.response.scene.allowed_collision_matrix; 405 | if (matrix.entry_names.empty()) 406 | { 407 | ROS_ERROR("Collision matrix should not be empty"); 408 | return false; 409 | } 410 | 411 | //ROS_INFO_STREAM("Matrix: "<::iterator Action::ensureExistsInACM(const std::string& name, 416 | moveit_msgs::AllowedCollisionMatrix& m, 417 | bool initFlag) 418 | { 419 | std::vector::iterator name_entry = std::find(m.entry_names.begin(), m.entry_names.end(), name); 420 | if (name_entry == m.entry_names.end()) 421 | { 422 | ROS_DEBUG_STREAM("Could not find object " << name 423 | << " in collision matrix. Inserting."); 424 | expandMoveItCollisionMatrix(name, m, initFlag); 425 | // re-assign the 'name_entry' iterator to the new entry_names place 426 | name_entry = std::find(m.entry_names.begin(), m.entry_names.end(), name); 427 | if (name_entry == m.entry_names.end()) 428 | { 429 | ROS_ERROR("consistency, name should now be in map"); 430 | } 431 | } 432 | return name_entry; 433 | } 434 | 435 | void Action::expandMoveItCollisionMatrix(const std::string& name, 436 | moveit_msgs::AllowedCollisionMatrix& m, 437 | bool default_val) 438 | { 439 | 440 | for (int i = 0; i < m.entry_names.size(); ++i) 441 | { 442 | m.entry_values[i].enabled.push_back(default_val); 443 | } 444 | 445 | m.entry_names.push_back(name); 446 | 447 | moveit_msgs::AllowedCollisionEntry e; 448 | e.enabled.assign(m.entry_names.size(), default_val); 449 | m.entry_values.push_back(e); 450 | } 451 | 452 | void Action::updateCollisionMatrix(const std::string& name) 453 | { 454 | moveit_msgs::AllowedCollisionMatrix m; 455 | if (!getCurrentMoveItAllowedCollisionMatrix(m)) 456 | return; 457 | 458 | //ROS_INFO_STREAM("Allowed collisoin: " << block->name_); 459 | std::vector::iterator objEntry = ensureExistsInACM(name, m, false); 460 | int obj_idx = objEntry - m.entry_names.begin(); 461 | 462 | std::vector::const_iterator it; 463 | for (it = allowedCollisionLinks_.begin(); it != allowedCollisionLinks_.end(); ++it) 464 | { 465 | std::vector::iterator linkEntry = ensureExistsInACM(*it, m, false); 466 | int link_idx = linkEntry - m.entry_names.begin(); 467 | m.entry_values[link_idx].enabled[obj_idx] = true; 468 | m.entry_values[obj_idx].enabled[link_idx] = true; 469 | } 470 | setAllowedMoveItCollisionMatrix(m); 471 | } 472 | 473 | bool Action::reachGrasp(MetaBlock *block, 474 | const std::string surface_name, 475 | int attempts_nbr, 476 | float tolerance_min, 477 | double planning_time) 478 | { 479 | bool success(false); 480 | 481 | //if (verbose_) 482 | ROS_INFO_STREAM("Reaching at position = " 483 | << block->pose_.position.x << " " 484 | << block->pose_.position.y << " " 485 | << block->pose_.position.z); 486 | 487 | if (attempts_nbr == 0) 488 | attempts_nbr = attempts_max_; 489 | 490 | if (planning_time == 0.0) 491 | planning_time = planning_time_; 492 | 493 | if (tolerance_min == 0.0) 494 | tolerance_min = tolerance_min_; 495 | 496 | move_group_->setGoalTolerance(tolerance_min); 497 | move_group_->setPlanningTime(planning_time); 498 | 499 | updateCollisionMatrix(block->name_); 500 | 501 | geometry_msgs::PoseStamped pose = getGraspPose(block); 502 | ros::Duration(1.0).sleep(); 503 | 504 | //reach the object 505 | if (!reachAction(pose, surface_name, attempts_nbr)) 506 | return false; 507 | 508 | sleep(8.0); 509 | 510 | //compute the distance to the object 511 | float dist = computeDistance(pose.pose); 512 | if (verbose_) 513 | ROS_INFO_STREAM("Reached at distance = " << dist); 514 | 515 | //close the hand, if the object is close enough 516 | if (dist < dist_th_) 517 | { 518 | poseHandClose(); 519 | success = true; 520 | } 521 | 522 | //attach the object 523 | if (success) 524 | { 525 | move_group_->attachObject(block->name_, grasp_data_.ee_parent_link_); 526 | object_attached_ = block->name_; 527 | } 528 | 529 | //if (verbose_) 530 | ROS_INFO_STREAM("Distance to the object= " << dist 531 | << "; is grasped=" << success); 532 | 533 | return success; 534 | } 535 | 536 | bool Action::reachAction(geometry_msgs::PoseStamped pose_target, 537 | const std::string surface_name, 538 | const int attempts_nbr) 539 | { 540 | bool success(false); 541 | 542 | if (verbose_) 543 | ROS_INFO_STREAM("Planning to the pose " << pose_target); 544 | 545 | if (!move_group_) 546 | return false; 547 | 548 | current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan()); 549 | 550 | // Prevent collision with table 551 | if (!surface_name.empty()) 552 | move_group_->setSupportSurfaceName(surface_name); 553 | 554 | move_group_->setPoseTarget(pose_target, 555 | move_group_->getEndEffectorLink().c_str()); 556 | 557 | //publish the target pose 558 | pub_obj_pose_.publish(pose_target); 559 | 560 | move_group_->setMaxVelocityScalingFactor(max_velocity_scaling_factor_); //TODO back 561 | move_group_->setNumPlanningAttempts(10); //Seems to not improve the results 562 | double tolerance = tolerance_min_; 563 | int attempts = 0; 564 | 565 | //find a planning solution while increasing tolerance 566 | while (!success && (attempts < attempts_nbr)) 567 | { 568 | move_group_->setGoalTolerance(tolerance); 569 | success = move_group_->plan(*current_plan_); 570 | 571 | if (verbose_ && success) 572 | ROS_INFO_STREAM("Reaching success with tolerance " << tolerance << "\n\n"); 573 | 574 | if (!success) 575 | { 576 | tolerance += tolerance_step_; 577 | 578 | if (verbose_) 579 | ROS_INFO_STREAM("Planning retry with the tolerance " << tolerance); 580 | } 581 | ++attempts; 582 | } 583 | 584 | //find an approximate solution 585 | if (!success) 586 | { 587 | move_group_->setApproximateJointValueTarget(pose_target, 588 | move_group_->getEndEffectorLink().c_str()); 589 | success = move_group_->plan(*current_plan_); 590 | if (verbose_ && success) 591 | ROS_INFO_STREAM("Reaching success with approximate joint value"); 592 | } 593 | 594 | if (success) 595 | { 596 | //publishPlanInfo(*current_plan_, pose_target.pose); 597 | success = executeAction(); 598 | } 599 | else 600 | current_plan_.reset(); 601 | 602 | return success; 603 | } 604 | 605 | bool Action::graspPlanAllPossible(MetaBlock *block, 606 | const std::string surface_name) 607 | { 608 | bool success(false); 609 | 610 | if (verbose_) 611 | ROS_INFO_STREAM("Planning all possible grasps to " << block->pose_); 612 | 613 | // Prevent collision with table 614 | if (!surface_name.empty()) 615 | move_group_->setSupportSurfaceName(surface_name); 616 | 617 | std::vector targets = 618 | configureForPlanning(generateGrasps(block)); 619 | 620 | moveit::planning_interface::MoveGroup::Plan plan; 621 | 622 | if (targets.size() == 0) 623 | return false; 624 | 625 | double tolerance_cur = move_group_->getGoalPositionTolerance(); 626 | move_group_->setGoalTolerance(0.1); 627 | 628 | int counts = 0; 629 | std::vector::iterator it=targets.begin(); 630 | for (; it!=targets.end();++it) 631 | { 632 | //move_group_->setPoseTarget(*it, move_group_->getEndEffectorLink().c_str()); 633 | //move_group_->setPositionTarget(it->position.x, it->position.y, it->position.z, move_group_->getEndEffectorLink().c_str()); 634 | success = move_group_->setApproximateJointValueTarget(*it, move_group_->getEndEffectorLink().c_str()); 635 | success = move_group_->plan(plan); 636 | if (success) 637 | ++counts; 638 | } 639 | if (verbose_) 640 | ROS_INFO_STREAM( "Planning success for " 641 | << counts << " generated poses! \n\n"); 642 | 643 | move_group_->setGoalTolerance(tolerance_cur); 644 | 645 | return success; 646 | } 647 | 648 | std::vector Action::generateGrasps(MetaBlock *block) 649 | { 650 | std::vector grasps; 651 | if (block->name_.empty()) 652 | { 653 | ROS_INFO_STREAM("No object choosen to grasp"); 654 | return grasps; 655 | } 656 | 657 | if (verbose_) 658 | visual_tools_->deleteAllMarkers(); 659 | 660 | geometry_msgs::Pose pose = block->pose_; 661 | pose.position.z += block->size_y_/2.0; 662 | simple_grasps_->generateBlockGrasps(pose, grasp_data_, grasps ); 663 | 664 | /*if (verbose_) 665 | { 666 | double speed = 0.01; 667 | visual_tools_->publishGrasps(grasps, grasp_data_.ee_parent_link_, speed); 668 | visual_tools_->deleteAllMarkers(); 669 | sleep(0.5); 670 | }*/ 671 | 672 | if (grasps.size() > 0) 673 | { 674 | /* an optional list of obstacles that we have semantic information about 675 | * and that can be touched/pushed/moved in the course of grasping */ 676 | std::vector allowed_touch_objects(1); 677 | allowed_touch_objects[0] = block->name_; 678 | for (std::size_t i = 0; i < grasps.size(); ++i) 679 | grasps[i].allowed_touch_objects = allowed_touch_objects; 680 | } 681 | 682 | return grasps; 683 | } 684 | 685 | std::vector Action::configureForPlanning( 686 | const std::vector &grasps) 687 | { 688 | std::vector targets(grasps.size()); 689 | 690 | if (grasps.size() > 0) 691 | { 692 | std::vector::const_iterator it_grasp = grasps.begin(); 693 | std::vector::iterator it_pose = targets.begin(); 694 | for (; it_grasp!=grasps.end(); ++it_grasp, ++it_pose) 695 | { 696 | *it_pose = it_grasp->grasp_pose.pose; 697 | } 698 | } 699 | 700 | return targets; 701 | } 702 | 703 | bool Action::pickAction(MetaBlock *block, 704 | const std::string surface_name, 705 | int attempts_nbr, 706 | double planning_time) 707 | { 708 | bool success(false); 709 | 710 | //if (verbose_) 711 | ROS_INFO_STREAM("Pick at pose " 712 | << block->pose_.position.x << " " 713 | << block->pose_.position.y << " " 714 | << block->pose_.position.z); 715 | 716 | if (attempts_nbr == 0) 717 | attempts_nbr = attempts_max_; 718 | 719 | if (planning_time == 0.0) 720 | planning_time = planning_time_; 721 | 722 | std::vector grasps = generateGrasps(block); 723 | 724 | if (grasps.size() == 0) 725 | return false; 726 | 727 | // Prevent collision with table 728 | if (!surface_name.empty()) 729 | move_group_->setSupportSurfaceName(surface_name); 730 | 731 | move_group_->setPlanningTime(planning_time); 732 | 733 | double tolerance = tolerance_min_; 734 | int attempts = 0; 735 | 736 | //find a planning solution while increasing tolerance 737 | while (!success && (attempts < attempts_nbr)) 738 | { 739 | move_group_->setGoalTolerance(tolerance); 740 | success = move_group_->pick(block->name_, grasps); 741 | 742 | if (!success) 743 | { 744 | tolerance += tolerance_step_; 745 | 746 | if (verbose_) 747 | ROS_INFO_STREAM("Try planning with the tolerance " << tolerance); 748 | } 749 | ++attempts; 750 | } 751 | 752 | move_group_->setGoalTolerance(tolerance_min_); 753 | 754 | if (verbose_ & success) 755 | ROS_INFO_STREAM("Pick success with tolerance " << tolerance << "\n\n"); 756 | 757 | return success; 758 | } 759 | 760 | bool Action::placeAction(MetaBlock *block, 761 | const std::string surface_name) 762 | { 763 | if (verbose_) 764 | ROS_INFO_STREAM("Placing " << block->name_ 765 | << " at pose " << block->goal_pose_); 766 | 767 | // Prevent collision with table 768 | if (!surface_name.empty()) 769 | move_group_->setSupportSurfaceName(surface_name); 770 | 771 | std::vector place_locations; 772 | 773 | // Re-usable datastruct 774 | geometry_msgs::PoseStamped pose_stamped; 775 | pose_stamped.header.frame_id = grasp_data_.base_link_; 776 | pose_stamped.header.stamp = ros::Time::now(); 777 | 778 | // Create 360 degrees of place location rotated around a center 779 | //for (double angle = 0; angle < 2*M_PI; angle += M_PI/2) 780 | //{ 781 | pose_stamped.pose = block->goal_pose_; 782 | 783 | // Create new place location 784 | moveit_msgs::PlaceLocation place_loc; 785 | place_loc.place_pose = pose_stamped; 786 | 787 | // Approach 788 | moveit_msgs::GripperTranslation pre_place_approach; 789 | pre_place_approach.direction.header.stamp = ros::Time::now(); 790 | // The distance the origin of a robot link needs to travel 791 | pre_place_approach.desired_distance = grasp_data_.approach_retreat_desired_dist_; 792 | // half of the desired? Untested. 793 | pre_place_approach.min_distance = grasp_data_.approach_retreat_min_dist_; 794 | pre_place_approach.direction.header.frame_id = grasp_data_.base_link_; 795 | pre_place_approach.direction.vector.x = 0; 796 | pre_place_approach.direction.vector.y = 0; 797 | //Approach direction (negative z axis) 798 | pre_place_approach.direction.vector.z = 0.1; //-1 799 | place_loc.pre_place_approach = pre_place_approach; 800 | 801 | // Retreat 802 | /*moveit_msgs::GripperTranslation post_place_retreat(pre_place_approach); 803 | post_place_retreat.direction.vector.x = 0; 804 | post_place_retreat.direction.vector.y = 0; 805 | post_place_retreat.direction.vector.z = -1; //1 // Retreat direction (pos z axis) 806 | place_loc.post_place_retreat = post_place_retreat;*/ 807 | // Post place posture - use same as pre-grasp posture (the OPEN command) 808 | place_loc.post_place_posture = grasp_data_.pre_grasp_posture_; 809 | 810 | place_locations.push_back(place_loc); 811 | //} 812 | 813 | //to test 814 | move_group_->setStartState(*move_group_->getCurrentState()); 815 | 816 | bool success = move_group_->place(block->name_, place_locations); 817 | if (verbose_) 818 | { 819 | if (success) 820 | ROS_INFO_STREAM("Place success! \n\n"); 821 | else 822 | ROS_ERROR_STREAM_NAMED("simple_actions:","Place failed."); 823 | } 824 | 825 | return success; 826 | } 827 | 828 | void Action::publishPlanInfo(moveit::planning_interface::MoveGroup::Plan plan, 829 | geometry_msgs::Pose pose_target) 830 | { 831 | // Get the last position of the trajectory plan and transform that joints values 832 | // into pose of end effector in /base_link frame 833 | // Then that is published to /pose_plan and the trajectory to /trajectory topics 834 | //TODO: Check if using directly robotStatePtr changes the real robot 835 | int num_points = plan.trajectory_.joint_trajectory.points.size(); 836 | moveit::core::RobotStatePtr robotStatePtr = move_group_->getCurrentState(); 837 | robotStatePtr->setJointGroupPositions(plan_group_, plan.trajectory_.joint_trajectory.points[num_points-1].positions); 838 | moveit_msgs::RobotState robotStateMsg; 839 | moveit::core::robotStateToRobotStateMsg(*robotStatePtr, robotStateMsg); 840 | 841 | std::vector links_vect; 842 | links_vect.push_back(move_group_->getEndEffectorLink()); 843 | 844 | moveit_msgs::GetPositionFK srv; 845 | srv.request.header.frame_id = "/base_link"; //grasp_data_.ee_parent_link_; //TODO: To check 846 | srv.request.fk_link_names = links_vect; 847 | srv.request.robot_state = robotStateMsg; 848 | 849 | if (client_fk_.call(srv)) 850 | { 851 | if(srv.response.pose_stamped.size() > 0) 852 | { 853 | int eef_index = srv.response.fk_link_names.size() - 1; 854 | 855 | pub_plan_pose_.publish(srv.response.pose_stamped[eef_index]); 856 | 857 | if(verbose_) 858 | { 859 | // Compute the distance between the last pose of the trajectory plan 860 | // and the target pose 861 | double x_target = pose_target.position.x; 862 | double y_target = pose_target.position.y; 863 | double z_target = pose_target.position.z; 864 | 865 | double x_pose = srv.response.pose_stamped[eef_index].pose.position.x; 866 | double y_pose = srv.response.pose_stamped[eef_index].pose.position.y; 867 | double z_pose = srv.response.pose_stamped[eef_index].pose.position.z; 868 | 869 | double error = sqrt(pow(x_target-x_pose,2) 870 | + pow(y_target-y_pose,2) 871 | + pow(z_target-z_pose,2)); 872 | 873 | ROS_INFO_STREAM("Distance of last trajectory pose from target pose: " 874 | << error << " meters"); 875 | } 876 | } 877 | else 878 | { 879 | ROS_WARN_STREAM("No result of service /compute_fk \nMoveitCodeError: " 880 | << srv.response.error_code); 881 | } 882 | } 883 | else 884 | { 885 | ROS_WARN("Failed to call service /compute_fk"); 886 | } 887 | pub_plan_traj_.publish(plan.trajectory_); 888 | } 889 | 890 | void Action::setPlanningTime(const double value) 891 | { 892 | planning_time_ = value; 893 | move_group_->setPlanningTime(value); 894 | if(verbose_) 895 | ROS_INFO_STREAM("Planning time set to " << value); 896 | } 897 | 898 | void Action::setToleranceStep(const double value) 899 | { 900 | tolerance_step_ = value; 901 | if(verbose_) 902 | ROS_INFO_STREAM("Tolerance step set to " << value); 903 | } 904 | 905 | void Action::setToleranceMin(const double value) 906 | { 907 | tolerance_min_ = value; 908 | if(verbose_) 909 | ROS_INFO_STREAM("Tolerance min set to " << value); 910 | } 911 | 912 | void Action::setMaxVelocityScalingFactor(const double value) 913 | { 914 | max_velocity_scaling_factor_ = value; 915 | move_group_->setMaxVelocityScalingFactor(max_velocity_scaling_factor_); 916 | if(verbose_) 917 | ROS_INFO_STREAM("Max velocity scaling factor set to " << value); 918 | } 919 | 920 | void Action::setVerbose(bool verbose) 921 | { 922 | verbose_ = verbose; 923 | if(verbose_) 924 | ROS_INFO_STREAM("Verbose set to " << verbose); 925 | } 926 | 927 | void Action::setAttemptsMax(int value) 928 | { 929 | attempts_max_ = value; 930 | if(verbose_) 931 | ROS_INFO_STREAM("Attempts max set to " << value); 932 | } 933 | 934 | void Action::setFlag(int flag) 935 | { 936 | if(flag == FLAG_MOVE || flag == FLAG_NO_MOVE) 937 | { 938 | flag_ = flag; 939 | if(verbose_) 940 | ROS_INFO_STREAM("Flag set to " << flag); 941 | } 942 | else 943 | ROS_WARN_STREAM("No value: " << flag 944 | << " for flag, will remain as: " << flag_); 945 | } 946 | 947 | void Action::detachObject(const std::string &block_name) 948 | { 949 | move_group_->detachObject(block_name); 950 | } 951 | 952 | void Action::attachObject(const std::string &block_name) 953 | { 954 | move_group_->attachObject(block_name, grasp_data_.ee_group_); 955 | } 956 | 957 | std::string Action::getBaseLink() 958 | { 959 | return grasp_data_.base_link_; 960 | } 961 | } 962 | --------------------------------------------------------------------------------