Go to the documentation of this file.00001
00011 #ifndef INTERACTIVE_WORLD_HIGH_LEVEL_ACTIONS_H_
00012 #define INTERACTIVE_WORLD_HIGH_LEVEL_ACTIONS_H_
00013
00014 #include <actionlib/client/simple_action_client.h>
00015 #include <actionlib/server/simple_action_server.h>
00016 #include <geometry_msgs/TransformStamped.h>
00017 #include <interactive_world_msgs/DriveAndSearchAction.h>
00018 #include <interactive_world_msgs/DriveToSurfaceAction.h>
00019 #include <interactive_world_msgs/FindSurface.h>
00020 #include <interactive_world_msgs/GetSurfaces.h>
00021 #include <interactive_world_msgs/TransformToSurfaceFrame.h>
00022 #include <interactive_world_tools/World.h>
00023 #include <move_base_msgs/MoveBaseAction.h>
00024 #include <rail_manipulation_msgs/ArmAction.h>
00025 #include <rail_manipulation_msgs/SegmentedObjectList.h>
00026 #include <ros/ros.h>
00027 #include <tf2/LinearMath/Transform.h>
00028 #include <tf2_ros/transform_listener.h>
00029
00030 #include <boost/thread/mutex.hpp>
00031
00032 namespace rail
00033 {
00034 namespace interactive_world
00035 {
00036
00043 class HighLevelActions
00044 {
00045 public:
00047 static const int AC_WAIT_TIME = 60;
00049 static const double QUATERNION_90_ROTATE = 0.7071067811865476;
00051 static const double SURFACE_Z_THRESH = 0.1;
00052
00059 HighLevelActions();
00060
00068 bool okay() const;
00069
00070 private:
00080 bool findSurfaceCallback(interactive_world_msgs::FindSurface::Request &req,
00081 interactive_world_msgs::FindSurface::Response &resp);
00082
00083 bool transformToSurfaceFrame(interactive_world_msgs::TransformToSurfaceFrame::Request &req,
00084 interactive_world_msgs::TransformToSurfaceFrame::Response &resp);
00085
00086 bool getSurfacesCallback(interactive_world_msgs::GetSurfaces::Request &req,
00087 interactive_world_msgs::GetSurfaces::Response &resp);
00088
00096 void driveAndSearch(const interactive_world_msgs::DriveAndSearchGoalConstPtr &goal);
00097
00105 void driveToSurface(const interactive_world_msgs::DriveToSurfaceGoalConstPtr &goal);
00106
00115 bool driveHelper(const std::string &frame_id);
00116
00125 tf2::Transform tfFromTFMessage(const geometry_msgs::Transform &tf);
00126
00135 tf2::Transform tfFromPoseMessage(const geometry_msgs::Pose &pose);
00136
00144 void recognizedObjectsCallback(const rail_manipulation_msgs::SegmentedObjectList &objects);
00145
00147 bool debug_, okay_;
00149 ros::NodeHandle node_, private_node_;
00151 std::string fixed_frame_;
00153 World world_;
00155 std::map<std::string, geometry_msgs::TransformStamped> static_tfs_;
00157 actionlib::SimpleActionServer<interactive_world_msgs::DriveAndSearchAction> drive_and_search_as_;
00159 actionlib::SimpleActionServer<interactive_world_msgs::DriveToSurfaceAction> drive_to_surface_as_;
00161 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> nav_ac_;
00163 actionlib::SimpleActionClient<rail_manipulation_msgs::ArmAction> home_arm_ac_;
00165 ros::ServiceClient look_at_frame_srv_, segment_srv_;
00167 ros::ServiceServer find_surface_srv_, transform_to_surface_frame_srv_, get_surfaces_srv_;
00169 ros::Subscriber recognized_objects_sub_;
00171 ros::Duration ac_wait_time_;
00173 boost::mutex nav_mutex_, recognized_objects_mutex_;
00175 rail_manipulation_msgs::SegmentedObjectList recognized_objects_;
00177 uint32_t recognized_objects_counter_;
00178 };
00179
00180 }
00181 }
00182
00183 #endif