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 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");
}
/*!
* \brief Serves as a psuedo constructor for nodelets.
*
* This function needs to do the MINIMUM amount of work to get the nodelet running. Nodelets should not call blocking functions here.
*/
void onInit()
{
// Get nodeHandles
ros::NodeHandle &nh = getMTNodeHandle();
ros::NodeHandle &pnh = getMTPrivateNodeHandle();
// Get a serial number through ros
int serial;
pnh.param<int>("serial", serial, 0);
pg_.setDesiredCamera((uint32_t)serial);
// Get the location of our camera config yaml
std::string camera_info_url;
pnh.param<std::string>("camera_info_url", camera_info_url, "");
// Get the desired frame_id, set to 'camera' if not found
pnh.param<std::string>("frame_id", frame_id_, "camera");
// Do not call the connectCb function until after we are done initializing.
boost::mutex::scoped_lock scopedLock(connect_mutex_);
// Start up the dynamic_reconfigure service, note that this needs to stick around after this function ends
srv_ = boost::make_shared <dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig> > (pnh);
dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig>::CallbackType f = boost::bind(&pointgrey_camera_driver::PointGreyCameraNodelet::paramCallback, this, _1, _2);
srv_->setCallback(f);
// Start the camera info manager and attempt to load any configurations
std::stringstream cinfo_name;
cinfo_name << serial;
cinfo_.reset(new camera_info_manager::CameraInfoManager(nh, cinfo_name.str(), camera_info_url));
// Publish topics using ImageTransport through camera_info_manager (gives cool things like compression)
it_.reset(new image_transport::ImageTransport(nh));
image_transport::SubscriberStatusCallback cb = boost::bind(&PointGreyCameraNodelet::connectCb, this);
it_pub_ = it_->advertiseCamera("image_raw", 5, cb, cb);
// Set up diagnostics
updater_.setHardwareID("pointgrey_camera " + cinfo_name.str());
// Set up a diagnosed publisher
double desired_freq;
pnh.param<double>("desired_freq", desired_freq, 7.0);
pnh.param<double>("min_freq", min_freq_, desired_freq);
pnh.param<double>("max_freq", max_freq_, desired_freq);
double freq_tolerance; // Tolerance before stating error on publish frequency, fractional percent of desired frequencies.
pnh.param<double>("freq_tolerance", freq_tolerance, 0.1);
int window_size; // Number of samples to consider in frequency
pnh.param<int>("window_size", window_size, 100);
double min_acceptable; // The minimum publishing delay (in seconds) before warning. Negative values mean future dated messages.
pnh.param<double>("min_acceptable_delay", min_acceptable, 0.0);
double max_acceptable; // The maximum publishing delay (in seconds) before warning.
pnh.param<double>("max_acceptable_delay", max_acceptable, 0.2);
ros::SubscriberStatusCallback cb2 = boost::bind(&PointGreyCameraNodelet::connectCb, this);
pub_.reset(new diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage>(nh.advertise<wfov_camera_msgs::WFOVImage>("image", 5, cb2, cb2),
updater_,
diagnostic_updater::FrequencyStatusParam(&min_freq_, &max_freq_, freq_tolerance, window_size),
diagnostic_updater::TimeStampStatusParam(min_acceptable, max_acceptable)));
}
void spin ()
{
trackerClient_ = boost::shared_ptr<visp_tracker::TrackerClient>
(new visp_tracker::TrackerClient
(getMTNodeHandle (),
getMTPrivateNodeHandle (),
exiting_, 5u));
if (ros::ok () && !exiting_)
trackerClient_->spin ();
}
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 spin ()
{
trackerViewer_ = boost::shared_ptr<visp_tracker::TrackerViewer>
(new visp_tracker::TrackerViewer
(getMTNodeHandle (),
getMTPrivateNodeHandle (),
exiting_, 5u));
while (ros::ok () && !exiting_)
trackerViewer_->spin ();
}
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 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);
}
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 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 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 onInit()
{
nh = getMTNodeHandle();
nh_priv = getMTPrivateNodeHandle();
// Parameters
nh_priv.param("use_backup_estimator_alt", use_backup_estimator_alt, use_backup_estimator_alt);
nh_priv.param("imu_to_kinect_offset", imu_to_kinect_offset, imu_to_kinect_offset);
nh_priv.param("max_est_kinect_delta_alt", max_est_kinect_delta_alt, max_est_kinect_delta_alt);
nh_priv.param("obstacle_height_threshold", obstacle_height_threshold, obstacle_height_threshold);
nh_priv.param("debug_throttle_rate", debug_throttle_rate, debug_throttle_rate);
nh_priv.param("z_vel_filt_a", z_vel_filt_a, z_vel_filt_a);
nh_priv.param("z_vel_filt_b", z_vel_filt_b, z_vel_filt_b);
// Publishers
odom_pub = nh_priv.advertise<nav_msgs::Odometry> ("output", 10);
obstacle_pub = nh_priv.advertise<starmac_kinect::Obstacle> ("obstacle", 10);
debug_pub = nh_priv.advertise<starmac_kinect::Debug> ("debug", 10);
marker_pub = nh_priv.advertise<visualization_msgs::Marker> ("marker", 10);
mask_indices_pub = nh.advertise<pcl::PointIndices> ("mask_indices", 10);
findplane_indices_pub = nh.advertise<pcl::PointIndices> ("findplane_indices", 10, true);
// Subscribers
sub_input_filter.subscribe(nh_priv, "input", max_queue_size);
sub_indices_filter.subscribe(nh_priv, "indices", max_queue_size);
sub_model_filter.subscribe(nh_priv, "model", max_queue_size);
sync_input_indices_e
= make_shared<Synchronizer<ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size);
sync_input_indices_e->connectInput(sub_input_filter, sub_indices_filter, sub_model_filter);
sync_input_indices_e->registerCallback(bind(&KinectEstimator::cloudIndicesModelCallback, this, _1, _2, _3));
// Fill out most of the output Odometry message (we'll only be filling in the z pose value)
odom_msg_output.child_frame_id = "imu";
odom_msg_output.header.frame_id = "ned";
odom_msg_output.pose.pose.orientation.x = 0;
odom_msg_output.pose.pose.orientation.y = 0;
odom_msg_output.pose.pose.orientation.z = 0;
odom_msg_output.pose.pose.orientation.w = 1;
odom_msg_output.pose.pose.position.x = 0;
odom_msg_output.pose.pose.position.y = 0;
if (use_backup_estimator_alt)
{
est_odom_sub = nh.subscribe("estimator/output", 1, &KinectEstimator::estOdomCallback, this,
ros::TransportHints().tcpNoDelay());
}
updateMask(); // force once to start things
}
void
pcl_ros::PCDWriter::onInit ()
{
ros::NodeHandle pnh = getMTPrivateNodeHandle ();
sub_input_ = pnh.subscribe ("input", 1, &PCDWriter::input_callback, this);
// ---[ Optional parameters
pnh.getParam ("filename", file_name_);
pnh.getParam ("binary_mode", binary_mode_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - filename : %s\n"
" - binary_mode : %s",
getName ().c_str (),
file_name_.c_str (), (binary_mode_) ? "true" : "false");
}
void
pcl_ros::BAGReader::onInit ()
{
boost::shared_ptr<ros::NodeHandle> pnh_;
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
// ---[ Mandatory parameters
if (!pnh_->getParam ("file_name", file_name_))
{
NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
return;
}
if (!pnh_->getParam ("topic_name", topic_name_))
{
NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!");
return;
}
// ---[ Optional parameters
int max_queue_size = 1;
pnh_->getParam ("publish_rate", publish_rate_);
pnh_->getParam ("max_queue_size", max_queue_size);
ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);
NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
" - file_name : %s\n"
" - topic_name : %s",
file_name_.c_str (), topic_name_.c_str ());
if (!open (file_name_, topic_name_))
return;
PointCloud output;
output_ = boost::make_shared<PointCloud> (output);
output_->header.stamp = ros::Time::now ();
// Continous publishing enabled?
while (pnh_->ok ())
{
PointCloudConstPtr cloud = getNextCloud ();
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
output_->header.stamp = ros::Time::now ();
pub_output.publish (output_);
ros::Duration (publish_rate_).sleep ();
ros::spinOnce ();
}
}
/*********************************************************
* @brief OnInit method from node handle.
* OnInit method from node handle. Sets up the parameters
* and topics.
*********************************************************/
virtual void onInit()
{
ROS_ERROR("envir: hello from onInit()");
ros::NodeHandle& nh = getMTNodeHandle();
ros::NodeHandle& private_nh = getMTPrivateNodeHandle();
private_nh.getParam("min_y", min_y_);
private_nh.getParam("max_y", max_y_);
private_nh.getParam("min_x", min_x_);
private_nh.getParam("max_x", max_x_);
private_nh.getParam("max_z", max_z_);
private_nh.getParam("goal_z", goal_z_);
private_nh.getParam("z_scale", z_scale_);
private_nh.getParam("x_scale", x_scale_);
private_nh.getParam("enabled", enabled_);
receivedRectCoord = false;
ul_x = ul_y = lr_x = lr_y = 0;
focal_length_ = 575.8157348632812;
goaldepthFlag = odomFlag = false;
DIST_TO_GOAL_ = 0.5;
APPROX_ORDER_ = 4;
TERMINAL_STATE_.isTerminal = true;
episodeNum=0;
stepCnt=0;
MAX_STEP_CNT_ = 50; // Max number of steps the agent can take to find goal
// Constants for discretizing state space
ANGLE_DIVIDER_ = 12;
DIST_DIVIDER_ = 1;
/** Agent's GPS location */
getmodelstate_ = nh.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
reinforcement_srv_ = nh.advertiseService("agent_environment", &LileeEnvironment::agentenvironmentcb, this);
respawnerpub_ = private_nh.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1);
cmdpub_ = private_nh.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1); //("/cmd_vel", 1);
rectanglesub_ = nh.subscribe<std_msgs::String>("/location_data", 1, &LileeEnvironment::rectanglecb, this);
goaldepthsub_ = nh.subscribe("/camera/depth/image_raw", 1, &LileeEnvironment::goaldepthcb, this);
bumpersub_ = nh.subscribe("/mobile_base/events/bumper", 1, &LileeEnvironment::bumpercb, this);
respawn_goal();
respawn_agent();
}
void XtionGrabber::onInit()
{
ros::NodeHandle& nh = getMTPrivateNodeHandle();
std::string depthDevice;
std::string colorDevice;
if(!nh.getParam("depth_device", depthDevice)
|| !nh.getParam("color_device", colorDevice))
{
ROS_FATAL("depth_device and color_device parameters are mandatory!");
throw std::runtime_error("depth_device and color_device parameters are mandatory!");
}
nh.param("depth_width", m_depthWidth, 640);
nh.param("depth_height", m_depthHeight, 480);
nh.param("color_width", m_colorWidth, 1280);
nh.param("color_height", m_colorHeight, 1024);
m_deviceName = getPrivateNodeHandle().getNamespace();
if(!setupColor(colorDevice))
throw std::runtime_error("Could not setup color channel");
if(!setupDepth(depthDevice))
throw std::runtime_error("Could not setup depth channel");
m_colorFocalLength = 525.0f * m_colorWidth / 640;
// The depth sensor actually has a different focal length, but since
// we are using hw registration, the rgb focal length applies.
m_depthFocalLength = 525.0f * m_depthWidth / 640;
m_cloudGenerator.init(m_depthWidth, m_depthHeight, m_depthFocalLength);
m_filledCloudGenerator.init(1280, 960, 525.0f * 1280 / 640);
setupRGBInfo();
setupDepthInfo();
m_pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
m_pub_filledCloud = nh.advertise<sensor_msgs::PointCloud2>("cloud_filled", 1);
NODELET_INFO("Starting streaming...");
m_thread = boost::thread(boost::bind(&XtionGrabber::read_thread, this));
}
void ConnectionBasedNodelet::onInit()
{
connection_status_ = NOT_SUBSCRIBED;
nh_.reset (new ros::NodeHandle (getMTNodeHandle ()));
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
pnh_->param("always_subscribe", always_subscribe_, false);
pnh_->param("verbose_connection", verbose_connection_, false);
if (!verbose_connection_) {
nh_->param("verbose_connection", verbose_connection_, false);
}
// timer to warn when no connection in a few seconds
ever_subscribed_ = false;
timer_ = nh_->createWallTimer(
ros::WallDuration(5),
&ConnectionBasedNodelet::warnNeverSubscribedCallback,
this,
/*oneshot=*/true);
}
void
pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
{
if (!isValid (cloud))
return;
getMTPrivateNodeHandle ().getParam ("filename", file_name_);
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
std::string fname;
if (file_name_.empty ())
fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
else
fname = file_name_;
impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
}
void
pcl_ros::ConvexHull2D::onInit ()
{
ros::NodeHandle pnh = getMTPrivateNodeHandle ();
pub_output_ = pnh.advertise<PointCloud> ("output", max_queue_size_);
pub_plane_ = pnh.advertise<geometry_msgs::PolygonStamped>("output_polygon", max_queue_size_);
// ---[ Optional parameters
pnh.getParam ("use_indices", use_indices_);
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
// Subscribe to the input using a filter
sub_input_filter_.subscribe (pnh, "input", 1);
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe (pnh, "indices", 1);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
}
}
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh.subscribe<PointCloud> ("input", 1, bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - use_indices : %s",
getName ().c_str (),
(use_indices_) ? "true" : "false");
}
void ControlMode::onInit()
{
NODELET_INFO("ControlMode onInit() called");
nh = getMTNodeHandle();
nh_priv = getMTPrivateNodeHandle();
// Diagnostics
diag_updater.add(getName() + " status", this, &ControlMode::diagnostics);
diag_updater.setHardwareID("none");
diag_updater.force_update();
// Parameters
nh_priv.param("status_report_rate", status_report_rate, status_report_rate);
nh_priv.param("control_output_rate", control_output_rate, control_output_rate);
CHECK_PARAMETER((status_report_rate > 0), "parameter value out of range");
CHECK_PARAMETER((control_output_rate > 0), "parameter value out of range");
// Publishers
control_mode_status_pub = nh_priv.advertise<control_mode_status> ("status", 1, true);
}
void HiwrCameraControllerNodelet::onInit(){
public_nh_ = getNodeHandle(); //Public namespace
private_nh_ = getMTPrivateNodeHandle();
//Retrieveing params from param/config__xx.yaml
//Can be better with have_param() before getting ...
private_nh_.getParam("absolute_exposure", config_.absolute_exposure);
private_nh_.getParam("camera_info_url", config_.camera_info_url);
private_nh_.getParam("camera_name", config_.camera_name);
private_nh_.getParam("brightness", config_.brightness);
private_nh_.getParam("contrast", config_.contrast);
private_nh_.getParam("device_name", config_.device);
private_nh_.getParam("exposure", config_.exposure);
private_nh_.getParam("focus_absolute", config_.focus_absolute);
private_nh_.getParam("focus_auto", config_.focus_auto);
private_nh_.getParam("format_mode", config_.format_mode);
private_nh_.getParam("frame_id", config_.frame_id);
private_nh_.getParam("frame_rate", config_.frame_rate);
private_nh_.getParam("gain", config_.gain);
private_nh_.getParam("height", config_.height);
private_nh_.getParam("power_line_frequency", config_.power_line_frequency);
private_nh_.getParam("saturation", config_.saturation);
private_nh_.getParam("sharpness", config_.sharpness);
private_nh_.getParam("white_balance_temperature", config_.white_balance_temperature);
private_nh_.getParam("width", config_.width);
configureSpinning(private_nh_);
it_ = new image_transport::ImageTransport(private_nh_);
image_pub_ = it_->advertise("output_video", 1);
running_ = true;
device_thread_ = boost::shared_ptr< boost::thread >
(new boost::thread(boost::bind(&HiwrCameraControllerNodelet::spin, this)));
NODELET_INFO("[UVC Cam Nodelet] Loading OK");
}
void
pcl_ros::PieceExtraction::onInit ()
{
// Call the super onInit ()
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
// Parameters that we care about only at startup
pnh_->getParam ("max_queue_size", max_queue_size_);
// ---[ Optional parameters
pnh_->getParam ("use_indices", use_indices_);
pnh_->getParam ("latched_indices", latched_indices_);
pnh_->getParam ("approximate_sync", approximate_sync_);
NODELET_DEBUG ("[%s::onInit] PCL Nodelet successfully created with the following parameters:\n"
" - approximate_sync : %s\n"
" - use_indices : %s\n"
" - latched_indices : %s\n"
" - max_queue_size : %d",
getName ().c_str (),
(approximate_sync_) ? "true" : "false",
(use_indices_) ? "true" : "false",
(latched_indices_) ? "true" : "false",
max_queue_size_);
// ---[ Mandatory parameters
double cluster_tolerance;
if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
{
NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ());
return;
}
int spatial_locator;
if (!pnh_->getParam ("spatial_locator", spatial_locator))
{
NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
return;
}
// output
pub_output_ = pnh_->advertise<chess_msgs::ChessBoard>("output", max_queue_size_);
//pub_output_ = pnh_->advertise<PointCloud>("output", max_queue_size_);
// Enable the dynamic reconfigure service
srv_ = boost::make_shared <dynamic_reconfigure::Server<PieceExtractionConfig> > (*pnh_);
dynamic_reconfigure::Server<PieceExtractionConfig>::CallbackType f = boost::bind (&PieceExtraction::config_callback, this, _1, _2);
srv_->setCallback (f);
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&PieceExtraction::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&PieceExtraction::input_indices_callback, this, _1, _2));
}
}
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&PieceExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - max_queue_size : %d\n"
" - use_indices : %s\n"
" - cluster_tolerance : %f\n"
" - spatial_locator : %d",
getName ().c_str (),
max_queue_size_,
(use_indices_) ? "true" : "false", cluster_tolerance, spatial_locator);
// Set given parameters here
impl_.setSpatialLocator (spatial_locator);
impl_.setClusterTolerance (cluster_tolerance);
}
void ARDroneVideoNodelet::onInit(){
ros::NodeHandle nh(getMTPrivateNodeHandle());
this->video = ar2mav::ARDroneVideo(nh);
this->worker = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&ar2mav::ARDroneVideo::fetch_video, this->video)));
}
void
pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud,
const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
return;
PointCloud output;
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
// Publish an empty message
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices, "indices"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
// Publish an empty message
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
// Reset the indices and surface pointers
IndicesPtr indices_ptr;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
impl_.setInputCloud (cloud);
impl_.setIndices (indices_ptr);
// Estimate the feature
impl_.reconstruct (output);
// If more than 3 points are present, send a PolygonStamped hull too
if (output.points.size () >= 3)
{
geometry_msgs::PolygonStamped poly;
poly.header = output.header;
poly.polygon.points.resize (output.points.size ());
// Get three consecutive points (without copying)
pcl::Vector4fMap O = output.points[1].getVector4fMap ();
pcl::Vector4fMap B = output.points[0].getVector4fMap ();
pcl::Vector4fMap A = output.points[2].getVector4fMap ();
// Check the direction of points -- polygon must have CCW direction
Eigen::Vector4f OA = A - O;
Eigen::Vector4f OB = B - O;
Eigen::Vector4f N = OA.cross3 (OB);
double theta = N.dot (O);
bool reversed = false;
if (theta < (M_PI / 2.0))
reversed = true;
for (size_t i = 0; i < output.points.size (); ++i)
{
if (reversed)
{
size_t j = output.points.size () - i - 1;
poly.polygon.points[i].x = output.points[j].x;
poly.polygon.points[i].y = output.points[j].y;
poly.polygon.points[i].z = output.points[j].z;
}
else
{
poly.polygon.points[i].x = output.points[i].x;
poly.polygon.points[i].y = output.points[i].y;
poly.polygon.points[i].z = output.points[i].z;
}
}
pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
}
// Publish a Boost shared ptr const data
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
}
void CameraTrackerNodelet::onInit()
{
//tracker_.reset(new CameraTracker(getNodeHandle(), getPrivateNodeHandle()));
tracker_.reset(new dvo_ros::CameraDenseTracker(getMTNodeHandle(), getMTPrivateNodeHandle()));
}
void ARDroneDriverNodelet::onInit(){
ros::NodeHandle nh(getMTPrivateNodeHandle());
this->driver = boost::shared_ptr<ar2mav::ARDroneDriver>(new ar2mav::ARDroneDriver(nh));
}
/*!
* \brief Connection callback to only do work when someone is listening.
*
* This function will connect/disconnect from the camera depending on who is using the output.
*/
void connectCb()
{
NODELET_DEBUG("Connect callback!");
boost::mutex::scoped_lock scopedLock(connect_mutex_); // Grab the mutex. Wait until we're done initializing before letting this function through.
// Check if we should disconnect (there are 0 subscribers to our data)
if(it_pub_.getNumSubscribers() == 0 && pub_->getPublisher().getNumSubscribers() == 0)
{
try
{
NODELET_DEBUG("Disconnecting.");
pubThread_->interrupt();
pubThread_->join();
sub_.shutdown();
NODELET_DEBUG("Stopping camera capture.");
pg_.stop();
/*NODELET_DEBUG("Disconnecting from camera.");
pg_.disconnect();*/
}
catch(std::runtime_error& e)
{
NODELET_ERROR("%s", e.what());
}
}
else if(!sub_) // We need to connect
{
NODELET_DEBUG("Connecting");
// Try connecting to the camera
volatile bool connected = false;
while(!connected && ros::ok())
{
try
{
NODELET_DEBUG("Connecting to camera.");
pg_.connect(); // Probably already connected from the reconfigure thread. This will will not throw if successfully connected.
connected = true;
}
catch(std::runtime_error& e)
{
NODELET_ERROR("%s", e.what());
ros::Duration(1.0).sleep(); // sleep for one second each time
}
}
// Set the timeout for grabbing images.
double timeout;
getMTPrivateNodeHandle().param("timeout", timeout, 1.0);
try
{
NODELET_DEBUG("Setting timeout to: %f.", timeout);
pg_.setTimeout(timeout);
}
catch(std::runtime_error& e)
{
NODELET_ERROR("%s", e.what());
}
// Subscribe to gain and white balance changes
sub_ = getMTNodeHandle().subscribe("image_exposure_sequence", 10, &pointgrey_camera_driver::PointGreyCameraNodelet::gainWBCallback, this);
volatile bool started = false;
while(!started && ros::ok())
{
try
{
NODELET_DEBUG("Starting camera capture.");
pg_.start();
started = true;
}
catch(std::runtime_error& e)
{
NODELET_ERROR("%s", e.what());
ros::Duration(1.0).sleep(); // sleep for one second each time
}
}
// Start the thread to loop through and publish messages
pubThread_.reset(new boost::thread(boost::bind(&pointgrey_camera_driver::PointGreyCameraNodelet::devicePoll, this)));
}
else
{
NODELET_DEBUG("Do nothing in callback.");
}
}
void
pcl_ros::PCDReader::onInit ()
{
ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
// Provide a latched topic
ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true);
private_nh.getParam ("publish_rate", publish_rate_);
private_nh.getParam ("tf_frame", tf_frame_);
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
" - publish_rate : %f\n"
" - tf_frame : %s",
getName ().c_str (),
publish_rate_, tf_frame_.c_str ());
PointCloud2Ptr output_new;
output_ = boost::make_shared<PointCloud2> ();
output_new = boost::make_shared<PointCloud2> ();
// Wait in a loop until someone connects
do
{
ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
ros::spinOnce ();
ros::Duration (0.01).sleep ();
}
while (private_nh.ok () && pub_output.getNumSubscribers () == 0);
std::string file_name;
while (private_nh.ok ())
{
// Get the current filename parameter. If no filename set, loop
if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ())
{
ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
ros::Duration (0.01).sleep ();
ros::spinOnce ();
continue;
}
// If the filename parameter holds a different value than the last one we read
if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
{
NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
file_name = file_name_;
PointCloud2 cloud;
if (impl_.read (file_name_, cloud) < 0)
{
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
return;
}
output_ = boost::make_shared<PointCloud2> (cloud);
output_->header.stamp = ros::Time::now ();
output_->header.frame_id = tf_frame_;
}
// We do not publish empty data
if (output_->data.size () == 0)
continue;
if (publish_rate_ == 0)
{
if (output_ != output_new)
{
NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
pub_output.publish (output_);
output_new = output_;
}
ros::Duration (0.01).sleep ();
}
else
{
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
output_->header.stamp = ros::Time::now ();
pub_output.publish (output_);
ros::Duration (publish_rate_).sleep ();
}
ros::spinOnce ();
// Update parameters from server
private_nh.getParam ("publish_rate", publish_rate_);
private_nh.getParam ("tf_frame", tf_frame_);
}
}
void
pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud,
const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
return;
// Output points have the same type as the input, they are only smoothed
PointCloudIn output;
// Normals are also estimated and published on a separate topic
NormalCloudOut::Ptr normals (new NormalCloudOut ());
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices, "indices"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
///
// Reset the indices and surface pointers
impl_.setInputCloud (cloud);
IndicesPtr indices_ptr;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
impl_.setIndices (indices_ptr);
// Initialize the spatial locator
// Do the reconstructon
//impl_.process (output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
normals->header = cloud->header;
pub_normals_.publish (normals);
}
