void SensorHandlerRGBDCamera::registerCallback() {
SensorRGBDCamera* s = dynamic_cast<SensorRGBDCamera*>(_sensor);
_depth_sub = new message_filters::Subscriber<sensor_msgs::Image>(*_nh, s->getDepthTopic(), 5);
_intensity_sub = new message_filters::Subscriber<sensor_msgs::Image>(*_nh, s->getIntensityTopic(), 5);
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
_sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10), *_intensity_sub, *_depth_sub);
_sync->registerCallback(boost::bind(&SensorHandlerRGBDCamera::callback, this, _1, _2));
std::cout << "Subscribed to topics: " << std::endl;
std::cout << "\t" << s->getDepthTopic() << std::endl;
std::cout << "\t" << s->getIntensityTopic() << std::endl;
}
VisualOdometry() :
frameId_("base_link"),
image_mono_sub_(nh_, "image", 1),
image_depth_sub_(nh_, "image_depth", 1),
info_sub_(nh_, "camera_info", 1),
sync_(MySyncPolicy(5), image_mono_sub_, image_depth_sub_, info_sub_)
{
pose_.setIdentity();
sync_.registerCallback(boost::bind(&VisualOdometry::callback, this, _1, _2, _3));
odomPub_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);
ros::NodeHandle pnh("~");
pnh.param("frame_id", frameId_, frameId_);
resetSrv_ = pnh.advertiseService("reset", &VisualOdometry::reset, this);
}
MappingNode::MappingNode()
: it_(nh_), depth_image_sub_( it_, "/camera/depth/image_raw", 1 ),
thermal_image_sub_( it_, "/flir_camera/temperature_image" , 1 ), sync( MySyncPolicy( 10 ), depth_image_sub_, thermal_image_sub_)
{
// ###########################################################################################
// ## ##
// ## /flir_camera/image_raw => 16UC1, RAW, contains Kelvin, unsigend short ##
// ## ##
// ## /flir_camera/temperature_image => 32FC1, RAW, contains Celsius, float ##
// ## ##
// ## /flir_camera/thermal_image_raw => BRG8, Colored, uchar ##
// ## ##
// ###########################################################################################
// Read the params from the launch file
ros::NodeHandle privateNH("~");
privateNH.param("package_name", package_name_, std::string("maskor_thermal_mapper"));
privateNH.param("database_path", database_path_, std::string("/database"));
privateNH.param("yaml_path", yaml_path_, std::string("/full_flir_ir_stereo_calib.yaml"));
privateNH.param("publish_topic", publish_topic_, std::string("flir_camera/mapped/image"));
privateNH.param("enable_colormap", enable_colormap_, bool(true));
// Path to package
pkg_path = ros::package::getPath(package_name_);
std::cout << "Open package = " << pkg_path << std::endl;
// Path to yaml file store
DATABASE = pkg_path + database_path_;
std::cout << "Open database = " << DATABASE << std::endl;
calibration_filename = DATABASE + yaml_path_;
std::cout << "Open calibrationfile = " << calibration_filename << std::endl;
// Publisher
ThermalImagePub_ = it_.advertise(publish_topic_,1);
// Synchronised callback
sync.registerCallback( boost::bind( &MappingNode::callback, this, _1, _2 ) );
}
GazeSimulation::GazeSimulation(const std::string & name, const ros::NodeHandle & nh) : Gaze(name,nh)
{
private_node_handle.param<std::string>("left_eye_frame", left_eye_frame, "left_eye_frame");
private_node_handle.param<std::string>("right_eye_frame", right_eye_frame, "right_eye_frame");
private_node_handle.param<std::string>("neck_frame", neck_frame, "neck_frame");
private_node_handle.param<std::string>("head_origin_frame", head_origin_frame, "head_origin_frame");
private_node_handle.param<std::string>("eyes_center_frame", eyes_center_frame, "eyes_center_frame");
private_node_handle.param<std::string>("world_frame", world_frame, "world_frame");
// Publishers
neck_pan_pub= nh_.advertise<std_msgs::Float64>("/vizzy/neck_pan_position_controller/command", 1);
neck_tilt_pub=nh_.advertise<std_msgs::Float64>("/vizzy/neck_tilt_position_controller/command", 1);
eyes_tilt_pub=nh_.advertise<std_msgs::Float64>("/vizzy/eyes_tilt_position_controller/command", 1);
l_eye_pub= nh_.advertise<std_msgs::Float64>("/vizzy/l_eye_position_controller/command", 1);
r_eye_pub= nh_.advertise<std_msgs::Float64>("/vizzy/r_eye_position_controller/command", 1);
ROS_INFO("Going to move head and eyes to home position.");
moveHome();
sleep(5.0); // THIS SHOULD CHANGE
ROS_INFO("Done.");
tf::StampedTransform transform;
while(nh_.ok())
{
try
{
tf_listener->waitForTransform(head_origin_frame, neck_frame, ros::Time(0), ros::Duration(10.0) );
tf_listener->lookupTransform(head_origin_frame, neck_frame, ros::Time(0), transform);
tf::Vector3 origin;
origin=transform.getOrigin();
y_offset=origin.getY();
tf_listener->waitForTransform(eyes_center_frame, head_origin_frame, ros::Time(0), ros::Duration(10.0) );
tf_listener->lookupTransform(eyes_center_frame, head_origin_frame, ros::Time(0), transform);
origin=transform.getOrigin();
z_offset=origin.getZ();
}
catch (tf::TransformException &ex)
{
ROS_WARN("%s",ex.what());
continue;
}
break;
}
// Subscribers
neck_pan_sub=boost::shared_ptr<message_filters::Subscriber<control_msgs::JointControllerState> > (new message_filters::Subscriber<control_msgs::JointControllerState>(nh_, "/vizzy/neck_pan_position_controller/state", 10));
neck_tilt_sub=boost::shared_ptr<message_filters::Subscriber<control_msgs::JointControllerState> > (new message_filters::Subscriber<control_msgs::JointControllerState>(nh_, "/vizzy/neck_tilt_position_controller/state", 10));
eyes_tilt_sub=boost::shared_ptr<message_filters::Subscriber<control_msgs::JointControllerState> > (new message_filters::Subscriber<control_msgs::JointControllerState>(nh_, "/vizzy/eyes_tilt_position_controller/state", 10));
fixation_point_sub=boost::shared_ptr<message_filters::Subscriber<geometry_msgs::PointStamped> > (new message_filters::Subscriber<geometry_msgs::PointStamped>(nh_, "fixation_point", 10));
sync=boost::shared_ptr<message_filters::Synchronizer<MySyncPolicy> > (new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),
*neck_pan_sub,
*neck_tilt_sub,
*eyes_tilt_sub,
*fixation_point_sub));
sync->registerCallback(boost::bind(&GazeSimulation::analysisCB, this, _1, _2, _3, _4));
as_.registerGoalCallback(boost::bind(&Gaze::goalCB, this));
as_.registerPreemptCallback(boost::bind(&Gaze::preemptCB, this));
as_.start();
}