//! 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();
}
