void ControllerManagerPlugin::setupROSComponents_()
{
map_list_service_client_.insert("kuka_lwr_left",getNodeHandle().serviceClient<controller_manager_msgs::ListControllers>("/kuka_lwr_left/controller_manager/list_controllers"));
map_list_service_client_.insert("kuka_lwr_right",getNodeHandle().serviceClient<controller_manager_msgs::ListControllers>("/kuka_lwr_right/controller_manager/list_controllers"));
map_switch_service_client_.insert("kuka_lwr_left",getNodeHandle().serviceClient<controller_manager_msgs::SwitchController>("/kuka_lwr_left/controller_manager/switch_controller"));
map_switch_service_client_.insert("kuka_lwr_right",getNodeHandle().serviceClient<controller_manager_msgs::SwitchController>("/kuka_lwr_right/controller_manager/switch_controller"));
}
virtual void onInit() {
std::string port; ROS_ASSERT_MSG(getPrivateNodeHandle().getParam("port", port),
"\"port\" param missing");
int baudrate = 115200; getPrivateNodeHandle().getParam("baudrate", baudrate);
ROS_ASSERT_MSG(getPrivateNodeHandle().getParam("frame_id", frame_id),
"\"frame_id\" param missing");
pub = getNodeHandle().advertise<uf_common::Float64Stamped>("depth", 10);
device = boost::make_shared<Device>(port, baudrate);
heartbeat_timer = getNodeHandle().createTimer(ros::Duration(0.5), boost::bind(&Nodelet::heartbeat_callback, this, _1));
running = true;
polling_thread_inst = boost::thread(boost::bind(&Nodelet::polling_thread, this));
}
virtual void onInit() {
std::string port = uf_common::getParam<std::string>(
getPrivateNodeHandle(), "port");
int baudrate = uf_common::getParam<int>(
getPrivateNodeHandle(), "baudrate", 115200);
std::string frame_id = uf_common::getParam<std::string>(
getPrivateNodeHandle(), "frame_id");
pub = getNodeHandle().advertise<uf_common::Float64Stamped>("depth", 10);
device = boost::make_shared<Device>(port, baudrate);
heartbeat_timer = getNodeHandle().createTimer(ros::Duration(0.5), boost::bind(&Nodelet::heartbeat_callback, this, _1));
running = true;
polling_thread_inst = boost::thread(boost::bind(&Nodelet::polling_thread, this));
}
void ArduinoOnboard::onInit()
{
ros::NodeHandle nh = getNodeHandle();
m_nhPvt = getPrivateNodeHandle();
m_lastTime = ros::Time::now();
std::string port;
if(!m_nhPvt.getParam("port", port) ||
!m_nhPvt.getParam("numMovingAverageValues", m_numMovingAverageValues) ||
!m_nhPvt.getParam("wheelDiameter", m_wheelDiameter) ||
!m_nhPvt.getParam("triggerFPS", m_triggerFPS) ||
!m_nhPvt.getParam("srvBatteryCrit", srvBatteryCrit) ||
!m_nhPvt.getParam("srvBatteryLow", srvBatteryLow) ||
!m_nhPvt.getParam("camBatteryCrit", camBatteryCrit) ||
!m_nhPvt.getParam("camBatteryLow", camBatteryLow) ||
!m_nhPvt.getParam("lfRotationEnabled", m_lfEnabled) ||
!m_nhPvt.getParam("rfRotationEnabled", m_rfEnabled) ||
!m_nhPvt.getParam("lbRotationEnabled", m_lbEnabled) ||
!m_nhPvt.getParam("rbRotationEnabled", m_rbEnabled) )
{
ROS_ERROR("ArduinoOnboard: Could not get all arduinoOnboard parameters");
}
m_wheelSpeedPub = nh.advertise<autorally_msgs::wheelSpeeds>
("wheelSpeeds", 1);
m_servoPub = nh.advertise<autorally_msgs::servoMSG> ("RC", 1);
m_port.init(m_nhPvt, getName(), "", "ArduinoOnboard", port, true);
m_port.registerDataCallback(
boost::bind(&ArduinoOnboard::arduinoDataCallback, this));
loadServoParams();
}
virtual void onInit()
{
nh_ = getNodeHandle();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
local_nh_ = ros::NodeHandle("~");
local_nh_.param("debug_view", debug_view_, false);
subscriber_count_ = 0;
prev_stamp_ = ros::Time(0, 0);
window_name_ = "Hough Circle Detection Demo";
canny_threshold_initial_value_ = 200;
accumulator_threshold_initial_value_ = 50;
max_accumulator_threshold_ = 200;
max_canny_threshold_ = 255;
//declare and initialize both parameters that are subjects to change
canny_threshold_ = canny_threshold_initial_value_;
accumulator_threshold_ = accumulator_threshold_initial_value_;
image_transport::SubscriberStatusCallback img_connect_cb = boost::bind(&HoughCirclesNodelet::img_connectCb, this, _1);
image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&HoughCirclesNodelet::img_disconnectCb, this, _1);
ros::SubscriberStatusCallback msg_connect_cb = boost::bind(&HoughCirclesNodelet::msg_connectCb, this, _1);
ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&HoughCirclesNodelet::msg_disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
msg_pub_ = local_nh_.advertise<opencv_apps::CircleArrayStamped>("circles", 1, msg_connect_cb, msg_disconnect_cb);
if( debug_view_ ) {
subscriber_count_++;
}
dynamic_reconfigure::Server<hough_circles::HoughCirclesConfig>::CallbackType f =
boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
virtual void onInit()
{
nh_ = getNodeHandle();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
local_nh_ = ros::NodeHandle("~");
local_nh_.param("debug_view", debug_view_, false);
subscriber_count_ = 0;
prev_stamp_ = ros::Time(0, 0);
window_name_ = "flow";
image_transport::SubscriberStatusCallback img_connect_cb = boost::bind(&FBackFlowNodelet::img_connectCb, this, _1);
image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FBackFlowNodelet::img_disconnectCb, this, _1);
ros::SubscriberStatusCallback msg_connect_cb = boost::bind(&FBackFlowNodelet::msg_connectCb, this, _1);
ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FBackFlowNodelet::msg_disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
msg_pub_ = local_nh_.advertise<opencv_apps::FlowArrayStamped>("flows", 1, msg_connect_cb, msg_disconnect_cb);
if( debug_view_ ) {
subscriber_count_++;
}
dynamic_reconfigure::Server<fback_flow::FBackFlowConfig>::CallbackType f =
boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
void ColorHistogram::onInit()
{
DiagnosticNodelet::onInit();
nh_ = ros::NodeHandle(getNodeHandle(), "image");
pnh_->param("use_mask", use_mask_, false);
b_hist_size_ = r_hist_size_ = g_hist_size_ =
h_hist_size_ = s_hist_size_ = i_hist_size_ = 512;
b_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
*pnh_, "blue_histogram", 1);
g_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
*pnh_, "green_histogram", 1);
r_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
*pnh_, "red_histogram", 1);
h_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
*pnh_, "hue_histogram", 1);
s_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
*pnh_, "saturation_histogram", 1);
i_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
*pnh_, "intensity_histogram", 1);
image_pub_ = advertise<sensor_msgs::Image>(
*pnh_, "input_image", 1);
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&ColorHistogram::configCallback, this, _1, _2);
srv_->setCallback (f);
}
status_t
_TDL_SpawnStatementData::startRunning ( )
{
switch ( getState() )
{
case _TDL_SpawnStatementData::NOT_ALLOCATED:
if ( allocate() == FAILURE )
{
TDL::getLogStream()
<< "[_TDL_SpawnStatementData::startRunning(\"" << getName()
<< "\")] ERROR: allocate() FAILED!!! Aborting..."
<< endl;
return FAILURE;
}
/* NO BREAK OR RETURN!!! GO DIRECTLY TO "ALLOCATED" case */
case _TDL_SpawnStatementData::ALLOCATED:
if ( getNodeHandle() . isNull() )
{
TDL::getLogStream()
<< "[_TDL_SpawnStatementData::startRunning(\"" << getName()
<< "\")] ERROR: In ALLOCATED-state, but node is NULL!! Aborting..."
<< endl;
return FAILURE;
}
/* Record that we are now "Running" this node. */
spawnState = _TDL_SpawnStatementData::RUNNING;
/* Yes, I am paranoid. Lets double check everything one last time. */
verifyDistributedCorrectly();
return SUCCESS;
case _TDL_SpawnStatementData::RUNNING:
/* It's already Running. What are we going to do? Fail? */
return SUCCESS;
case _TDL_SpawnStatementData::DESTROYED:
TDL::getLogStream()
<< "[_TDL_SpawnStatementData::startRunning(\"" << getName()
<< "\")] ERROR: Node has already been destroyed!" << endl;
return FAILURE;
case _TDL_SpawnStatementData::DELETED:
TDL::getLogStream()
<< "[_TDL_SpawnStatementData::startRunning(\"" << getName()
<< "\")] ERROR: Object has already been DELETED!" << endl;
return FAILURE;
default:
TDL::getLogStream()
<< "[_TDL_SpawnStatementData::startRunning(\"" << getName()
<< "\")] ERROR: Unknown state: " << int4(getState()) << endl;
return FAILURE;
}
}
virtual void onInit()
{
NODELET_WARN_STREAM("Initializing nodelet..." << getName());
lp_.reset(new bta_tof_driver::BtaRos(getNodeHandle(), getPrivateNodeHandle(), getName()));
stream_thread_.reset(new boost::thread(boost::bind(&BtaRos::initialize, lp_.get())));
};
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
std::string modelPath;
pnh.param("model", modelPath, modelPath);
if(modelPath.empty())
{
NODELET_ERROR("undistort_depth: \"model\" parameter should be set!");
}
model_.load(modelPath);
if(!model_.isValid())
{
NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str());
}
else
{
image_transport::ImageTransport it(nh);
sub_ = it.subscribe("depth", 1, &UndistortDepth::callback, this);
pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1);
}
}
void base_controller_nodelet::onInit( )
{
controller = new base_controller( getNodeHandle( ), getPrivateNodeHandle( ) );
if( autostart && !start( ) )
ROS_ERROR( "Failed to start controller" );
}
virtual void onInit() {
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
dynamic_set = false;
private_nh.getParam("min_height", min_height_);
private_nh.getParam("max_height", max_height_);
private_nh.getParam("row_min", u_min_);
private_nh.getParam("row_max", u_max_);
private_nh.getParam("output_frame_id", output_frame_id_);
pub_ = nh.advertise<sensor_msgs::LaserScan> ("scan", 10);
sub_ = nh.subscribe<PointCloud> ("cloud", 10, &CloudToScan::callback,
this);
marker_pub = nh.advertise<visualization_msgs::Marker> (
"visualization_marker", 10);
pcl_to_scan::cloud_to_scan_paramsConfig config;
config.max_height = max_height_;
config.min_height = min_height_;
// Set up a dynamic reconfigure server.
dr_srv = new dynamic_reconfigure::Server < pcl_to_scan::cloud_to_scan_paramsConfig > ();
cb = boost::bind(&CloudToScan::configCallback, this, _1, _2);
dr_srv->updateConfig(config);
dr_srv->setCallback(cb);
}
/*!
* @brief OnInit method from node handle.
* OnInit method from node handle. Sets up the parameters
* and topics.
*/
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
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_);
cmdpub_ = private_nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
markerpub_ = private_nh.advertise<visualization_msgs::Marker>("marker",1);
bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1);
sub_= nh.subscribe<PointCloud>("depth/points", 1, &TurtlebotFollower::cloudcb, this);
switch_srv_ = private_nh.advertiseService("change_state", &TurtlebotFollower::changeModeSrvCb, this);
config_srv_ = new dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>(private_nh);
dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>::CallbackType f =
boost::bind(&TurtlebotFollower::reconfigure, this, _1, _2);
config_srv_->setCallback(f);
}
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 Dm32ToDmNodelet::onInit()
{
nh_ = getNodeHandle();
private_nh_ = getPrivateNodeHandle();
nh_.param("sensor/name", sensor_name_, std::string("realsenseR200"));
image_transport::ImageTransport it(nh_);
image_pub_ = it.advertise("dm32_to_dm/output", 1);
sub_ = it.subscribe("dm32_to_dm/input", 1, &Dm32ToDmNodelet::cb, this);
scale_ = 0.001;
if(sensor_name_ == "realsenseF200")
{
sensor_depth_max_ = 1200;
}
else if(sensor_name_ == "realsenseR200")
{
private_nh_.param("sensor_depth_max", sensor_depth_max_, int(4000));
}
else if(sensor_name_ == "picoflexx")
{
nh_.param("sensor/depth/max", sensor_depth_max_, int(5000));
}
else {
NODELET_ERROR("Wrong sensor name!");
return;
}
NODELET_INFO_STREAM("Initialized dm32 to dm nodelet for sensor " << sensor_name_
<< ", with sensor_depth_max=" << sensor_depth_max_);
}
void SLICSuperPixels::onInit()
{
ROS_WARN("Maybe this node does not work for large size images with segfault.");
nh_ = ros::NodeHandle(getNodeHandle(), "image");
pnh_ = getPrivateNodeHandle();
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh_);
dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (
&SLICSuperPixels::configCallback, this, _1, _2);
srv_->setCallback (f);
pnh_.param("publish_debug_images", debug_image_, false);
it_.reset(new image_transport::ImageTransport(nh_));
pub_ = pnh_.advertise<sensor_msgs::Image>("output", 1);
if (debug_image_) {
pub_debug_ = pnh_.advertise<sensor_msgs::Image>("debug", 1);
pub_debug_mean_color_ = pnh_.advertise<sensor_msgs::Image>("debug/mean_color", 1);
pub_debug_center_grid_ = pnh_.advertise<sensor_msgs::Image>("debug/center_grid", 1);
}
image_sub_ = it_->subscribe("", 1, &SLICSuperPixels::imageCallback, this);
ros::V_string names = boost::assign::list_of("image");
jsk_topic_tools::warnNoRemap(names);
}
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);
}
// Handles (un)subscribing when clients (un)subscribe
void Stereoproc::connectCb()
{
boost::lock_guard<boost::mutex> connect_lock(connect_mutex_);
connected_.DebayerMonoLeft = (pub_mono_left_.getNumSubscribers() > 0)?1:0;
connected_.DebayerMonoRight = (pub_mono_right_.getNumSubscribers() > 0)?1:0;
connected_.DebayerColorLeft = (pub_color_left_.getNumSubscribers() > 0)?1:0;
connected_.DebayerColorRight = (pub_color_right_.getNumSubscribers() > 0)?1:0;
connected_.RectifyMonoLeft = (pub_mono_rect_left_.getNumSubscribers() > 0)?1:0;
connected_.RectifyMonoRight = (pub_mono_rect_right_.getNumSubscribers() > 0)?1:0;
connected_.RectifyColorLeft = (pub_color_rect_left_.getNumSubscribers() > 0)?1:0;
connected_.RectifyColorRight = (pub_color_rect_right_.getNumSubscribers() > 0)?1:0;
connected_.Disparity = (pub_disparity_.getNumSubscribers() > 0)?1:0;
connected_.DisparityVis = (pub_disparity_vis_.getNumSubscribers() > 0)?1:0;
connected_.Pointcloud = (pub_pointcloud_.getNumSubscribers() > 0)?1:0;
int level = connected_.level();
if (level == 0)
{
sub_l_raw_image_.unsubscribe();
sub_l_info_.unsubscribe();
sub_r_raw_image_.unsubscribe();
sub_r_info_.unsubscribe();
}
else if (!sub_l_raw_image_.getSubscriber())
{
ros::NodeHandle &nh = getNodeHandle();
// Queue size 1 should be OK; the one that matters is the synchronizer queue size.
/// @todo Allow remapping left, right?
image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
sub_l_raw_image_.subscribe(*it_, "left/image_raw", 1, hints);
sub_l_info_ .subscribe(nh, "left/camera_info", 1);
sub_r_raw_image_.subscribe(*it_, "right/image_raw", 1, hints);
sub_r_info_ .subscribe(nh, "right/camera_info", 1);
}
}
virtual void onInit()
{
nh_ = getNodeHandle();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
local_nh_ = ros::NodeHandle("~");
local_nh_.param("debug_view", debug_view_, false);
subscriber_count_ = 0;
prev_stamp_ = ros::Time(0, 0);
window_name_ = "Demo code to find contours in an image";
low_threshold_ = 100; // only for canny
image_transport::SubscriberStatusCallback img_connect_cb = boost::bind(&FindContoursNodelet::img_connectCb, this, _1);
image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FindContoursNodelet::img_disconnectCb, this, _1);
ros::SubscriberStatusCallback msg_connect_cb = boost::bind(&FindContoursNodelet::msg_connectCb, this, _1);
ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FindContoursNodelet::msg_disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
if( debug_view_ ) {
subscriber_count_++;
}
dynamic_reconfigure::Server<find_contours::FindContoursConfig>::CallbackType f =
boost::bind(&FindContoursNodelet::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
virtual void onInit()
{
nh_ = getNodeHandle();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
local_nh_ = ros::NodeHandle("~");
local_nh_.param("debug_view", debug_view_, false);
subscriber_count_ = 0;
prev_stamp_ = ros::Time(0, 0);
image_transport::SubscriberStatusCallback img_connect_cb = boost::bind(&FaceDetectionNodelet::img_connectCb, this, _1);
image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FaceDetectionNodelet::img_disconnectCb, this, _1);
ros::SubscriberStatusCallback msg_connect_cb = boost::bind(&FaceDetectionNodelet::msg_connectCb, this, _1);
ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FaceDetectionNodelet::msg_disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
msg_pub_ = local_nh_.advertise<opencv_apps::FaceArrayStamped>("faces", 1, msg_connect_cb, msg_disconnect_cb);
if( debug_view_ ) {
subscriber_count_++;
}
std::string face_cascade_name, eyes_cascade_name;
local_nh_.param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
local_nh_.param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); };
if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); };
dynamic_reconfigure::Server<face_detection::FaceDetectionConfig>::CallbackType f =
boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
virtual void onInit()
{
nh_ = getNodeHandle();
it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
local_nh_ = ros::NodeHandle("~");
local_nh_.param("debug_view", debug_view_, false);
subscriber_count_ = 0;
prev_stamp_ = ros::Time(0, 0);
window_name_ = "Hough Lines Demo";
min_threshold_ = 50;
max_threshold_ = 150;
threshold_ = max_threshold_;
image_transport::SubscriberStatusCallback img_connect_cb = boost::bind(&HoughLinesNodelet::img_connectCb, this, _1);
image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&HoughLinesNodelet::img_disconnectCb, this, _1);
ros::SubscriberStatusCallback msg_connect_cb = boost::bind(&HoughLinesNodelet::msg_connectCb, this, _1);
ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&HoughLinesNodelet::msg_disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);
if( debug_view_ ) {
subscriber_count_++;
}
dynamic_reconfigure::Server<hough_lines::HoughLinesConfig>::CallbackType f =
boost::bind(&HoughLinesNodelet::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
void pc_motion_filter::PcMotionFilter::onInit()
{
ros::NodeHandle& nh = getNodeHandle();
tf_buffer.reset(new tf2_ros::Buffer());
tf_listener.reset(new tf2_ros::TransformListener(*tf_buffer, nh));
cloud_out_publisher = nh.advertise<sensor_msgs::PointCloud2>("output", 1, false);
cloud_in_subscriber = nh.subscribe<sensor_msgs::PointCloud2>("input", 1, boost::bind(&PcMotionFilter::cloud_in_cb, this, _1));
}
virtual void onInit() {
image_publisher_.reset(new ImagePublisher(getNodeHandle(), getPrivateNodeHandle()));
image_publisher_->load();
thread_available_ = true;
thread_ = boost::shared_ptr<boost::thread>(
new boost::thread(boost::bind(&ImagePublisherNodelet::spin, this)));
}
void UsbCamNodelet::onInit()
{
ROS_INFO("Usb cam nodelet init");
usb_cam_.reset(new UsbCamWrapper(getNodeHandle(), getPrivateNodeHandle()));
// spawn device poll thread
device_thread_ = boost::shared_ptr<boost::thread>
(new boost::thread(boost::bind(&UsbCamWrapper::spin, usb_cam_)));
}
void
Image_nodelet::onInit(){
inst_.reset( new image_publisher(getNodeHandle(), getPrivateNodeHandle()));
//topic_listener_thread_ = boost::shared_ptr<boost::thread>(new boost::thread( boost::bind( &Image_nodelet::topic_listener, this )));
// ros::MultiThreadedSpinner spinner(0);
// spinner.spin();
}
void
onInit()
{
n_ = getNodeHandle();
ros::NodeHandle pn = getPrivateNodeHandle();
pn.getParam("target_frame_id", target_frame_id_);
pn.getParam("table_frame_id", table_frame_id_);
pn.getParam("height_min", height_min_);
pn.getParam("height_max", height_max_);
XmlRpc::XmlRpcValue v;
pn.getParam("table_dimensions", v);
if( v.size() < 3)
{
ROS_ERROR("Hull points not set correctly, nodelet will not work");
return;
}
double x ,y, z;
x = (double)v[0];
y = (double)v[1];
z = (double)v[2];
Eigen::Vector3d p;
p << -x/2, -y/2, z;
hull_points_[0] = p;
p << -x/2, y/2, z;
hull_points_[1] = p;
p << x/2, y/2, z;
hull_points_[2] = p;
p << x/2, -y/2, z;
hull_points_[3] = p;
tf::StampedTransform trf_table;
try
{
tf_listener_.waitForTransform(target_frame_id_, table_frame_id_, ros::Time(), ros::Duration(2));
tf_listener_.lookupTransform(target_frame_id_, table_frame_id_, ros::Time(), trf_table);
}
catch (tf::TransformException& ex) {
ROS_ERROR("[transform region crop] : %s",ex.what());
return;
}
Eigen::Affine3d ad;
tf::transformTFToEigen(trf_table, ad);
for(int i = 0; i < hull_points_.size(); i++)
{
Point p;
p.getVector3fMap() = (ad*hull_points_[i]).cast<float>();
hull_->points[i] = p;
}
pc_sub_ = n_.subscribe<PointCloud>("point_cloud_in", 1, boost::bind(&TableRegionCropNodelet::topicCallback, this, _1));
pc_pub_ = n_.advertise<PointCloud>("point_cloud_out", 1);
eppd_.setInputPlanarHull(hull_);
eppd_.setHeightLimits(height_min_, height_max_);
eppd_.setViewPoint(0,0,5);
}
void Stereoproc::onInit()
{
ros::NodeHandle &nh = getNodeHandle();
ros::NodeHandle &private_nh = getPrivateNodeHandle();
it_.reset(new image_transport::ImageTransport(nh));
// Synchronize inputs. Topic subscriptions happen on demand in the connection
// callback. Optionally do approximate synchronization.
int queue_size;
private_nh.param("queue_size", queue_size, 5);
bool approx;
private_nh.param("approximate_sync", approx, false);
if (approx)
{
approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size),
sub_l_raw_image_, sub_l_info_,
sub_r_raw_image_, sub_r_info_) );
approximate_sync_->registerCallback(boost::bind(&Stereoproc::imageCb,
this, _1, _2, _3, _4));
}
else
{
exact_sync_.reset( new ExactSync(ExactPolicy(queue_size),
sub_l_raw_image_, sub_l_info_,
sub_r_raw_image_, sub_r_info_) );
exact_sync_->registerCallback(boost::bind(&Stereoproc::imageCb,
this, _1, _2, _3, _4));
}
// Set up dynamic reconfiguration
ReconfigureServer::CallbackType f = boost::bind(&Stereoproc::configCb,
this, _1, _2);
reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
reconfigure_server_->setCallback(f);
// Monitor whether anyone is subscribed to the output
ros::SubscriberStatusCallback connect_cb = boost::bind(&Stereoproc::connectCb, this);
// Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_
boost::lock_guard<boost::mutex> lock(connect_mutex_);
int publisher_queue_size;
private_nh.param("publisher_queue_size", publisher_queue_size, 1);
pub_mono_left_ = nh.advertise<sensor_msgs::Image>("left/image_mono", publisher_queue_size, connect_cb, connect_cb);
pub_mono_right_ = nh.advertise<sensor_msgs::Image>("right/image_mono", publisher_queue_size, connect_cb, connect_cb);
pub_color_left_ = nh.advertise<sensor_msgs::Image>("left/image_color", publisher_queue_size, connect_cb, connect_cb);
pub_color_right_ = nh.advertise<sensor_msgs::Image>("right/image_color", publisher_queue_size, connect_cb, connect_cb);
pub_mono_rect_left_ = nh.advertise<sensor_msgs::Image>("left/rect_mono", publisher_queue_size, connect_cb, connect_cb);
pub_color_rect_left_ = nh.advertise<sensor_msgs::Image>("left/rect_color", publisher_queue_size, connect_cb, connect_cb);
pub_mono_rect_right_ = nh.advertise<sensor_msgs::Image>("right/rect_mono", publisher_queue_size, connect_cb, connect_cb);
pub_color_rect_right_ = nh.advertise<sensor_msgs::Image>("right/rect_color", publisher_queue_size, connect_cb, connect_cb);
pub_disparity_ = nh.advertise<stereo_msgs::DisparityImage>("disparity", publisher_queue_size, connect_cb, connect_cb);
pub_disparity_vis_ = nh.advertise<sensor_msgs::Image>("disparity_vis", publisher_queue_size, connect_cb, connect_cb);
pub_pointcloud_ = nh.advertise<sensor_msgs::PointCloud2>("pointcloud", publisher_queue_size, connect_cb, connect_cb);
cv::cuda::printShortCudaDeviceInfo(cv::cuda::getDevice());
}
void GpsTransformPublisher::onInit()
{
ros::NodeHandle priv = getPrivateNodeHandle();
swri::param(priv,"child_frame_id", veh_frame_id_, std::string("base_link"));
swri::param(priv,"parent_frame_id", global_frame_id_, std::string("map"));
gps_sub_ = getNodeHandle().subscribe("gps", 100, &GpsTransformPublisher::HandleGps, this);
tf_manager_.Initialize();
}
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));
}
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
private_nh.getParam("max_rate", max_update_rate_);
pub_ = nh.advertise<PointCloud>("cloud_out", 10);
sub_ = nh.subscribe<PointCloud>("cloud_in", 10, &CloudThrottle::callback, this);
};
