Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21065] pkg/trunk/stacks
|
From: <jam...@us...> - 2009-08-07 22:14:13
|
Revision: 21065
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21065&view=rev
Author: jamesbowman
Date: 2009-08-07 22:14:04 +0000 (Fri, 07 Aug 2009)
Log Message:
-----------
New sensor_msgs::Image message
Modified Paths:
--------------
pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/fake_poll_node.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_node.cpp
pkg/trunk/stacks/common_msgs/sensor_msgs/include/sensor_msgs/FillImage.h
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Image.msg
pkg/trunk/stacks/opencv/image_view/image_view.cpp
pkg/trunk/stacks/opencv/opencv_latest/include/opencv_latest/CvBridge.h
pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h
pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp
pkg/trunk/stacks/stereo/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/image/ros_image_texture.cpp
Added Paths:
-----------
pkg/trunk/stacks/opencv/opencv_tests/
pkg/trunk/stacks/opencv/opencv_tests/CMakeLists.txt
pkg/trunk/stacks/opencv/opencv_tests/Makefile
pkg/trunk/stacks/opencv/opencv_tests/mainpage.dox
pkg/trunk/stacks/opencv/opencv_tests/manifest.xml
pkg/trunk/stacks/opencv/opencv_tests/nodes/
pkg/trunk/stacks/opencv/opencv_tests/nodes/source.py
pkg/trunk/stacks/opencv/opencv_tests/test/
pkg/trunk/stacks/opencv/opencv_tests/test/enumerants.py
Modified: pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -203,10 +203,9 @@
{
if (img_data->imRawType != COLOR_CODING_NONE)
{
- fillImage(img_, "image_raw",
- img_data->imHeight, img_data->imWidth, 1,
- "mono", "uint8",
- img_data->imRaw );
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data->imHeight, img_data->imWidth, img_data->imWidth,
+ img_data->imRaw);
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
timestamp_diag_.tick(img_.header.stamp);
@@ -215,10 +214,9 @@
if (img_data->imType != COLOR_CODING_NONE)
{
- fillImage(img_, "image",
- img_data->imHeight, img_data->imWidth, 1,
- "mono", "uint8",
- img_data->im );
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data->imHeight, img_data->imWidth, img_data->imWidth,
+ img_data->im);
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
timestamp_diag_.tick(img_.header.stamp);
publish(base_name + std::string("image"), img_);
@@ -226,9 +224,8 @@
if (img_data->imColorType != COLOR_CODING_NONE && img_data->imColorType == COLOR_CODING_RGB8)
{
- fillImage(img_, "image_color",
- img_data->imHeight, img_data->imWidth, 3,
- "rgb", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC3,
+ img_data->imHeight, img_data->imWidth, 3 * img_data->imWidth,
img_data->imColor );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
@@ -238,9 +235,8 @@
if (img_data->imRectType != COLOR_CODING_NONE)
{
- fillImage(img_, "image_rect",
- img_data->imHeight, img_data->imWidth, 1,
- "mono", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data->imHeight, img_data->imWidth, img_data->imWidth,
img_data->imRect );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
timestamp_diag_.tick(img_.header.stamp);
@@ -249,9 +245,8 @@
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGB8)
{
- fillImage(img_, "image_rect_color",
- img_data->imHeight, img_data->imWidth, 3,
- "rgb", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC3,
+ img_data->imHeight, img_data->imWidth, 3 * img_data->imWidth,
img_data->imRectColor );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
timestamp_diag_.tick(img_.header.stamp);
@@ -260,9 +255,8 @@
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGBA8)
{
- fillImage(img_, "image_rect_color",
- img_data->imHeight, img_data->imWidth, 4,
- "rgba", "uint8",
+ fillImage(img_,sensor_msgs::Image::TYPE_8UC4,
+ img_data->imHeight, img_data->imWidth, 4 * img_data->imWidth,
img_data->imRectColor );
img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
timestamp_diag_.tick(img_.header.stamp);
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -954,7 +954,7 @@
int publishImage(size_t width, size_t height, uint8_t *frameData, ros::Time t)
{
- fillImage(image_, "image", height, width, 1, "bayer_bggr", "uint8", frameData);
+ fillImage(image_, sensor_msgs::Image::TYPE_8UC1, height, width, width, frameData);
image_.header.stamp = t;
cam_pub_.publish(image_);
Modified: pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/fake_poll_node.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/fake_poll_node.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/fake_poll_node.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -86,8 +86,9 @@
return false;
// Copy into result
- fillImage(res.image, "image", img.Height(), img.Width(), 3,
- "bgr", "uint8", img.ImageData(), 3, img.WidthStep());
+ fillImage(res.image, sensor_msgs::Image::TYPE_8UC3,
+ img.Height(), img.Width(), 3 * img.Width(),
+ img.ImageData());
// Copy cam info we care about
memcpy(&res.cam_info.D[0], D_->data.db, 5*sizeof(double));
Modified: pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_node.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_node.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_node.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -543,20 +543,11 @@
static void setBgrLayout(sensor_msgs::Image &image, int width, int height)
{
- image.label = "image";
- image.encoding = "bgr";
- image.depth = "uint8";
- image.uint8_data.layout.dim.resize(3);
- image.uint8_data.layout.dim[0].label = "height";
- image.uint8_data.layout.dim[0].size = height;
- image.uint8_data.layout.dim[0].stride = height * (width * 3);
- image.uint8_data.layout.dim[1].label = "width";
- image.uint8_data.layout.dim[1].size = width;
- image.uint8_data.layout.dim[1].stride = width * 3;
- image.uint8_data.layout.dim[2].label = "channel";
- image.uint8_data.layout.dim[2].size = 3;
- image.uint8_data.layout.dim[2].stride = 3;
- image.uint8_data.data.resize(height * (width * 3));
+ image.type = sensor_msgs::Image::TYPE_8UC3;
+ image.rows = height;
+ image.cols = width;
+ image.step = width * 3;
+ image.data.resize(height * (width * 3));
}
static bool frameToImage(tPvFrame* frame, sensor_msgs::Image &image)
@@ -564,41 +555,42 @@
// NOTE: 16-bit formats and Yuv444 not supported
switch (frame->Format)
{
+
case ePvFmtMono8:
- fillImage(image, "image", frame->Height, frame->Width, 1,
- "mono", "uint8", frame->ImageBuffer);
+ fillImage(image, sensor_msgs::Image::TYPE_8UC1, frame->Height, frame->Width, frame->Width, frame->ImageBuffer);
break;
+
case ePvFmtBayer8:
// Debayer to bgr format so CvBridge can handle it
setBgrLayout(image, frame->Width, frame->Height);
- PvUtilityColorInterpolate(frame, &image.uint8_data.data[2],
- &image.uint8_data.data[1], &image.uint8_data.data[0],
+ PvUtilityColorInterpolate(frame, &image.data[2],
+ &image.data[1], &image.data[0],
2, 0);
break;
- case ePvFmtRgb24:
- fillImage(image, "image", frame->Height, frame->Width, 3,
- "rgb", "uint8", frame->ImageBuffer);
- break;
- case ePvFmtYuv411:
- fillImage(image, "image", frame->Height, frame->Width, 6,
- "yuv411", "uint8", frame->ImageBuffer);
- break;
- case ePvFmtYuv422:
- fillImage(image, "image", frame->Height, frame->Width, 4,
- "yuv422", "uint8", frame->ImageBuffer);
- break;
+
case ePvFmtBgr24:
- fillImage(image, "image", frame->Height, frame->Width, 3,
- "bgr", "uint8", frame->ImageBuffer);
+ fillImage(image, sensor_msgs::Image::TYPE_8UC3, frame->Height, frame->Width, 3 * frame->Width, frame->ImageBuffer);
break;
+
case ePvFmtRgba32:
- fillImage(image, "image", frame->Height, frame->Width, 4,
- "rgba", "uint8", frame->ImageBuffer);
+ fillImage(image, sensor_msgs::Image::TYPE_8UC4, frame->Height, frame->Width, 4 * frame->Width, frame->ImageBuffer);
break;
+
case ePvFmtBgra32:
- fillImage(image, "image", frame->Height, frame->Width, 4,
- "bgra", "uint8", frame->ImageBuffer);
+ fillImage(image, sensor_msgs::Image::TYPE_8UC4, frame->Height, frame->Width, 4 * frame->Width, frame->ImageBuffer);
break;
+
+#if 0 // XXX JCB - these cannot be represented in a CvMat
+ case ePvFmtRgb24:
+ // fillImage(image, sensor_msgs::Image::TYPE_8UC3, frame->Height, frame->Width, 3 * frame->Width, frame->ImageBuffer);
+ break;
+ case ePvFmtYuv411:
+ // fillImage(image, "image", frame->Height, frame->Width, 6, "yuv411", "uint8", frame->ImageBuffer);
+ break;
+ case ePvFmtYuv422:
+ // fillImage(image, "image", frame->Height, frame->Width, 4, "yuv422", "uint8", frame->ImageBuffer);
+ break;
+#endif
default:
ROS_WARN("Received frame with unsupported pixel format %d", frame->Format);
return false;
@@ -611,8 +603,8 @@
sensor_msgs::CameraInfo &cam_info)
{
// Currently assume BGR format so bridge.toIpl() image points to msg data buffer
- if (img.encoding != "bgr") {
- ROS_WARN("Couldn't rectify frame, unsupported encoding %s", img.encoding.c_str());
+ if (img.type != sensor_msgs::Image::TYPE_8UC1) {
+ ROS_WARN("Couldn't rectify frame, unsupported encoding %d", img.type);
return false;
}
Modified: pkg/trunk/stacks/common_msgs/sensor_msgs/include/sensor_msgs/FillImage.h
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/include/sensor_msgs/FillImage.h 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/include/sensor_msgs/FillImage.h 2009-08-07 22:14:04 UTC (rev 21065)
@@ -40,105 +40,28 @@
namespace sensor_msgs
{
- template <class M>
- void fillImageHelper(M &m,
- uint32_t sz0, uint32_t st0,
- uint32_t sz1, uint32_t st1,
- uint32_t sz2, uint32_t st2,
- void *d)
- {
- m.layout.dim.resize(3);
- m.layout.dim[0].label = "height";
- m.layout.dim[0].size = sz0;
- m.layout.dim[0].stride = st0;
- m.layout.dim[1].label = "width";
- m.layout.dim[1].size = sz1;
- m.layout.dim[1].stride = st1;
- m.layout.dim[2].label = "channel";
- m.layout.dim[2].size = sz2;
- m.layout.dim[2].stride = st2;
- m.data.resize(st0);
- memcpy((char*)(&m.data[0]), (char*)(d), st0*sizeof(m.data[0]));
- }
-
- template <class M>
- void clearImageHelper(M &m)
- {
- m.layout.dim.resize(0);
- m.data.resize(0);
- }
-
bool fillImage(Image& image,
- std::string label_arg,
- uint32_t height_arg, uint32_t width_arg, uint32_t channel_arg,
- std::string encoding_arg, std::string depth_arg,
- void* data_arg,
- uint32_t channel_step = 0, uint32_t width_step = 0, uint32_t height_step = 0)
+ uint32_t type_arg,
+ uint32_t rows_arg,
+ uint32_t cols_arg,
+ uint32_t step_arg,
+ void* data_arg)
{
- image.label = label_arg;
- image.encoding = encoding_arg;
- image.depth = depth_arg;
-
- if (channel_step == 0)
- channel_step = channel_arg;
-
- if (width_step == 0)
- width_step = width_arg * channel_step;
-
- if (height_step == 0)
- height_step = height_arg * width_step;
-
- if (image.depth == "uint8")
- fillImageHelper(image.uint8_data,
- height_arg, height_step,
- width_arg, width_step,
- channel_arg, channel_step,
- data_arg);
-
- else if (image.depth == "uint16")
- fillImageHelper(image.uint16_data,
- height_arg, height_step,
- width_arg, width_step,
- channel_arg, channel_step,
- data_arg);
+ image.type = type_arg;
+ image.rows = rows_arg;
+ image.cols = cols_arg;
+ image.step = step_arg;
+ size_t st0 = (step_arg * rows_arg);
+ image.data.resize(st0);
+ memcpy((char*)(&image.data[0]), (char*)(data_arg), st0);
- else if (image.depth == "int16")
- fillImageHelper(image.int16_data,
- height_arg, height_step,
- width_arg, width_step,
- channel_arg, channel_step,
- data_arg);
- else
- {
- return false;
- }
-
+ image.is_bigendian = 0;
return true;
}
void clearImage(Image& image)
{
- image.label = "none";
- image.encoding = "other";
- if (image.depth == "uint8")
- clearImageHelper(image.uint8_data);
- else if (image.depth == "uint16")
- clearImageHelper(image.uint16_data);
- else if (image.depth == "int16")
- clearImageHelper(image.int16_data);
- else if (image.depth == "uint32")
- clearImageHelper(image.uint32_data);
- else if (image.depth == "int32")
- clearImageHelper(image.int32_data);
- else if (image.depth == "uint64")
- clearImageHelper(image.uint64_data);
- else if (image.depth == "int64")
- clearImageHelper(image.int64_data);
- else if (image.depth == "float32")
- clearImageHelper(image.float32_data);
- else if (image.depth == "float64")
- clearImageHelper(image.float64_data);
- image.depth = "none";
+ image.data.resize(0);
}
}
Modified: pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Image.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Image.msg 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Image.msg 2009-08-07 22:14:04 UTC (rev 21065)
@@ -1,38 +1,37 @@
Header header # Header
-string label # Label for the image
-string encoding # Specifies the color encoding of the data
- # Acceptable values are:
- # 1 channel types:
- # mono, bayer_rggb, bayer_gbrg, bayer_grbg, bayer_bggr
- # 3 channel types:
- # rgb, bgr
- # 4 channel types:
- # rgba, bgra, yuv422
- # 6 channel types:
- # yuv411
- # N channel types:
- # other
-string depth # Specifies the depth of the data:
- # Acceptable values:
- # uint8, int8, uint16, int16, uint32, int32, uint64, int64, float32, float64
-# Based on depth ONE of the following MultiArrays may contain data.
-# The multi-array MUST have 3 dimensions, labeled as "height",
-# "width", and "channel", though depending on usage the ordering of
-# the dimensions may very. Note that IPL Image convention will order
-# these as: height, width, channel, which is the preferred ordering
-# unless performance dictates otherwise.
-#
-# Height, width, and number of channels are specified in the dimension
-# sizes within the appropriate MultiArray
+int32 TYPE_8UC1=0
+int32 TYPE_8UC2=8
+int32 TYPE_8UC3=16
+int32 TYPE_8UC4=24
+int32 TYPE_8SC1=1
+int32 TYPE_8SC2=9
+int32 TYPE_8SC3=17
+int32 TYPE_8SC4=25
+int32 TYPE_16UC1=2
+int32 TYPE_16UC2=10
+int32 TYPE_16UC3=18
+int32 TYPE_16UC4=26
+int32 TYPE_16SC1=3
+int32 TYPE_16SC2=11
+int32 TYPE_16SC3=19
+int32 TYPE_16SC4=27
+int32 TYPE_32SC1=4
+int32 TYPE_32SC2=12
+int32 TYPE_32SC3=20
+int32 TYPE_32SC4=28
+int32 TYPE_32FC1=5
+int32 TYPE_32FC2=13
+int32 TYPE_32FC3=21
+int32 TYPE_32FC4=29
+int32 TYPE_64FC1=6
+int32 TYPE_64FC2=14
+int32 TYPE_64FC3=22
+int32 TYPE_64FC4=30
-std_msgs/UInt8MultiArray uint8_data
-std_msgs/Int8MultiArray int8_data
-std_msgs/UInt16MultiArray uint16_data
-std_msgs/Int16MultiArray int16_data
-std_msgs/UInt32MultiArray uint32_data
-std_msgs/Int32MultiArray int32_data
-std_msgs/UInt64MultiArray uint64_data
-std_msgs/Int64MultiArray int64_data
-std_msgs/Float32MultiArray float32_data
-std_msgs/Float64MultiArray float64_data
+int32 type # Type of elements, and channels. Encoded as in OpenCV's TYPE_8UC1 .. TYPE_64FC4.
+int32 step # Full row length in bytes
+uint8[] data # actual matrix data, size is (step * rows)
+int32 rows # Number of rows
+int32 cols # Number of columns
+uint8 is_bigendian # is this data bigendian
Modified: pkg/trunk/stacks/opencv/image_view/image_view.cpp
===================================================================
--- pkg/trunk/stacks/opencv/image_view/image_view.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/opencv/image_view/image_view.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -98,7 +98,7 @@
if (img_bridge_.fromImage(*msg, "bgr"))
cvShowImage(window_name_.c_str(), img_bridge_.toIpl());
else
- ROS_ERROR("Unable to convert from %s to bgr", msg->encoding.c_str());
+ ROS_ERROR("Unable to convert from %d to bgr", msg->type);
}
static void mouse_cb(int event, int x, int y, int flags, void* param)
Modified: pkg/trunk/stacks/opencv/opencv_latest/include/opencv_latest/CvBridge.h
===================================================================
--- pkg/trunk/stacks/opencv/opencv_latest/include/opencv_latest/CvBridge.h 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/opencv/opencv_latest/include/opencv_latest/CvBridge.h 2009-08-07 22:14:04 UTC (rev 21065)
@@ -44,6 +44,8 @@
class CvBridge
{
+ public:
+
IplImage* img_;
IplImage* rosimg_;
IplImage* cvtimg_;
@@ -68,8 +70,6 @@
return false;
}
- public:
-
CvBridge() : img_(0), rosimg_(0), cvtimg_(0)
{
rosimg_ = cvCreateImageHeader( cvSize(0,0), IPL_DEPTH_8U, 1 );
@@ -88,68 +88,41 @@
}
}
-
inline IplImage* toIpl()
{
return img_;
}
+ /**
+ * Converts a ROS Image into an OpenCV IPL Image.
+ * \param rosimg The ROS Image message
+ */
bool fromImage(const Image& rosimg, std::string encoding = "")
{
- unsigned int depth;
- if (rosimg.depth == "uint8")
- {
- depth = IPL_DEPTH_8U;
- cvInitImageHeader(rosimg_, cvSize(rosimg.uint8_data.layout.dim[1].size, rosimg.uint8_data.layout.dim[0].size),
- IPL_DEPTH_8U, rosimg.uint8_data.layout.dim[2].size);
- cvSetData(rosimg_, const_cast<uint8_t*>(&(rosimg.uint8_data.data[0])), rosimg.uint8_data.layout.dim[1].stride);
+ CvMat cvmHeader;
+
+ cvInitMatHeader(&cvmHeader, rosimg.rows, rosimg.cols, rosimg.type, const_cast<uint8_t*>(&(rosimg.data[0])), rosimg.step);
+ cvGetImage(&cvmHeader, rosimg_);
+
+ // cvSetData(rosimg_, const_cast<uint8_t*>(&(rosimg.data[0])), rosimg.step);
+
+ if (encoding == "") {
img_ = rosimg_;
- } else if (rosimg.depth == "uint16") {
- depth = IPL_DEPTH_16U;
- cvInitImageHeader(rosimg_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size),
- IPL_DEPTH_16U, rosimg.uint16_data.layout.dim[2].size);
- cvSetData(rosimg_, const_cast<uint16_t*>(&(rosimg.uint16_data.data[0])), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
- img_ = rosimg_;
} else {
- return false;
- }
-
- if (encoding != "" && (encoding != rosimg.encoding))
- {
- if (encoding == "bgr" && rosimg.encoding == "rgb")
+ if ((encoding == "bgr" || encoding == "rgb") && rosimg.type == CV_8UC1)
{
- reallocIfNeeded(&cvtimg_, depth, 3);
- cvCvtColor(rosimg_, cvtimg_, CV_RGB2BGR);
- img_ = cvtimg_;
- }
- else if (encoding == "rgb" && rosimg.encoding == "bgr")
- {
- reallocIfNeeded(&cvtimg_, depth, 3);
- cvCvtColor(rosimg_, cvtimg_, CV_BGR2RGB);
- img_ = cvtimg_;
- }
- else if (encoding == "bgr" && rosimg.encoding == "mono" )
- {
- reallocIfNeeded(&cvtimg_, depth, 3);
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 3);
cvCvtColor(rosimg_, cvtimg_, CV_GRAY2BGR);
img_ = cvtimg_;
}
- else if (encoding == "mono" && rosimg.encoding == "rgb" )
+ else if (encoding == "mono" && rosimg.type == CV_8UC3)
{
- reallocIfNeeded(&cvtimg_, depth, 1);
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 1);
cvCvtColor(rosimg_, cvtimg_, CV_RGB2GRAY);
img_ = cvtimg_;
+ } else {
+ img_ = rosimg_;
}
- else if (encoding == "mono" && rosimg.encoding == "bgr" )
- {
- reallocIfNeeded(&cvtimg_, depth, 1);
- cvCvtColor(rosimg_, cvtimg_, CV_BGR2GRAY);
- img_ = cvtimg_;
- }
- else
- {
- return false;
- }
}
return true;
}
@@ -164,59 +137,23 @@
}
/**
- * Converts an openCV IPL Image into a ROS Image that can be sent 'over the wire'.
- * Note that this hasn't been rigorously tested
+ * Converts an OpenCV IPL Image into a ROS Image that can be sent 'over the wire'.
* \param source The original Ipl Image that we want to copy from
* \param dest The ROS Image message that we want to copy to
*/
static bool fromIpltoRosImage(const IplImage* source, sensor_msgs::Image& dest)
{
- switch(source->nChannels)
- {
- case 1: dest.encoding = "mono"; break;
- case 3: dest.encoding = "rgb"; break;
- case 4: dest.encoding = "rgba"; break;
- default:
- ROS_ERROR("unknown image format\n");
- return false;
- }
- switch(source->depth)
- {
- case IPL_DEPTH_8U : dest.depth = "uint8"; fillImageHelperCV(dest.uint8_data, source); break;
- case IPL_DEPTH_8S : dest.depth = "int8"; fillImageHelperCV(dest.int8_data, source); break;
- case IPL_DEPTH_16U: dest.depth = "uint16"; fillImageHelperCV(dest.uint16_data, source); break;
- case IPL_DEPTH_16S: dest.depth = "int16"; fillImageHelperCV(dest.int16_data, source); break;
- case IPL_DEPTH_32S: dest.depth = "int32"; fillImageHelperCV(dest.int32_data, source); break;
- case IPL_DEPTH_32F: dest.depth = "float32"; fillImageHelperCV(dest.float32_data, source); break;
- case IPL_DEPTH_64F: dest.depth = "float64"; fillImageHelperCV(dest.float64_data, source); break;
- default:
- ROS_ERROR("unsupported depth %d\n", source->depth);
- return false;
- }
+ CvMat header, *cvm;
+
+ cvm = cvGetMat(source, &header);
+ dest.type = cvm->type & (CV_MAT_TYPE_MASK | CV_MAT_DEPTH_MASK);
+ dest.cols = cvm->cols;
+ dest.rows = cvm->rows;
+ dest.step = cvm->step;
+ dest.data.resize(cvm->step * cvm->rows);
+ memcpy((char*)(&dest.data[0]), source->imageData, cvm->step * cvm->rows);
return true;
}
-
- private:
- /**
- * Helper method used by fromIplToRosImage in order to populate the images
- */
- template <typename T>
- static void fillImageHelperCV(T& m, const IplImage* frame)
- {
- m.layout.dim.resize(3);
- m.layout.dim.resize(3);
- m.layout.dim[0].label = "height";
- m.layout.dim[0].size = frame->height;
- m.layout.dim[0].stride = frame->widthStep*frame->height/sizeof(m.data[0]);
- m.layout.dim[1].label = "width";
- m.layout.dim[1].size = frame->width;
- m.layout.dim[1].stride = frame->widthStep/sizeof(m.data[0]);
- m.layout.dim[2].label = "channel";
- m.layout.dim[2].size = frame->nChannels;
- m.layout.dim[2].stride = frame->nChannels*sizeof(m.data[0]);
- m.data.resize(frame->widthStep*frame->height/sizeof(m.data[0]));
- memcpy((char*)(&m.data[0]), frame->imageData, m.data.size()*sizeof(m.data[0]));
- }
};
}
Added: pkg/trunk/stacks/opencv/opencv_tests/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/opencv/opencv_tests/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/opencv/opencv_tests/CMakeLists.txt 2009-08-07 22:14:04 UTC (rev 21065)
@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(opencv_tests)
+
+#set the default path for built executables to the "bin" directory
+#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#genmsg()
+#uncomment if you have defined services
+#gensrv()
+
+#common commands for building c++ executables and libraries
+#rospack_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rospack_add_boost_directories()
+#rospack_link_boost(${PROJECT_NAME} thread)
+#rospack_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+rospack_add_gtest(test/utest test/utest.cpp)
+rospack_add_pyunit(test/enumerants.py)
+
Added: pkg/trunk/stacks/opencv/opencv_tests/Makefile
===================================================================
--- pkg/trunk/stacks/opencv/opencv_tests/Makefile (rev 0)
+++ pkg/trunk/stacks/opencv/opencv_tests/Makefile 2009-08-07 22:14:04 UTC (rev 21065)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/stacks/opencv/opencv_tests/mainpage.dox
===================================================================
--- pkg/trunk/stacks/opencv/opencv_tests/mainpage.dox (rev 0)
+++ pkg/trunk/stacks/opencv/opencv_tests/mainpage.dox 2009-08-07 22:14:04 UTC (rev 21065)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b opencv_tests is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/stacks/opencv/opencv_tests/manifest.xml
===================================================================
--- pkg/trunk/stacks/opencv/opencv_tests/manifest.xml (rev 0)
+++ pkg/trunk/stacks/opencv/opencv_tests/manifest.xml 2009-08-07 22:14:04 UTC (rev 21065)
@@ -0,0 +1,20 @@
+<package>
+ <description brief="opencv_tests">
+
+ opencv_tests
+
+ </description>
+ <author>James Bowman</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/opencv_tests</url>
+ <depend package="opencv_latest"/>
+ <depend package="opencvpython"/>
+ <depend package="rostest"/>
+ <depend package="rospy"/>
+ <depend package="opencvpython"/>
+ <depend package="sensor_msgs"/>
+
+</package>
+
+
Added: pkg/trunk/stacks/opencv/opencv_tests/nodes/source.py
===================================================================
--- pkg/trunk/stacks/opencv/opencv_tests/nodes/source.py (rev 0)
+++ pkg/trunk/stacks/opencv/opencv_tests/nodes/source.py 2009-08-07 22:14:04 UTC (rev 21065)
@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import roslib
+roslib.load_manifest('opencv_tests')
+
+import sys
+import time
+import math
+import rospy
+import cv
+
+import sensor_msgs.msg
+
+class source:
+
+ def __init__(self):
+ self.pub = rospy.Publisher("/opencv_tests/images", sensor_msgs.msg.Image)
+
+ def spin(self):
+ time.sleep(1.0)
+ started = time.time()
+ counter = 0
+ cvim = cv.CreateImage((640,480), cv.IPL_DEPTH_8U, 1)
+ ball_xv = 10
+ ball_yv = 10
+ ball_x = 100
+ ball_y = 100
+ while not rospy.core.is_shutdown():
+
+ cv.Set(cvim, 0)
+ cv.Circle(cvim, (ball_x, ball_y), 10, 255, -1)
+
+ ball_x += ball_xv
+ ball_y += ball_yv
+ if ball_x in [10, 630]:
+ ball_xv = -ball_xv
+ if ball_y in [10, 470]:
+ ball_yv = -ball_yv
+
+ img_msg = sensor_msgs.msg.Image()
+ img_msg.rows = 480
+ img_msg.cols = 640
+ img_msg.type = cv.CV_8UC1
+ img_msg.step = 640
+ img_msg.data = cvim.tostring()
+ self.pub.publish(img_msg)
+
+ time.sleep(0.03)
+
+def main(args):
+ s = source()
+ rospy.init_node('source')
+ try:
+ s.spin()
+ rospy.spin()
+ outcome = 'test completed'
+ except KeyboardInterrupt:
+ print "shutting down"
+ outcome = 'keyboard interrupt'
+ rospy.core.signal_shutdown(outcome)
+
+if __name__ == '__main__':
+ main(sys.argv)
+
Added: pkg/trunk/stacks/opencv/opencv_tests/test/enumerants.py
===================================================================
--- pkg/trunk/stacks/opencv/opencv_tests/test/enumerants.py (rev 0)
+++ pkg/trunk/stacks/opencv/opencv_tests/test/enumerants.py 2009-08-07 22:14:04 UTC (rev 21065)
@@ -0,0 +1,26 @@
+import roslib
+roslib.load_manifest('opencv_tests')
+import rostest
+import rospy
+import unittest
+
+import sensor_msgs.msg
+import cv
+
+class TestEnumerants(unittest.TestCase):
+
+ def test_enumerants(self):
+ for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F" ]:
+ for c in [1,2,3,4]:
+ nm = "%sC%d" % (t, c)
+ cv_num = eval("cv.CV_%s" % nm)
+ msg_num = eval("sensor_msgs.msg.Image.TYPE_%s" % nm)
+ self.assertEqual(cv_num, msg_num)
+
+if __name__ == '__main__':
+ if 1:
+ rostest.unitrun('opencv_tests', 'enumerants', TestEnumerants)
+ else:
+ suite = unittest.TestSuite()
+ suite.addTest(TestEnumerants('test_enumerants'))
+ unittest.TextTestRunner(verbosity=2).run(suite)
Modified: pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h 2009-08-07 22:14:04 UTC (rev 21065)
@@ -45,58 +45,37 @@
{
if (im->imRawType != COLOR_CODING_NONE)
{
- fillImage(im_msg, "image_raw",
- im->imHeight, im->imWidth, 1,
- "mono", "uint8",
- im->imRaw );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC1, im->imHeight, im->imWidth, im->imWidth, im->imRaw);
type = sensor_msgs::RawStereo::IMAGE_RAW;
}
else if (im->imType != COLOR_CODING_NONE)
{
- fillImage(im_msg, "image",
- im->imHeight, im->imWidth, 1,
- "mono", "uint8",
- im->im );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC1, im->imHeight, im->imWidth, im->imWidth, im->im);
type = sensor_msgs::RawStereo::IMAGE;
}
else if (im->imColorType != COLOR_CODING_NONE && im->imColorType == COLOR_CODING_RGBA8)
{
- fillImage(im_msg, "image_color",
- im->imHeight, im->imWidth, 4,
- "rgba", "uint8",
- im->imColor );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC4, im->imHeight, im->imWidth, 4 * im->imWidth, im->imColor);
type = sensor_msgs::RawStereo::IMAGE_COLOR;
}
else if (im->imColorType != COLOR_CODING_NONE && im->imColorType == COLOR_CODING_RGB8)
{
- fillImage(im_msg, "image_color",
- im->imHeight, im->imWidth, 3,
- "rgb", "uint8",
- im->imColor );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC3, im->imHeight, im->imWidth, 3 * im->imWidth, im->imColor);
type = sensor_msgs::RawStereo::IMAGE_COLOR;
}
else if (im->imRectType != COLOR_CODING_NONE)
{
- fillImage(im_msg, "image_rect",
- im->imHeight, im->imWidth, 1,
- "mono", "uint8",
- im->imRect );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC1, im->imHeight, im->imWidth, im->imWidth, im->imRect);
type = sensor_msgs::RawStereo::IMAGE_RECT;
}
else if (im->imRectColorType != COLOR_CODING_NONE && im->imRectColorType == COLOR_CODING_RGBA8)
{
- fillImage(im_msg, "image_rect_color",
- im->imHeight, im->imWidth, 4,
- "rgba", "uint8",
- im->imRectColor );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC4, im->imHeight, im->imWidth, 4 * im->imWidth, im->imRectColor);
type = sensor_msgs::RawStereo::IMAGE_RECT_COLOR;
}
else if (im->imRectColorType != COLOR_CODING_NONE && im->imRectColorType == COLOR_CODING_RGB8)
{
- fillImage(im_msg, "image_rect_color",
- im->imHeight, im->imWidth, 3,
- "rgb", "uint8",
- im->imRectColor );
+ fillImage(im_msg, sensor_msgs::Image::TYPE_8UC3, im->imHeight, im->imWidth, 3 * im->imWidth, im->imRectColor);
type = sensor_msgs::RawStereo::IMAGE_RECT_COLOR;
}
@@ -115,9 +94,11 @@
if (stIm->hasDisparity)
{
- fillImage(raw_stereo.disparity_image, "disparity",
- stIm->imHeight, stIm->imWidth, 1,
- "mono", "int16",
+ fillImage(raw_stereo.disparity_image,
+ sensor_msgs::Image::TYPE_16SC1,
+ stIm->imWidth,
+ stIm->imHeight,
+ 2 * stIm->imWidth,
stIm->imDisp );
raw_stereo.has_disparity = true;
@@ -157,9 +138,9 @@
}
- void extractImage(std_msgs::UInt8MultiArray& arr, size_t* sz, uint8_t **d)
+ void extractImage(std::vector<uint8_t> data, size_t* sz, uint8_t **d)
{
- size_t new_size = arr.layout.dim[0].stride;
+ size_t new_size = data.size();
if (*sz < new_size)
{
@@ -167,12 +148,11 @@
*d = (uint8_t *)MEMALIGN(new_size);
*sz = new_size;
}
- memcpy((char*)(*d), (char*)(&arr.data[0]), new_size);
+ memcpy((char*)(*d), (char*)(&data[0]), new_size);
}
-
- void extractImage(std_msgs::Int16MultiArray& arr, size_t* sz, int16_t **d)
+ void extractImage(std::vector<uint8_t> data, size_t* sz, int16_t **d)
{
- size_t new_size = arr.layout.dim[0].stride*2;
+ size_t new_size = data.size();
if (*sz < new_size)
{
@@ -180,7 +160,7 @@
*d = (int16_t *)MEMALIGN(new_size);
*sz = new_size;
}
- memcpy((char*)(*d), (char*)(&arr.data[0]), new_size);
+ memcpy((char*)(*d), (char*)(&data[0]), new_size);
}
void RawStereoToCamData(sensor_msgs::Image& im_msg, sensor_msgs::CameraInfo& info_msg, uint8_t type, cam::ImageData* im)
@@ -194,45 +174,45 @@
if (type == sensor_msgs::RawStereo::IMAGE_RAW)
{
- extractImage(im_msg.uint8_data, &im->imRawSize, &im->imRaw);
+ extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
im->imRawType = COLOR_CODING_BAYER8_GRBG;
}
else if (type == sensor_msgs::RawStereo::IMAGE)
{
- extractImage(im_msg.uint8_data, &im->imSize, &im->im);
+ extractImage(im_msg.data, &im->imSize, &im->im);
im->imType = COLOR_CODING_MONO8;
}
- else if (type == sensor_msgs::RawStereo::IMAGE_COLOR && im_msg.encoding == "rgba")
+ else if (type == sensor_msgs::RawStereo::IMAGE_COLOR && im_msg.type == sensor_msgs::Image::TYPE_8UC4)
{
- extractImage(im_msg.uint8_data, &im->imColorSize, &im->imColor);
+ extractImage(im_msg.data, &im->imColorSize, &im->imColor);
im->imColorType = COLOR_CODING_RGBA8;
}
- else if (type == sensor_msgs::RawStereo::IMAGE_COLOR && im_msg.encoding == "rgb")
+ else if (type == sensor_msgs::RawStereo::IMAGE_COLOR && im_msg.type == sensor_msgs::Image::TYPE_8UC3)
{
- extractImage(im_msg.uint8_data, &im->imColorSize, &im->imColor);
+ extractImage(im_msg.data, &im->imColorSize, &im->imColor);
im->imColorType = COLOR_CODING_RGB8;
}
else if (type == sensor_msgs::RawStereo::IMAGE_RECT)
{
- extractImage(im_msg.uint8_data, &im->imRectSize, &im->imRect);
+ extractImage(im_msg.data, &im->imRectSize, &im->imRect);
im->imRectType = COLOR_CODING_MONO8;
}
- else if (type == sensor_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.encoding == "rgba")
+ else if (type == sensor_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.type == sensor_msgs::Image::TYPE_8UC4)
{
- extractImage(im_msg.uint8_data, &im->imRectColorSize, &im->imRectColor);
+ extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
im->imRectColorType = COLOR_CODING_RGBA8;
}
- else if (type == sensor_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.encoding == "rgb")
+ else if (type == sensor_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.type == sensor_msgs::Image::TYPE_8UC3)
{
- extractImage(im_msg.uint8_data, &im->imRectColorSize, &im->imRectColor);
+ extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
im->imRectColorType = COLOR_CODING_RGB8;
}
// FIXME: this to avoid segfault when right image empty (disparity image requested instead).
// this all seems kind of hacky
- if (im_msg.depth == "uint8") {
- im->imHeight = im_msg.uint8_data.layout.dim[0].size;
- im->imWidth = im_msg.uint8_data.layout.dim[1].size;
+ if (im_msg.type == sensor_msgs::Image::TYPE_8UC1) {
+ im->imHeight = im_msg.rows;
+ im->imWidth = im_msg.cols;
} else {
im->imHeight = info_msg.height;
im->imWidth = info_msg.width;
@@ -255,7 +235,7 @@
if (raw_stereo.has_disparity)
{
- extractImage(raw_stereo.disparity_image.int16_data, &stIm->imDispSize, &stIm->imDisp);
+ extractImage(raw_stereo.disparity_image.data, &stIm->imDispSize, &stIm->imDisp);
stIm->hasDisparity = true;
stIm->dpp = raw_stereo.disparity_info.dpp;
Modified: pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -109,46 +109,41 @@
if (img_data_.imType != COLOR_CODING_NONE)
{
- fillImage(img_, "image",
- img_data_.imHeight, img_data_.imWidth, 1,
- "mono", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data_.imHeight, img_data_.imWidth, img_data_.imWidth,
img_data_.im );
node_.publish(cam_name_ + "image", img_);
}
if (img_data_.imColorType == COLOR_CODING_RGB8)
{
- fillImage(img_, "image_color",
- img_data_.imHeight, img_data_.imWidth, 3,
- "rgb", "uint8",
+ fillImage(img_,sensor_msgs::Image::TYPE_8UC3,
+ img_data_.imHeight, img_data_.imWidth, 3 * img_data_.imWidth,
img_data_.imColor );
node_.publish(cam_name_ + "image_color", img_);
}
if (img_data_.imRectType != COLOR_CODING_NONE)
{
- fillImage(img_, "image_rect",
- img_data_.imHeight, img_data_.imWidth, 1,
- "mono", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data_.imHeight, img_data_.imWidth, img_data_.imWidth,
img_data_.imRect );
node_.publish(cam_name_ + "image_rect", img_);
}
if (img_data_.imRectColorType == COLOR_CODING_RGB8)
{
- fillImage(img_, "image_rect_color",
- img_data_.imHeight, img_data_.imWidth, 3,
- "rgb", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC3,
+ img_data_.imHeight, img_data_.imWidth, 3 * img_data_.imWidth,
img_data_.imRectColor );
node_.publish(cam_name_ + "image_rect_color", img_);
}
if (img_data_.imRectColorType == COLOR_CODING_RGBA8)
{
- fillImage(img_, "image_rect_color",
- img_data_.imHeight, img_data_.imWidth, 4,
- "rgba", "uint8",
- img_data_.imRectColor );
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC4,
+ img_data_.imHeight, img_data_.imWidth, 4 * img_data_.imWidth,
+ img_data_.imRectColor);
node_.publish(cam_name_ + "image_rect_color", img_);
}
}
Modified: pkg/trunk/stacks/stereo/stereo_image_proc/src/stereoproc.cpp
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/src/stereoproc.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/src/stereoproc.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -214,10 +214,7 @@
publish("disparity_info", disparity_info_);
- fillImage(img_, "disparity",
- stdata_->imHeight, stdata_->imWidth, 1,
- "mono", "uint16",
- stdata_->imDisp );
+ fillImage(img_, sensor_msgs::Image::TYPE_16SC1, stdata_->imHeight, stdata_->imWidth, 2 * stdata_->imWidth, stdata_->imDisp);
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
@@ -280,10 +277,7 @@
{
if (img_data->imRawType != COLOR_CODING_NONE)
{
- fillImage(img_, "image_raw",
- img_data->imHeight, img_data->imWidth, 1,
- "mono", "uint8",
- img_data->imRaw );
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1, img_data->imHeight, img_data->imWidth, img_data->imWidth, img_data->imRaw);
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
@@ -293,10 +287,9 @@
if (img_data->imType != COLOR_CODING_NONE)
{
- fillImage(img_, "image",
- img_data->imHeight, img_data->imWidth, 1,
- "mono", "uint8",
- img_data->im );
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data->imHeight, img_data->imWidth, 2 * img_data->imWidth,
+ img_data->im);
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image"), img_);
@@ -304,9 +297,8 @@
if (img_data->imColorType != COLOR_CODING_NONE && img_data->imColorType == COLOR_CODING_RGB8)
{
- fillImage(img_, "image_color",
- img_data->imHeight, img_data->imWidth, 3,
- "rgb", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC3,
+ img_data->imHeight, img_data->imWidth, 3 * img_data->imWidth,
img_data->imColor );
img_.header.stamp = raw_stereo_.header.stamp;
@@ -316,9 +308,8 @@
if (img_data->imRectType != COLOR_CODING_NONE)
{
- fillImage(img_, "image_rect",
- img_data->imHeight, img_data->imWidth, 1,
- "mono", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC1,
+ img_data->imHeight, img_data->imWidth, img_data->imWidth,
img_data->imRect );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
@@ -327,10 +318,9 @@
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGB8)
{
- fillImage(img_, "image_rect_color",
- img_data->imHeight, img_data->imWidth, 3,
- "rgb", "uint8",
- img_data->imRectColor );
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC3,
+ img_data->imHeight, img_data->imWidth, 3 * img_data->imWidth,
+ img_data->imRectColor);
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
publish(base_name + std::string("image_rect_color"), img_);
@@ -338,9 +328,8 @@
if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGBA8)
{
- fillImage(img_, "image_rect_color",
- img_data->imHeight, img_data->imWidth, 4,
- "rgba", "uint8",
+ fillImage(img_, sensor_msgs::Image::TYPE_8UC4,
+ img_data->imHeight, img_data->imWidth, 4 * img_data->imWidth,
img_data->imRectColor );
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/image/ros_image_texture.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/image/ros_image_texture.cpp 2009-08-07 22:11:46 UTC (rev 21064)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/image/ros_image_texture.cpp 2009-08-07 22:14:04 UTC (rev 21065)
@@ -92,60 +92,26 @@
return false;
}
- if (image->depth != "uint8")
+ if (image->type == sensor_msgs::Image::TYPE_8UC4)
{
- ROS_ERROR("Unsupported image depth [%s]", image->depth.c_str());
- return false;
- }
-
- if (image->uint8_data.layout.dim.size() != 3)
- {
- ROS_ERROR("Unsupported # of dimensions [%d]", image->uint8_data.layout.dim.size());
- return false;
- }
-
- if (image->uint8_data.layout.dim[0].label != "height"
- || image->uint8_data.layout.dim[1].label != "width"
- || image->uint8_data.layout.dim[2].label != "channel")
- {
- ROS_ERROR("Unsupported image layout. Currently only 0=height/1=width/2=channel is supported");
- return false;
- }
-
- Ogre::PixelFormat format = Ogre::PF_R8G8B8;
-
- if (image->encoding == "rgb")
- {
- format = Ogre::PF_R8G8B8;
- }
- else if (image->encoding == "bgr")
- {
- format = Ogre::PF_B8G8R8;
- }
- else if (image->encoding == "rgba")
- {
- format = Ogre::PF_R8G8B8A8;
- }
- else if (image->encoding == "bgra")
- {
format = Ogre::PF_B8G8R8A8;
}
- else if (image->encoding == "mono")
+ else if (image->type == sensor_msgs::Image::TYPE_8UC1)
{
format = Ogre::PF_L8;
}
else
{
- ROS_ERROR("Unsupported image encoding [%s]", image->encoding.c_str());
+ ROS_ERROR("Unsupported image encoding [%d]", image->type);
return false;
}
- width_ = image->uint8_data.layout.dim[1].size;
- height_ = image->uint8_data.layout.dim[0].size;
+ width_ = image->cols;
+ height_ = image->rows;
- uint32_t size = image->uint8_data.layout.dim[0].stride;
+ uint32_t size = image->rows * image->stride;
Ogre::DataStreamPtr pixel_stream;
- pixel_stream.bind(new Ogre::MemoryDataStream((void*)(&image->uint8_data.data[0] + image->uint8_data.layout.data_offset), size));
+ pixel_stream.bind(new Ogre::MemoryDataStream((void*)(&image->data[0]), size));
texture_->unload();
texture_->loadRawData(pixel_stream, width_, height_, format);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
