4 #include <std_msgs/Float64MultiArray.h> 6 #include <geometry_msgs/PoseStamped.h> 8 #include <pr2_controllers_msgs/PointHeadAction.h> 10 #include <std_srvs/SetBool.h> 16 void callback(
const geometry_msgs::PoseStamped::ConstPtr& pose);
56 int main(
int argc,
char **argv) {
57 ros::init(argc, argv,
"pan_tilt_object_trackking");
68 point_head_client =
new PointHeadClient(
"/pan_tilt_trajectory_controller/point_head_action",
true);
72 ROS_INFO(
"Waiting for the point_head_action server to come up");
79 ROS_INFO(
"Waiting for update_collision service...");
81 std_srvs::SetBool srv;
82 srv.request.data=
true;
87 std::vector<std::string> ids;
88 ids.push_back(object_name);
89 std::map<std::string, moveit_msgs::CollisionObject> poses=planning_scene_interface.getObjects(ids);
90 std::map<std::string, moveit_msgs::CollisionObject>::iterator it;
92 it = poses.find(object_name);
94 if (it != poses.end()) {
95 moveit_msgs::CollisionObject obj=it->second;
97 pr2_controllers_msgs::PointHeadGoal goal;
101 geometry_msgs::PointStamped point;
103 point.header.frame_id = obj.header.frame_id;
104 point.header.stamp=obj.header.stamp;
105 point.point = obj.primitive_poses[0].position;
111 goal.pointing_frame =
"kinect2_depth_frame";
112 goal.pointing_axis.x = 1;
113 goal.pointing_axis.y = 0;
114 goal.pointing_axis.z = 0;
120 goal.max_velocity = 0.2;
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
int main(int argc, char **argv)
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
void callback(const geometry_msgs::PoseStamped::ConstPtr &pose)
actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction > PointHeadClient
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
PointHeadClient * point_head_client
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
ROSCPP_DECL void spinOnce()