void onInitImpl()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& pn = getPrivateNodeHandle();
// Retrieve parameters from server
std::string device_serial;
double open_camera_retry_period;
double update_rate;
pn.param<std::string>("frame_id", frame_id_, "/camera_optical_frame");
pn.param<std::string>("device_serial", device_serial, "");
pn.param<double>("open_camera_retry_period", open_camera_retry_period, 3);
pn.param<double>("update_rate", update_rate, 30);
pn.param<bool>("points_with_amplitudes", points_with_amplitudes_, false);
// Open camera
while (!camera_)
{
try
{
camera_ = boost::make_shared<PMDCamboardNano>(device_serial);
NODELET_INFO("Opened PMD camera with serial number \"%s\"", camera_->getSerialNumber().c_str());
loadCalibrationData();
camera_->update();
camera_info_ = camera_->getCameraInfo();
}
catch (PMDCameraNotOpenedException& e)
{
if (device_serial != "")
NODELET_INFO("Unable to open PMD camera with serial number %s...", device_serial.c_str());
else
NODELET_INFO("Unable to open PMD camera...");
}
boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period));
}
// Advertise topics
ros::NodeHandle distance_nh(nh, "distance");
image_transport::ImageTransport distance_it(distance_nh);
distance_publisher_ = distance_it.advertiseCamera("image", 1);
ros::NodeHandle depth_nh(nh, "depth");
image_transport::ImageTransport depth_it(depth_nh);
depth_publisher_ = depth_it.advertiseCamera("image", 1);
ros::NodeHandle amplitude_nh(nh, "amplitude");
image_transport::ImageTransport amplitude_it(amplitude_nh);
amplitude_publisher_ = amplitude_it.advertiseCamera("image", 1);
points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points", 1);
// Setup periodic callback to get new data from the camera
update_timer_ = nh.createTimer(ros::Rate(update_rate).expectedCycleTime(), &DriverNodelet::updateCallback, this);
// Setup dynamic reconfigure server
reconfigure_server_.reset(new ReconfigureServer(pn));
reconfigure_server_->setCallback(boost::bind(&DriverNodelet::reconfigureCallback, this, _1, _2));
}
void onInitImpl()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& pn = getPrivateNodeHandle();
// Retrieve parameters from server
update_rate=30;
pn.param<std::string>("frame_id", frame_id_, "/camera_optical_frame");
NODELET_INFO("Loaded param frame_id: %s", frame_id_.c_str());
pn.param<std::string>("device_serial", device_serial, "");
NODELET_INFO("Loaded param device_serial: %s", device_serial.c_str());
pn.param<double>("open_camera_retry_period", open_camera_retry_period, 3.);
pn.param<std::string>("plugin_dir", plugin_dir, "/usr/local/pmd/plugins");
NODELET_INFO("Loaded param plugin_dir: %s", plugin_dir.c_str());
pn.param<std::string>("source_plugin", source_plugin, "camboardnano");
NODELET_INFO("Loaded param source_plugin: %s", source_plugin.c_str());
pn.param<std::string>("process_plugin", process_plugin, "camboardnanoproc");
NODELET_INFO("Loaded param process_plugin: %s", process_plugin.c_str());
// Setup updater
camera_state_ = OPENING;
state_info_ = "opening camera " + device_serial;
updater.setHardwareIDf("%s", device_serial.c_str());
if(device_serial.empty())
{
updater.setHardwareID("unknown");
}
updater.add(getName().c_str(), this, &DriverNodelet::getCurrentState);
// Setup periodic callback to get new data from the camera
update_timer_ = nh.createTimer(ros::Rate(update_rate).expectedCycleTime(), &DriverNodelet::updateCallback, this, false ,false);
// Open camera
openCamera(update_timer_);
// Advertise topics
ros::NodeHandle distance_nh(nh, "distance");
image_transport::ImageTransport distance_it(distance_nh);
distance_publisher_ = distance_it.advertiseCamera("image", 1);
ros::NodeHandle depth_nh(nh, "depth");
image_transport::ImageTransport depth_it(depth_nh);
depth_publisher_ = depth_it.advertiseCamera("image", 1);
ros::NodeHandle amplitude_nh(nh, "amplitude");
image_transport::ImageTransport amplitude_it(amplitude_nh);
amplitude_publisher_ = amplitude_it.advertiseCamera("image", 1);
points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points_unrectified", 1);
// Setup dynamic reconfigure server
reconfigure_server_.reset(new ReconfigureServer(config_mutex_, pn));
ReconfigureServer::CallbackType f = boost::bind(&DriverNodelet::reconfigureCallback, this, _1, _2);
reconfigure_server_->setCallback(f);
}