diff --git a/pointcloud_image_streamer/src/pointcloud_image.cpp b/pointcloud_image_streamer/src/pointcloud_image.cpp index 7f463e5..0aaa549 100644 --- a/pointcloud_image_streamer/src/pointcloud_image.cpp +++ b/pointcloud_image_streamer/src/pointcloud_image.cpp @@ -66,18 +66,24 @@ class DepthRGBEncoder pub_it_(nh_), crop_size_(512) { + // ROS parameters + ros::NodeHandle priv_nh_("~"); + // read depth map topic from param server std::string depthmap_topic; - nh_.param("depth", depthmap_topic, "/camera/depth/image"); + priv_nh_.param("depth", depthmap_topic, "/camera/depth/image"); // read rgb topic from param server std::string rgb_image_topic; - nh_.param("rgb", rgb_image_topic, "/camera/rgb/image_color"); + priv_nh_.param("rgb", rgb_image_topic, "/camera/rgb/image_color"); - nh_.param("frame", base_frame_, "/camera_link"); + priv_nh_.param("frame", base_frame_, "/camera_link"); subscribe(depthmap_topic, rgb_image_topic); + ROS_INFO_STREAM("Subscribing to "<< rgb_image_topic); + ROS_INFO_STREAM("Subscribing to "<< depthmap_topic); + pub_ = pub_it_.advertise("depth_color_combined", 1); } virtual ~DepthRGBEncoder() @@ -119,7 +125,7 @@ class DepthRGBEncoder void depth_with_color_cb(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::ImageConstPtr& color_msg) { - ROS_INFO("Image depth/color pair received"); + ROS_DEBUG("Image depth/color pair received"); process(depth_msg, color_msg, crop_size_); } diff --git a/www/demo/demo-low-res.html b/www/demo/demo-low-res.html index e34192b..e97573f 100644 --- a/www/demo/demo-low-res.html +++ b/www/demo/demo-low-res.html @@ -1,7 +1,7 @@ - Interactive Marker Example: Basic Controls + Depth Cloud + Interactive Markers Demo