Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef JOINTSPACEARMCONTROLLER_H_
00010 #define JOINTSPACEARMCONTROLLER_H_
00011
00012 #include <ros/ros.h>
00013 #include <vector>
00014 #include <string>
00015 #include <actionlib/client/simple_action_client.h>
00016 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00017
00018 #include <actionlib/client/simple_action_client.h>
00019 #include <arm_navigation_msgs/MoveArmAction.h>
00020
00021
00022 #include <arm_navigation_msgs/CollisionOperation.h>
00023 #include <arm_navigation_msgs/OrderedCollisionOperations.h>
00024 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00025 #include <arm_navigation_msgs/GetMotionPlan.h>
00026
00027 #include <tf/transform_listener.h>
00028 #include <arm_navigation_msgs/GetPlanningScene.h>
00029 #include <planning_environment/models/collision_models.h>
00030 #include <coverage_3d_arm_navigation/TrajectoryPlanner.h>
00031 using namespace std;
00032
00033 class JointSpaceArmController {
00034
00035 public:
00036 JointSpaceArmController(ros::NodeHandle& nh);
00037 ~JointSpaceArmController();
00038
00039 void followTrajectory(const std::vector<std::vector<double> >& joint_traj,const std::vector<tf::Pose>& pose_traj, const std::string arm);
00040 void followEntireTrajectory(const std::vector<std::vector<double> >& path, const std::string arm,
00041 const std::vector<unsigned int>&action ,const std::vector<tf::Pose>& pose_path, std::vector<tf::Pose>& touch_points_allowed);
00042 bool followEntireTrajectory(const std::vector<std::vector<double> >& path, const std::string arm,
00043 const std::vector<unsigned int>&action ,const std::vector<tf::Pose>& pose_path, std::vector<tf::Pose>& touch_points_allowed, std::vector<tf::Pose>& poses_wide_above_surface);
00044 bool moveArmToPoseGoalUsingPlanner(const std::string arm, const tf::Pose& pose,
00045 const bool collision_check);
00046 bool moveArmToJointGoalUsingPlanner(const std::string arm, const std::vector<double> & state,
00047 const bool collision_check, std::vector<tf::Pose>& touch_points_allowed);
00048 private:
00049 ros::NodeHandle nh_;
00050 std::vector<double> start_r_arm_;
00051 std::vector<std::string> r_arm_joints_;
00052 std::vector<std::string> l_arm_joints_;
00053 actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* traj_client_;
00054 TrajectoryPlanner traj_planner_;
00055 ros::ServiceClient filter_trajectory_client_;
00056 ros::ServiceClient reset_collider_client_;
00057 };
00058 #endif