//! Points the high-def camera frame at a point in a given frame void lookAt(std::string frame_id, double x, double y, double z) { //the goal message we will be sending pr2_controllers_msgs::PointHeadGoal goal; //the target point, expressed in the requested frame geometry_msgs::PointStamped point; point.header.frame_id = frame_id; point.point.x = x; point.point.y = y; point.point.z = z; goal.target = point; //we are pointing the high-def camera frame //(pointing_axis defaults to X-axis) goal.pointing_frame = "high_def_frame"; //take at least 0.5 seconds to get there goal.min_duration = ros::Duration(0.5); //and go no faster than 1 rad/s goal.max_velocity = 1.0; //send the goal point_head_client_->sendGoal(goal); //wait for it to get there (abort after 2 secs to prevent getting stuck) point_head_client_->waitForResult(ros::Duration(2)); }
//! Action client initialization RobotHead() { //Initialize the client for the Action interface to the head controller point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true); //wait for head controller action server to come up while(!point_head_client_->waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the point_head_action server to come up"); } }
void move_head (std::string frame_id, double x, double y, double z) { //the goal message we will be sending pr2_controllers_msgs::PointHeadGoal goal; //the target point, expressed in the requested frame geometry_msgs::PointStamped point; point.header.frame_id = frame_id; point.point.x = x; point.point.y = y; point.point.z = z; goal.target = point; //we are pointing the wide_stereo camera frame //(pointing_axis defaults to X-axis) goal.pointing_frame = "narrow_stereo_optical_frame"; //take at least 2 seconds to get there goal.min_duration = ros::Duration(1); //and go no faster than 0.1 rad/s goal.max_velocity = 0.5; //send the goal point_head_client_->sendGoal(goal); //wait for it to get there bool finished_within_time = point_head_client_->waitForResult(ros::Duration(20.0)); if (!finished_within_time) { ROS_ERROR("Head did not move to pose: %f %f %f", point.point.x, point.point.y, point.point.z); } else { actionlib::SimpleClientGoalState state = point_head_client_->getState(); bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); if(success) ROS_INFO("Action move head finished: %s position: %f %f %f", state.toString().c_str(), point.point.x, point.point.y, point.point.z); else ROS_ERROR("Action move head failed: %s", state.toString().c_str()); } }
bool pointHead (race_msgs::BoundingBox bb, float z=1.0) { bool success = true; //the goal message we will be sending pr2_controllers_msgs::PointHeadGoal goal; //the point to be looking at is expressed in the "base_link" frame geometry_msgs::PointStamped point; //point.header.frame_id = bb.pose_stamped.header.frame_id; //point.point.x = bb.pose_stamped.pose.position.x; //point.point.y = bb.pose_stamped.pose.position.y; point.header.frame_id = "/base_link"; point.point.x = 1.0; point.point.y = 0.0; point.point.z = z; goal.target = point; //we want the X axis of the camera frame to be pointing at the target goal.pointing_frame = "high_def_frame"; goal.pointing_axis.x = 1; goal.pointing_axis.y = 0; goal.pointing_axis.z = 0; //take at least 0.5 seconds to get there goal.min_duration = ros::Duration(0.5); //and go no faster than 1 rad/s goal.max_velocity = 1.0; //send the goal point_head_client_->sendGoal(goal); //wait for it to get there (abort after 2 secs to prevent getting stuck) point_head_client_->waitForResult(ros::Duration(2)); return success; }
PointCloudCapturer(ros::NodeHandle &n) : nh_(n), synchronizer_( MySyncPolicy(1), cloud_sub_, camera_sub_) { nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/kinect_head/camera/rgb/points")); nh_.param("input_image_topic", input_image_topic_, std::string("/kinect_head/camera/rgb/image_color")); nh_.param("input_camera_info_topic", input_camera_info_topic_, std::string("/kinect_head/camera/rgb/camera_info")); // cloud_sub_= nh_.subscribe (input_cloud_topic_, 10, &PointCloudCapturer::cloud_cb, this); camera_sub_.subscribe( nh_, input_image_topic_, 1000 ); cloud_sub_.subscribe( nh_, input_cloud_topic_, 1000 ); sync_connection_ = synchronizer_.registerCallback( &PointCloudCapturer::callback, this ); ROS_INFO("[PointCloudColorizer:] Subscribing to cloud topic %s", input_cloud_topic_.c_str()); point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true); //wait for head controller action server to come up while(!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok()) { ROS_INFO("Waiting for the point_head_action server to come up"); } cloud_and_image_received_ = false; nh_.param("move_offset_y_min", move_offset_y_min_, -1.5); nh_.param("move_offset_y_max", move_offset_y_max_, 1.5); nh_.param("step_y", step_y_, 0.3); nh_.param("move_offset_z_min", move_offset_z_min_, 0.3); nh_.param("move_offset_z_max", move_offset_z_max_, 1.5); nh_.param("step_z", step_z_, 0.3); nh_.param("move_offset_x", move_offset_x_, 1.0); nh_.param("bag_name", bag_name_, std::string("bosch_kitchen_tr.bag")); nh_.param("to_frame", to_frame_, std::string("base_link")); nh_.param("rate", rate_, 1.0); current_position_x_ = move_offset_x_; current_position_y_ = move_offset_y_min_; current_position_z_ = move_offset_z_min_; move_head("base_link", current_position_x_, current_position_y_, current_position_z_); EPS = 1e-5; //thread ROS spinner spin_thread_ = boost::thread (boost::bind (&ros::spin)); bag_.open(bag_name_, rosbag::bagmode::Write); }
WaitUntilUnblockedAction (std::string name) : as_(nh_, name, boost::bind(&WaitUntilUnblockedAction::executeCB, this, _1), false), action_name_(name) { // Point head //Initialize the client for the Action interface to the head controller point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true); //wait for head controller action server to come up while(!point_head_client_->waitForServer(ros::Duration(5.0))) { ROS_INFO("Waiting for the point_head_action server to come up"); } // human detection activated = false; human_detected = false; obstacle_detected = false; marker_pub = nh_.advertise<visualization_msgs::Marker>("obstacle_boundingbox", 1); as_.start(); }