void onInit() {
NODELET_INFO("Auto white balancing initialising");
ros::NodeHandle &nh = getNodeHandle();
it.reset(new image_transport::ImageTransport(nh));
sub = it->subscribe("in", 1, &Whitebalancing::ballancing, this);
// The positions of the reference area relative to the image size
nh.param("vertical_startposition", vertical_startposition, 0.5);
nh.param("vertical_endposition", vertical_endposition, 0.65);
nh.param("horizontal_startposition", horizontal_startposition, 0.94);
nh.param("horizontal_endposition", horizontal_endposition, 1.0);
// only read the image every 10 published images
nh.param("wait_images",count,10);
nh.param("cor_blue",cor_blue,1.0);
nh.param("cor_red",cor_red,1.0);
// intial white balancing settings
white_bal_BU=700;
white_bal_RU=512;
// Using system calls for dynamic reconfigure.
system("rosrun dynamic_reconfigure dynparam set /viz/camera1394_driver white_balance_BU 700");
sleep(0.5);
system("rosrun dynamic_reconfigure dynparam set /viz/camera1394_driver white_balance_RV 512");
sleep(0.5);
NODELET_INFO("Auto white balancing started");
}
void loadCalibrationData()
{
ros::NodeHandle& pn = getPrivateNodeHandle();
std::string calibration_file;
// Try to load a specific calibration file (if requested)
if (pn.getParam("calibration_file", calibration_file))
{
if (camera_->loadCalibrationData(calibration_file))
{
NODELET_INFO("Loaded calibration data from \"%s\"", calibration_file.c_str());
return;
}
else
{
NODELET_WARN("Failed to load calibration data from \"%s\"", calibration_file.c_str());
}
}
// Check whether the calibration data was loaded from the default location
if (camera_->isCalibrationDataLoaded())
{
NODELET_INFO("Loaded calibration data from default location (\"%s.dat\" in the working directory)", camera_->getSerialNumber().c_str());
}
else
{
NODELET_WARN("Will use default calibration data");
}
}
void ImageProcessing::initPublishersAndSubscribers(){
double vital_rate;
pnh_->param("vital_rate", vital_rate, 1.0);
image_vital_.reset(
new jsk_topic_tools::VitalChecker(1 / vital_rate));
info_vital_.reset(
new jsk_topic_tools::VitalChecker(1 / vital_rate));
it_ = new image_transport::ImageTransport(*pnh_);
std::string img = nh_->resolveName("image");
std::string cam = nh_->resolveName("camera");
if (img.at(0) == '/') {
img.erase(0, 1);
}
NODELET_INFO("camera = %s", cam.c_str());
NODELET_INFO("image = %s", img.c_str());
width_scale_pub_ = advertise<std_msgs::Float32>(*pnh_, "output/width_scale", max_queue_size_);
height_scale_pub_ = advertise<std_msgs::Float32>(*pnh_, "output/height_scale", max_queue_size_);
if (use_snapshot_) {
publish_once_ = false;
srv_ = pnh_->advertiseService("snapshot", &ImageProcessing::snapshot_srv_cb, this);
}
if (use_camera_info_) {
cp_ = advertiseCamera(*pnh_, *it_, "output/image", max_queue_size_);
}
else {
image_pub_ = advertise<sensor_msgs::Image>(*pnh_, "output/image", max_queue_size_);
}
}
void loadCalibrationData()
{
ros::NodeHandle& pn = getPrivateNodeHandle();
std::string calibration_file;
// Try to load a specific calibration file (if requested)
if (pn.getParam("calibration_file", calibration_file) && !calibration_file.empty())
{
NODELET_INFO("Trying to load calibration from \"%s\"", calibration_file.c_str());
try
{
boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
camera_->loadCalibrationData(calibration_file);
NODELET_INFO("Loaded calibration data from \"%s\"", calibration_file.c_str());
return;
}
catch(PMDCameraNotOpenedException& e)
{
NODELET_WARN("Failed to load calibration data from \"%s\"", calibration_file.c_str());
}
}
// Check whether the calibration data was loaded from the default location
if (camera_->isCalibrationDataLoaded())
{
NODELET_INFO("Loaded calibration data from default location (\"%s.dat\" in the working directory)", camera_->getSerialNumber().c_str());
}
else
{
NODELET_WARN("Will use default calibration data");
}
}
void controller_nodelet::onInit()
{
NODELET_INFO("Initialising controller nodelet");
ros::NodeHandle node = getMTNodeHandle();
/* Get differential base parameters */
node.param<float>("diffbase/wheelbase", wheelbase, 0.194f);
node.param<float>("diffbase/right_wheel/radius", right_wheel_radius, 0.016f);
node.param<float>("diffbase/left_wheel/radius", left_wheel_radius, 0.016f);
node.param<int>("diffbase/right_wheel/direction", right_wheel_direction, 1);
node.param<int>("diffbase/left_wheel/direction", left_wheel_direction, 1);
node.param<int>("diffbase/external_to_internal_wheelbase_encoder_direction",
external_to_internal_wheelbase_encoder_direction, 1);
/* Initialise controller */
right_setpt_pub = node.advertise
<cvra_msgs::MotorControlSetpoint>("right_wheel/setpoint", 10);
left_setpt_pub = node.advertise
<cvra_msgs::MotorControlSetpoint>("left_wheel/setpoint", 10);
cmdvel_sub = node.subscribe("cmd_vel", 10,
&goldorak_base::controller_nodelet::cmdvel_cb, this);
NODELET_INFO("Controller nodelet ready");
}
void HiwrCameraControllerNodelet::configureSpinning(ros::NodeHandle& nh){
NODELET_INFO("[UVCCam Nodelet] START config_uring spinning state");
ros::NodeHandle& ph = getMTPrivateNodeHandle();
service_spinning_state_setter_ = ph.advertiseService("setSpinningState", &HiwrCameraControllerNodelet::serviceSetSpinningState, this);
service_spinning_state_getter_ = ph.advertiseService("getSpinningState", &HiwrCameraControllerNodelet::serviceGetSpinningState, this);
NODELET_INFO("[UVCCam Nodelet] DONE");
}
void PlaneSupportedCuboidEstimator::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_INFO("cloudCallback");
if (!latest_polygon_msg_ || !latest_coefficients_msg_) {
JSK_NODELET_WARN("Not yet polygon is available");
return;
}
if (!tracker_) {
NODELET_INFO("initTracker");
// Update polygons_ vector
polygons_.clear();
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygons_.push_back(polygon);
}
// viewpoint
tf::StampedTransform transform
= lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
ros::Time(0.0),
ros::Duration(0.0));
tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
ParticleCloud::Ptr particles = initParticles();
tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
tracker_->setParticles(particles);
tracker_->setParticleNum(particle_num_);
support_plane_updated_ = false;
// Publish histograms
publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
// Publish particles
sensor_msgs::PointCloud2 ros_particles;
pcl::toROSMsg(*particles, ros_particles);
ros_particles.header = msg->header;
pub_particles_.publish(ros_particles);
}
else {
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() > fast_cloud_threshold_) {
estimate(msg);
}
}
}
void HiwrCameraControllerNodelet::applyNewConfig(Config &new_config, uint32_t level)
{
printf("### dynamic reconfig_ure level 0x%x \n", level);
ROS_DEBUG("dynamic reconfig_ure level 0x%x", level);
reconfig_number_++;
// resolve frame ID using tf_prefix parameter
if (new_config.frame_id == "")
new_config.frame_id = "camera";
ROS_DEBUG("dynamic reconfig_ure level 0x%x", level);
std::string tf_prefix = tf::getPrefixParam(private_nh_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
new_config.frame_id = tf::resolve(tf_prefix, new_config.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
if (openCamera(new_config))
{
// update camera name string
new_config.camera_name = camera_name_;
}
}
if(reconfig_number_ == 1 || config_.focus_auto != new_config.focus_auto){
try {
NODELET_INFO("will try to set autofocus to %d \n " , new_config.focus_auto);
cam_->set_control( 0x009A090C , new_config.focus_auto);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting focus_auto. Exception was " << e.what());
}
}
if(reconfig_number_ == 1 || config_.focus_absolute != new_config.focus_absolute){
try {
NODELET_INFO("will try to set focus_absolute to %d \n " , new_config.focus_absolute);
cam_->set_control( 0x009A090A , new_config.focus_absolute);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting focus_absolute. Exception was " << e.what() << "value was" << new_config.focus_absolute ) ;
}
}
config_ = new_config;
config_width_ = new_config.width;
config_height_ = new_config.height;
next_config_update_ = false;
printf("### dynamic reconfig_ure will unlock\n");
}
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));
}
virtual ~MVCameraNodelet()
{
if (running_)
{
NODELET_INFO("shutting down driver thread");
running_ = false;
deviceThread_->join();
NODELET_INFO("driver thread stopped");
}
dvr_->shutdown();
}
~Bumblebee1394Nodelet()
{
if (running_)
{
NODELET_INFO("shutting down driver thread");
running_ = false;
deviceThread_->join();
NODELET_INFO("driver thread stopped");
}
dvr_->shutdown();
}
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);
}
void ScanToPointCloud::onInit() {
NODELET_INFO("ScanToPointCloud::onInit: Initializing...");
#ifdef USE_MT_NODE_HANDLE
ros::NodeHandle &nh = getMTNodeHandle();
ros::NodeHandle &pnh = getMTPrivateNodeHandle();
#else
ros::NodeHandle &nh = getNodeHandle();
ros::NodeHandle &pnh = getPrivateNodeHandle();
#endif
// Process parameters.
pnh.param("target_frame", targetFrame, targetFrame);
NODELET_INFO("ScanToPointCloud::onInit: Using target frame: %s.", targetFrame.data());
pnh.param("wait_for_transform", waitForTransform, waitForTransform);
NODELET_INFO("ScanToPointCloud::onInit: Maximum time to wait for transform: %.2f s.", waitForTransform);
pnh.param("channel_options", channelOptions, channelOptions);
NODELET_INFO("ScanToPointCloud::onInit: Channel options: %#x.", channelOptions);
pnh.param("scan_queue", scanQueue, scanQueue);
NODELET_INFO("ScanToPointCloud::onInit: Scan queue size: %i.", scanQueue);
pnh.param("point_cloud_queue", pointCloudQueue, pointCloudQueue);
NODELET_INFO("ScanToPointCloud::onInit: Point cloud queue size: %i.", pointCloudQueue);
// Subscribe scan topic.
std::string scanTopic = nh.resolveName("scan", true);
NODELET_INFO("ScanToPointCloud::onInit: Subscribing scan %s.", scanTopic.data());
scanSubscriber = nh.subscribe<sensor_msgs::LaserScan>(scanTopic, scanQueue, &ScanToPointCloud::scanCallback, this);
// Advertise scan point cloud.
std::string cloudTopic = nh.resolveName("cloud", true);
NODELET_INFO("ScanToPointCloud::onInit: Advertising point cloud %s.", cloudTopic.data());
pointCloudPublisher = nh.advertise<sensor_msgs::PointCloud2>(cloudTopic, pointCloudQueue, false);
}
void DepthCalibration::printModel()
{
NODELET_INFO("C2(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
coefficients2_[0], coefficients2_[1], coefficients2_[2], coefficients2_[3], coefficients2_[4]);
NODELET_INFO("C1(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
coefficients1_[0], coefficients1_[1], coefficients1_[2], coefficients1_[3], coefficients1_[4]);
NODELET_INFO("C0(u, v) = %fu^2 + %fu + %fv^2 + %fv + %f",
coefficients0_[0], coefficients0_[1], coefficients0_[2], coefficients0_[3], coefficients0_[4]);
if (use_abs_) {
NODELET_INFO("use_abs: True");
}
else {
NODELET_INFO("use_abs: False");
}
}
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(private_nh, "rgb");
ros::NodeHandle depth_pnh(private_nh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
int queueSize = 10;
bool approxSync = true;
if(private_nh.getParam("max_rate", rate_))
{
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
}
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
imagePub_ = rgb_it.advertise("image_out", 1);
imageDepthPub_ = depth_it.advertise("image_out", 1);
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
};
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
NODELET_INFO( "visual_odometry: reset odom!");
odometry_->reset();
this->flushCallbacks();
return true;
}
void FindObjectOnPlane::generateStartPoints(
const cv::Point2f& point_2d,
const image_geometry::PinholeCameraModel& model,
const pcl::ModelCoefficients::Ptr& coefficients,
std::vector<cv::Point3f>& search_points_3d,
std::vector<cv::Point2f>& search_points_2d)
{
NODELET_INFO("generateStartPoints");
jsk_recognition_utils::Plane::Ptr plane
(new jsk_recognition_utils::Plane(coefficients->values));
cv::Point3d ray = model.projectPixelTo3dRay(point_2d);
Eigen::Vector3f projected_point = rayPlaneInteersect(ray, plane);
const double resolution_step = 0.01;
const int resolution = 5;
search_points_3d.clear();
search_points_2d.clear();
for (int i = - resolution; i < resolution; ++i) {
for (int j = - resolution; j < resolution; ++j) {
double x_diff = resolution_step * i;
double y_diff = resolution_step * j;
Eigen::Vector3f moved_point = projected_point + Eigen::Vector3f(x_diff, y_diff, 0);
Eigen::Vector3f projected_moved_point;
plane->project(moved_point, projected_moved_point);
cv::Point3f projected_moved_point_cv(projected_moved_point[0],
projected_moved_point[1],
projected_moved_point[2]);
search_points_3d.push_back(cv::Point3f(projected_moved_point_cv));
cv::Point2d p2d = model.project3dToPixel(projected_moved_point_cv);
search_points_2d.push_back(p2d);
}
}
}
void callback(const std_msgs::Float64::ConstPtr& input)
{
std_msgs::Float64Ptr output(new std_msgs::Float64());
output->data = input->data + value_;
NODELET_INFO("subscriber number: %d + Adding %f to get %f", pub.getNumSubscribers(), value_, output->data);
//pub.publish(output);
}
void BarcodeReaderNodelet::connectCb()
{
if (!camera_sub_ && barcode_pub_.getNumSubscribers() > 0)
{
NODELET_INFO("Connecting to camera topic.");
camera_sub_ = nh_.subscribe("/usb_cam/image_raw", 10, &BarcodeReaderNodelet::imageCb, this);
}
}
bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros::ResetPose::Response&)
{
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose);
this->flushCallbacks();
return true;
}
void BarcodeReaderNodelet::disconnectCb()
{
if (barcode_pub_.getNumSubscribers() == 0)
{
NODELET_INFO("Unsubscribing from camera topic.");
camera_sub_.shutdown();
}
}
void ColorHistogramMatcher::referenceHistogram(
const jsk_pcl_ros::ColorHistogram::ConstPtr& input_histogram)
{
boost::mutex::scoped_lock(mutex_);
NODELET_INFO("update reference");
reference_histogram_ = input_histogram->histogram;
reference_histogram_pub_.publish(input_histogram);
reference_set_ = true;
}
void VisualOdometryNodelet::onInit()
{
NODELET_INFO("Initializing Visual Odometry Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getNodeHandle();
ros::NodeHandle nh_private = getPrivateNodeHandle();
visual_odometry_.reset(new VisualOdometry(nh, nh_private));
}
void QuadJoyTeleopNodelet::onInit ()
{
NODELET_INFO("Initializing QuadJoyTeleop Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getMTNodeHandle();
ros::NodeHandle nh_private = getMTPrivateNodeHandle();
quad_joy_teleop_ = new QuadJoyTeleop(nh, nh_private);
}
void LaserScanSplitterNodelet::onInit ()
{
NODELET_INFO("Initializing LaserScanSplitter Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getMTNodeHandle();
ros::NodeHandle nh_private = getMTPrivateNodeHandle();
laser_scan_splitter_ = new LaserScanSplitter(nh, nh_private);
}
void asctec::AutoPilotNodelet::onInit ()
{
NODELET_INFO("Initializing Autopilot Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getMTNodeHandle();
ros::NodeHandle nh_private = getMTPrivateNodeHandle();
autopilot = new asctec::AutoPilot(nh, nh_private);
}
void RGBDImageProcNodelet::onInit()
{
NODELET_INFO("Initializing RGBD Image Proc Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getMTNodeHandle();
ros::NodeHandle nh_private = getMTPrivateNodeHandle();
rgbd_image_proc_ = new RGBDImageProc(nh, nh_private);
}
void ImageResizer::config_cb ( ImageResizerConfig &config, uint32_t level) {
NODELET_INFO("config_cb");
resize_x_ = config.resize_scale_x;
resize_y_ = config.resize_scale_y;
period_ = ros::Duration(1.0/config.msg_par_second);
verbose_ = config.verbose;
NODELET_DEBUG("resize_scale_x : %f", resize_x_);
NODELET_DEBUG("resize_scale_y : %f", resize_y_);
NODELET_DEBUG("message period : %f", period_.toSec());
}
void LaserOrthoProjectorNodelet::onInit ()
{
NODELET_INFO("Initializing LaserOrthoProjector Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getMTNodeHandle();
ros::NodeHandle nh_private = getMTPrivateNodeHandle();
laser_ortho_projector_ = new scan_tools::LaserOrthoProjector(nh, nh_private);
}
void PhidgetsImuNodelet::onInit()
{
NODELET_INFO("Initializing Phidgets IMU Nodelet");
// TODO: Do we want the single threaded or multithreaded NH?
ros::NodeHandle nh = getMTNodeHandle();
ros::NodeHandle nh_private = getMTPrivateNodeHandle();
imu_ = new ImuRosI(nh, nh_private);
}
