Author: hsu
Date: 2012-04-18 23:49:02 -0700 (Wed, 18 Apr 2012)
New Revision: 39335
Modified:
stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h
stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h
stacks/simulator_gazebo/trunk/gazebo_plugins/manifest.xml
stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_depth_camera.cpp
stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp
Log:
update PointCloud2 publisher from depth/openni camera, still missing rgb data from corresponding image
Modified: stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h
===================================================================
--- stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h 2012-04-18 21:11:36 UTC (rev 39334)
+++ stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h 2012-04-19 06:49:02 UTC (rev 39335)
@@ -36,7 +36,7 @@
#include <pcl/point_types.h>
// ros messages stuff
-#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include "sensor_msgs/fill_image.h"
@@ -96,7 +96,7 @@
private: void PointCloudConnect();
private: void PointCloudDisconnect();
- private: bool FillPointCloudHelper( pcl::PointCloud<pcl::PointXYZ>& point_cloud,
+ private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg,
uint32_t rows_arg, uint32_t cols_arg,
uint32_t step_arg, void* data_arg);
@@ -109,7 +109,7 @@
private: ros::Publisher depth_image_pub_;
/// \brief PCL point cloud message
- private: pcl::PointCloud<pcl::PointXYZ> point_cloud_msg_;
+ private: sensor_msgs::PointCloud2 point_cloud_msg_;
private: sensor_msgs::Image depth_image_msg_;
private: double point_cloud_cutoff_;
Modified: stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h
===================================================================
--- stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h 2012-04-18 21:11:36 UTC (rev 39334)
+++ stacks/simulator_gazebo/trunk/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h 2012-04-19 06:49:02 UTC (rev 39335)
@@ -36,7 +36,7 @@
#include <pcl/point_types.h>
// ros messages stuff
-#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include "sensor_msgs/fill_image.h"
@@ -103,7 +103,7 @@
private: void DepthImageConnect();
private: void DepthImageDisconnect();
- private: bool FillPointCloudHelper( pcl::PointCloud<pcl::PointXYZ>& point_cloud,
+ private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg,
uint32_t rows_arg, uint32_t cols_arg,
uint32_t step_arg, void* data_arg);
@@ -116,7 +116,7 @@
private: ros::Publisher depth_image_pub_;
/// \brief PCL point cloud message
- private: pcl::PointCloud<pcl::PointXYZ> point_cloud_msg_;
+ private: sensor_msgs::PointCloud2 point_cloud_msg_;
private: sensor_msgs::Image depth_image_msg_;
private: double point_cloud_cutoff_;
Modified: stacks/simulator_gazebo/trunk/gazebo_plugins/manifest.xml
===================================================================
--- stacks/simulator_gazebo/trunk/gazebo_plugins/manifest.xml 2012-04-18 21:11:36 UTC (rev 39334)
+++ stacks/simulator_gazebo/trunk/gazebo_plugins/manifest.xml 2012-04-19 06:49:02 UTC (rev 39335)
@@ -21,6 +21,7 @@
<depend package="driver_base" />
<depend package="rosgraph_msgs" />
<depend package="pcl_ros" />
+ <depend package="pcl" />
<depend package="image_transport" />
<export>
<cpp cflags="-I${prefix}/cfg/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
Modified: stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_depth_camera.cpp
===================================================================
--- stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_depth_camera.cpp 2012-04-18 21:11:36 UTC (rev 39334)
+++ stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_depth_camera.cpp 2012-04-19 06:49:02 UTC (rev 39335)
@@ -40,6 +40,9 @@
#include "sdf/interface/SDF.hh"
#include "sensors/SensorTypes.hh"
+// for creating PointCloud2 from pcl point cloud
+#include "pcl/ros/conversions.h"
+
namespace gazebo
{
@@ -114,7 +117,7 @@
this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->GetValueDouble();
ros::AdvertiseOptions point_cloud_ao =
- ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZ> >(
+ ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZRGB> >(
this->point_cloud_topic_name_,1,
boost::bind( &GazeboRosDepthCamera::PointCloudConnect,this),
boost::bind( &GazeboRosDepthCamera::PointCloudDisconnect,this),
@@ -239,10 +242,12 @@
// Fill depth information
bool GazeboRosDepthCamera::FillPointCloudHelper(
- pcl::PointCloud<pcl::PointXYZ>& point_cloud,
+ sensor_msgs::PointCloud2 &point_cloud_msg,
uint32_t rows_arg, uint32_t cols_arg,
uint32_t step_arg, void* data_arg)
{
+ pcl::PointCloud<pcl::PointXYZRGB> point_cloud;
+
point_cloud.points.resize(0);
point_cloud.is_dense = true;
@@ -276,7 +281,7 @@
// hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
// to urdf, where the *_optical_frame should have above relative
// rotation from the physical camera *_frame
- pcl::PointXYZ point;
+ pcl::PointXYZRGB point;
point.x = depth * tan(yAngle);
point.y = depth * tan(pAngle);
if(depth > this->point_cloud_cutoff_)
@@ -288,9 +293,16 @@
point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
point_cloud.is_dense = false;
}
+
+ point.r = 0;
+ point.g = 0;
+ point.b = 0;
+
point_cloud.points.push_back(point);
}
}
+
+ pcl::toROSMsg(point_cloud, point_cloud_msg);
return true;
}
@@ -338,7 +350,7 @@
// hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
// to urdf, where the *_optical_frame should have above relative
// rotation from the physical camera *_frame
- pcl::PointXYZ point;
+ pcl::PointXYZRGB point;
point.x = depth * tan(yAngle);
point.y = depth * tan(pAngle);
if(depth > this->point_cloud_cutoff_)
Modified: stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp
===================================================================
--- stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp 2012-04-18 21:11:36 UTC (rev 39334)
+++ stacks/simulator_gazebo/trunk/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp 2012-04-19 06:49:02 UTC (rev 39335)
@@ -40,6 +40,9 @@
#include "sdf/interface/SDF.hh"
#include "sensors/SensorTypes.hh"
+// for creating PointCloud2 from pcl point cloud
+#include "pcl/ros/conversions.h"
+
namespace gazebo
{
@@ -102,7 +105,7 @@
this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->GetValueDouble();
ros::AdvertiseOptions point_cloud_ao =
- ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZ> >(
+ ros::AdvertiseOptions::create<pcl::PointCloud<pcl::PointXYZRGB> >(
this->point_cloud_topic_name_,1,
boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this),
boost::bind( &GazeboRosOpenniKinect::PointCloudDisconnect,this),
@@ -281,10 +284,12 @@
// Fill depth information
bool GazeboRosOpenniKinect::FillPointCloudHelper(
- pcl::PointCloud<pcl::PointXYZ>& point_cloud,
+ sensor_msgs::PointCloud2 &point_cloud_msg,
uint32_t rows_arg, uint32_t cols_arg,
uint32_t step_arg, void* data_arg)
{
+
+ pcl::PointCloud<pcl::PointXYZRGB> point_cloud;
point_cloud.points.resize(0);
point_cloud.is_dense = true;
@@ -318,7 +323,7 @@
// hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in
// to urdf, where the *_optical_frame should have above relative
// rotation from the physical camera *_frame
- pcl::PointXYZ point;
+ pcl::PointXYZRGB point;
point.x = depth * tan(yAngle);
point.y = depth * tan(pAngle);
if(depth > this->point_cloud_cutoff_)
@@ -330,9 +335,16 @@
point.x = point.y = point.z = std::numeric_limits<float>::quiet_NaN ();
point_cloud.is_dense = false;
}
+
+ point.r = 0;
+ point.g = 0;
+ point.b = 0;
+
point_cloud.points.push_back(point);
}
}
+
+ pcl::toROSMsg(point_cloud, point_cloud_msg);
return true;
}
_______________________________________________
Ros-pkg-commits mailing list
Ros-pkg-commits@code.ros.org
https://code.ros.org/mailman/listinfo/ros-pkg-commits
(text/plain)
