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