mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +08:00
Merged Audio branch to trunk
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@560 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -0,0 +1,57 @@
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.base.283151101">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.base.283151101" moduleId="org.eclipse.cdt.core.settings" name="Default">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
<configuration artifactName="${ProjName}" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.base.283151101" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
|
||||
<folderInfo id="cdt.managedbuild.toolchain.gnu.base.283151101.248109540" name="/" resourcePath="">
|
||||
<toolChain id="cdt.managedbuild.toolchain.gnu.base.167646636" name="cdt.managedbuild.toolchain.gnu.base" superClass="cdt.managedbuild.toolchain.gnu.base">
|
||||
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.788965009" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
|
||||
<builder arguments="VERBOSE=TRUE" command="make" id="cdt.managedbuild.target.gnu.builder.base.1653582432" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.base"/>
|
||||
<tool id="cdt.managedbuild.tool.gnu.archiver.base.1628870487" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
|
||||
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.813130495" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base">
|
||||
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.408339469" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
|
||||
</tool>
|
||||
<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1588707877" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base">
|
||||
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1741107391" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
|
||||
</tool>
|
||||
<tool id="cdt.managedbuild.tool.gnu.c.linker.base.491007307" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
|
||||
<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.705663340" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base">
|
||||
<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1225880006" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
|
||||
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
|
||||
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
|
||||
</inputType>
|
||||
</tool>
|
||||
<tool id="cdt.managedbuild.tool.gnu.assembler.base.1515840903" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base">
|
||||
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1604501456" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
|
||||
</tool>
|
||||
</toolChain>
|
||||
</folderInfo>
|
||||
</configuration>
|
||||
</storageModule>
|
||||
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
|
||||
</cconfiguration>
|
||||
</storageModule>
|
||||
<storageModule moduleId="scannerConfiguration">
|
||||
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
<project id="rtabmap-image.null.1862955112" name="rtabmap-image"/>
|
||||
</storageModule>
|
||||
<storageModule moduleId="refreshScope" versionNumber="1">
|
||||
<resource resourceType="PROJECT" workspacePath="/rtabmap-image"/>
|
||||
</storageModule>
|
||||
</cproject>
|
||||
@@ -0,0 +1,79 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<projectDescription>
|
||||
<name>rtabmap-image</name>
|
||||
<comment></comment>
|
||||
<projects>
|
||||
</projects>
|
||||
<buildSpec>
|
||||
<buildCommand>
|
||||
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
|
||||
<triggers>clean,full,incremental,</triggers>
|
||||
<arguments>
|
||||
<dictionary>
|
||||
<key>?name?</key>
|
||||
<value></value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.append_environment</key>
|
||||
<value>true</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
|
||||
<value>all</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.buildArguments</key>
|
||||
<value>VERBOSE=TRUE</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.buildCommand</key>
|
||||
<value>make</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
|
||||
<value>clean</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.contents</key>
|
||||
<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
|
||||
<value>false</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
|
||||
<value>true</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.enableFullBuild</key>
|
||||
<value>true</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
|
||||
<value>all</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.stopOnError</key>
|
||||
<value>true</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
|
||||
<value>false</value>
|
||||
</dictionary>
|
||||
</arguments>
|
||||
</buildCommand>
|
||||
<buildCommand>
|
||||
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
|
||||
<triggers>full,incremental,</triggers>
|
||||
<arguments>
|
||||
</arguments>
|
||||
</buildCommand>
|
||||
</buildSpec>
|
||||
<natures>
|
||||
<nature>org.eclipse.cdt.core.cnature</nature>
|
||||
<nature>org.eclipse.cdt.core.ccnature</nature>
|
||||
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
|
||||
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
|
||||
</natures>
|
||||
</projectDescription>
|
||||
@@ -0,0 +1,55 @@
|
||||
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)
|
||||
|
||||
rosbuild_init()
|
||||
|
||||
#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
|
||||
#rosbuild_genmsg()
|
||||
#uncomment if you have defined services
|
||||
rosbuild_gensrv()
|
||||
|
||||
#add dynamic reconfigure api
|
||||
rosbuild_find_ros_package(dynamic_reconfigure)
|
||||
include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
|
||||
gencfg()
|
||||
|
||||
#common commands for building c++ executables and libraries
|
||||
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
|
||||
#target_link_libraries(${PROJECT_NAME} another_library)
|
||||
#rosbuild_add_boost_directories()
|
||||
#rosbuild_link_boost(${PROJECT_NAME} thread)
|
||||
#rosbuild_add_executable(example examples/example.cpp)
|
||||
#target_link_libraries(example ${PROJECT_NAME})
|
||||
|
||||
find_package(OpenCV REQUIRED)
|
||||
rosbuild_add_executable(camera src/CameraNode.cpp)
|
||||
target_link_libraries(camera ${OpenCV_LIBS})
|
||||
|
||||
rosbuild_add_executable(rgb2ind src/RGB2IndexedNode.cpp)
|
||||
target_link_libraries(rgb2ind ${OpenCV_LIBS})
|
||||
|
||||
rosbuild_add_executable(xy2polar src/Cartesian2PolarNode.cpp)
|
||||
target_link_libraries(xy2polar ${OpenCV_LIBS})
|
||||
|
||||
rosbuild_add_executable(motion_filter src/MotionFilterNode.cpp)
|
||||
target_link_libraries(motion_filter ${OpenCV_LIBS})
|
||||
|
||||
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
|
||||
INCLUDE(${QT_USE_FILE})
|
||||
#This will generate moc_* for Qt
|
||||
QT4_WRAP_CPP(moc_srcs src/ImageViewQt.hpp)
|
||||
rosbuild_add_executable(image_view_qt src/ImageViewQtNode.cpp ${moc_srcs})
|
||||
target_link_libraries(image_view_qt ${QT_LIBRARIES} ${OpenCV_LIBS})
|
||||
@@ -0,0 +1 @@
|
||||
include $(shell rospack find mk)/cmake.mk
|
||||
Executable
+14
@@ -0,0 +1,14 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "rtabmap_image"
|
||||
import roslib;roslib.load_manifest(PACKAGE)
|
||||
|
||||
from dynamic_reconfigure.parameter_generator import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("deviceId", int_t, 0, "Camera device ID", 0, 0, 7)
|
||||
gen.add("frameRate", double_t, 0, "Frame rate", 15.0, 0.0, 100.0)
|
||||
gen.add("width", int_t, 0, "Width", 640, 1, 1920)
|
||||
gen.add("height", int_t, 0, "Image height", 480, 1, 1080)
|
||||
|
||||
exit(gen.generate(PACKAGE, "dynamic_camera", "camera"))
|
||||
Executable
+11
@@ -0,0 +1,11 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "rtabmap_image"
|
||||
import roslib;roslib.load_manifest(PACKAGE)
|
||||
|
||||
from dynamic_reconfigure.parameter_generator import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("ratio", double_t, 0, "Motion ratio thresholding", 0.2, 0.0, 1.0)
|
||||
|
||||
exit(gen.generate(PACKAGE, "dynamic_motion_filter", "motionFilter"))
|
||||
Executable
+22
@@ -0,0 +1,22 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "rtabmap_image"
|
||||
import roslib;roslib.load_manifest(PACKAGE)
|
||||
|
||||
from dynamic_reconfigure.parameter_generator import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
size_enum = gen.enum([ gen.const("8", int_t, 0, "Color index table of size 8"),
|
||||
gen.const("16", int_t, 1, "Color index table of size 16"),
|
||||
gen.const("32", int_t, 2, "Color index table of size 32"),
|
||||
gen.const("64", int_t, 3, "Color index table of size 64"),
|
||||
gen.const("128", int_t, 4, "Color index table of size 128"),
|
||||
gen.const("256", int_t, 5, "Color index table of size 256"),
|
||||
gen.const("512", int_t, 6, "Color index table of size 512"),
|
||||
gen.const("1024", int_t, 7, "Color index table of size 1024"),
|
||||
gen.const("65536", int_t, 8, "Color index table of size 65536") ],
|
||||
"An enum to set size")
|
||||
|
||||
gen.add("color_table_size", int_t, 0, "Color index table size", 7, 0, 8, edit_method=size_enum)
|
||||
|
||||
exit(gen.generate(PACKAGE, "dynamic_rgb2ind", "rgb2ind"))
|
||||
Executable
+12
@@ -0,0 +1,12 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE = "rtabmap_image"
|
||||
import roslib;roslib.load_manifest(PACKAGE)
|
||||
|
||||
from dynamic_reconfigure.parameter_generator import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
gen.add("rays", int_t, 0, "Number of polar rays", 128, 1, 1024)
|
||||
gen.add("rings", int_t, 0, "Number of polar rings", 64, 1, 1024)
|
||||
|
||||
exit(gen.generate(PACKAGE, "dynamic_xy2polar", "xy2polar"))
|
||||
@@ -0,0 +1,9 @@
|
||||
<launch>
|
||||
<!-- Nodes -->
|
||||
<node name="camera" pkg="rtabmap" type="camera" output="screen">
|
||||
<param name="device_id" value="0" type="int"/>
|
||||
<param name="image_hz" value="0" type="double"/>
|
||||
<param name="image_width" value="640" type="int"/>
|
||||
<param name="image_height" value="480" type="int"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,31 @@
|
||||
<launch>
|
||||
<!-- Nodes -->
|
||||
<node name="camera" pkg="rtabmap_image" type="camera" output="screen">
|
||||
<remap from="camera/image" to="image"/>
|
||||
<param name="device_id" value="0" type="int"/>
|
||||
<param name="image_hz" value="0" type="double"/>
|
||||
<param name="image_width" value="640" type="int"/>
|
||||
<param name="image_height" value="480" type="int"/>
|
||||
</node>
|
||||
|
||||
<node name="rgb2ind" pkg="rtabmap_image" type="rgb2ind"/>
|
||||
<node name="xy2polar" pkg="rtabmap_image" type="xy2polar"/>
|
||||
<node name="motion_filter" pkg="rtabmap_image" type="motion_filter"/>
|
||||
|
||||
<!-- Create some image_view_qt to see images -->
|
||||
<node name="view_indexed" pkg="rtabmap_image" type="image_view_qt">
|
||||
<remap from="image" to="image_indexed"/>
|
||||
</node>
|
||||
<node name="view_polar" pkg="rtabmap_image" type="image_view_qt">
|
||||
<remap from="image" to="image_polar"/>
|
||||
</node>
|
||||
<node name="view_polar_reconstructed" pkg="rtabmap_image" type="image_view_qt">
|
||||
<remap from="image" to="image_polar_reconstructed"/>
|
||||
</node>
|
||||
<node name="view_motion" pkg="rtabmap_image" type="image_view_qt">
|
||||
<remap from="image" to="image_motion_filtered"/>
|
||||
</node>
|
||||
|
||||
<!-- pop up a dynamic reconfigure -->
|
||||
<node name="dynamic_reconfigure" pkg="dynamic_reconfigure" type="reconfigure_gui"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,14 @@
|
||||
/**
|
||||
\mainpage
|
||||
\htmlinclude manifest.html
|
||||
|
||||
\b rtabmap_image
|
||||
|
||||
<!--
|
||||
Provide an overview of your package.
|
||||
-->
|
||||
|
||||
-->
|
||||
|
||||
|
||||
*/
|
||||
@@ -0,0 +1,22 @@
|
||||
<package>
|
||||
<description brief="rtabmap_image">
|
||||
|
||||
rtabmap_image
|
||||
|
||||
</description>
|
||||
<author>Mathieu Labbé</author>
|
||||
<license>BSD</license>
|
||||
<review status="unreviewed" notes=""/>
|
||||
<url>http://ros.org/wiki/rtabmap_image</url>
|
||||
<depend package="roscpp"/>
|
||||
<depend package="rospy"/>
|
||||
<depend package="std_msgs"/>
|
||||
<depend package="rtabmap_lib"/>
|
||||
<depend package="image_transport"/>
|
||||
<depend package="cv_bridge"/>
|
||||
<depend package="sensor_msgs"/>
|
||||
<depend package="dynamic_reconfigure"/>
|
||||
|
||||
</package>
|
||||
|
||||
|
||||
@@ -0,0 +1,203 @@
|
||||
/*
|
||||
* CameraNode.cpp
|
||||
*
|
||||
* Created on: 1 févr. 2010
|
||||
* Author: labm2414
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <std_srvs/Empty.h>
|
||||
|
||||
#include <rtabmap/core/Camera.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
|
||||
#include <utilite/ULogger.h>
|
||||
#include <utilite/UEventsHandler.h>
|
||||
#include <utilite/UEventsManager.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rtabmap_image/cameraConfig.h>
|
||||
|
||||
// See the launch file to change the camera values
|
||||
#define DEFAULT_DEVICE_ID 0
|
||||
#define DEFAULT_IMG_RATE 10.0 //Hz
|
||||
#define DEFAULT_IMG_WIDTH 640
|
||||
#define DEFAULT_IMG_HEIGHT 480
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
class Camera;
|
||||
class CamPostTreatment;
|
||||
}
|
||||
|
||||
class CameraVideoWrapper : public UEventsHandler
|
||||
{
|
||||
public:
|
||||
// Usb device like a Webcam
|
||||
CameraVideoWrapper(int usbDevice = 0,
|
||||
float imageRate = 0,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0)
|
||||
{
|
||||
ros::NodeHandle nh("~");
|
||||
image_transport::ImageTransport it(nh);
|
||||
rosPublisher_ = it.advertise("image", 1);
|
||||
startSrv_ = nh.advertiseService("start", &CameraVideoWrapper::startSrv, this);
|
||||
stopSrv_ = nh.advertiseService("stop", &CameraVideoWrapper::stopSrv, this);
|
||||
UEventsManager::addHandler(this);
|
||||
|
||||
camera_ = new rtabmap::CameraVideo(usbDevice, imageRate, false, imageWidth, imageHeight);
|
||||
}
|
||||
|
||||
virtual ~CameraVideoWrapper()
|
||||
{
|
||||
if(camera_)
|
||||
{
|
||||
camera_->join(true);
|
||||
delete camera_;
|
||||
}
|
||||
}
|
||||
|
||||
bool init()
|
||||
{
|
||||
if(camera_)
|
||||
{
|
||||
return camera_->init();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void start()
|
||||
{
|
||||
if(camera_)
|
||||
{
|
||||
return camera_->start();
|
||||
}
|
||||
}
|
||||
|
||||
void updateParameters();
|
||||
|
||||
bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("Camera started...");
|
||||
if(camera_)
|
||||
{
|
||||
camera_->start();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("Camera stopped...");
|
||||
if(camera_)
|
||||
{
|
||||
camera_->kill();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void setParameters(int deviceId, double frameRate, int width, int height)
|
||||
{
|
||||
if(camera_)
|
||||
{
|
||||
if(deviceId!=camera_->getUsbDevice() || width!=(int)camera_->getImageWidth() || height!=(int)camera_->getImageHeight())
|
||||
{
|
||||
//restart required
|
||||
camera_->join(true);
|
||||
delete camera_;
|
||||
camera_ = new rtabmap::CameraVideo(deviceId, frameRate, false, width, height);
|
||||
init();
|
||||
start();
|
||||
}
|
||||
else
|
||||
{
|
||||
camera_->setImageRate(frameRate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void handleEvent(UEvent * anEvent)
|
||||
{
|
||||
if(anEvent->getClassName().compare("CameraEvent") == 0)
|
||||
{
|
||||
rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)anEvent;
|
||||
const cv::Mat & image = e->image();
|
||||
if(!image.empty())
|
||||
{
|
||||
cv_bridge::CvImage img;
|
||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||
img.image = image;
|
||||
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
|
||||
rosMsg->header.frame_id = "camera";
|
||||
rosMsg->header.stamp = ros::Time::now();
|
||||
rosPublisher_.publish(rosMsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
image_transport::Publisher rosPublisher_;
|
||||
rtabmap::CameraVideo * camera_;
|
||||
ros::ServiceServer startSrv_;
|
||||
ros::ServiceServer stopSrv_;
|
||||
};
|
||||
|
||||
CameraVideoWrapper * camera = 0;
|
||||
void callback(rtabmap_image::cameraConfig &config, uint32_t level)
|
||||
{
|
||||
if(camera)
|
||||
{
|
||||
camera->setParameters(config.deviceId, config.frameRate, config.width, config.height);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ULogger::setType(ULogger::kTypeConsole);
|
||||
//ULogger::setLevel(ULogger::kDebug);
|
||||
|
||||
ros::init(argc, argv, "camera");
|
||||
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
int deviceId = DEFAULT_DEVICE_ID;
|
||||
double imgRate = DEFAULT_IMG_RATE;
|
||||
int imgWidth = DEFAULT_IMG_WIDTH;
|
||||
int imgHeight = DEFAULT_IMG_HEIGHT;
|
||||
|
||||
camera = new CameraVideoWrapper(deviceId, float(imgRate), imgWidth, imgHeight); // webcam device 0
|
||||
|
||||
if(!camera || (camera && !camera->init()))
|
||||
{
|
||||
ROS_ERROR("Cannot initiate the camera. Verify if OpenCV is built with ffmpeg support.");
|
||||
}
|
||||
else
|
||||
{
|
||||
// Start the camera
|
||||
camera->start();
|
||||
ROS_INFO("Camera started...");
|
||||
|
||||
|
||||
dynamic_reconfigure::Server<rtabmap_image::cameraConfig> server;
|
||||
dynamic_reconfigure::Server<rtabmap_image::cameraConfig>::CallbackType f;
|
||||
f = boost::bind(&callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
|
||||
//cleanup
|
||||
if(camera)
|
||||
{
|
||||
delete camera;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
/*
|
||||
* RGB2IndexedNode.cpp
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <opencv2/imgproc/imgproc_c.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rtabmap_image/xy2polarConfig.h>
|
||||
|
||||
int dp_rays = 128;
|
||||
int dp_rings = 64;
|
||||
|
||||
image_transport::Publisher rosPublisherPolar;
|
||||
image_transport::Publisher rosPublisherPolarReconstructed;
|
||||
|
||||
void callback(rtabmap_image::xy2polarConfig &config, uint32_t level)
|
||||
{
|
||||
dp_rays = config.rays;
|
||||
dp_rings = config.rings;
|
||||
}
|
||||
|
||||
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||
{
|
||||
if(!rosPublisherPolar.getNumSubscribers() && !rosPublisherPolarReconstructed.getNumSubscribers())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if(msg->data.size())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
||||
|
||||
if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3)
|
||||
{
|
||||
int radiusX = ptr->image.cols/2;
|
||||
int radiusY = ptr->image.rows/2;
|
||||
int radius = radiusX<radiusY?radiusX:radiusX;
|
||||
float M = dp_rings/std::log(radius);
|
||||
cv::Mat polar(dp_rays, dp_rings, CV_8UC3);
|
||||
IplImage iplPolar = polar;
|
||||
IplImage iplImage = ptr->image;
|
||||
cvLogPolar( &iplImage, &iplPolar, cvPoint2D32f(radiusX, radiusY), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
|
||||
|
||||
if(rosPublisherPolar.getNumSubscribers())
|
||||
{
|
||||
cv_bridge::CvImage img;
|
||||
img.header.stamp = ros::Time::now();
|
||||
img.header.frame_id = msg->header.frame_id;
|
||||
img.encoding = ptr->encoding;
|
||||
img.image = polar;
|
||||
rosPublisherPolar.publish(img.toImageMsg());
|
||||
}
|
||||
|
||||
if(rosPublisherPolarReconstructed.getNumSubscribers())
|
||||
{
|
||||
cv::Mat reconstructed(ptr->image.rows, ptr->image.cols, ptr->image.type());
|
||||
IplImage iplReconstructed = reconstructed;
|
||||
cvLogPolar( &iplPolar, &iplReconstructed, cvPoint2D32f(radiusX, radiusY), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
|
||||
|
||||
cv_bridge::CvImage img;
|
||||
img.header.stamp = ros::Time::now();
|
||||
img.header.frame_id = msg->header.frame_id;
|
||||
img.encoding = ptr->encoding;
|
||||
img.image = reconstructed;
|
||||
rosPublisherPolarReconstructed.publish(img.toImageMsg());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
ros::init(argc, argv, "xy2polar");
|
||||
|
||||
ros::NodeHandle n;
|
||||
image_transport::ImageTransport it(n);
|
||||
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
||||
rosPublisherPolar = it.advertise("image_polar", 1);
|
||||
rosPublisherPolarReconstructed = it.advertise("image_polar_reconstructed", 1);
|
||||
|
||||
dynamic_reconfigure::Server<rtabmap_image::xy2polarConfig> server;
|
||||
dynamic_reconfigure::Server<rtabmap_image::xy2polarConfig>::CallbackType f;
|
||||
f = boost::bind(&callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,212 @@
|
||||
/*
|
||||
* ImageViewQt.hpp
|
||||
*
|
||||
* Created on: 2012-06-20
|
||||
* Author: mathieu
|
||||
*/
|
||||
|
||||
#ifndef IMAGEVIEWQT_HPP_
|
||||
#define IMAGEVIEWQT_HPP_
|
||||
|
||||
#include <QtCore/QTimer>
|
||||
#include <QtGui/QMouseEvent>
|
||||
#include <QtGui/QApplication>
|
||||
#include <QtGui/QWidget>
|
||||
#include <QtGui/QPainter>
|
||||
#include <QtGui/QToolTip>
|
||||
#include <QtGui/QMenu>
|
||||
|
||||
#include <utilite/UPlot.h>
|
||||
|
||||
class RGBPlot: public UPlot
|
||||
{
|
||||
public:
|
||||
RGBPlot(int x, int y, QWidget * parent = 0) :
|
||||
x_(x),
|
||||
y_(y)
|
||||
{
|
||||
r_ = this->addCurve("R", Qt::red);
|
||||
g_ = this->addCurve("G", Qt::green);
|
||||
b_ = this->addCurve("B", Qt::blue);
|
||||
}
|
||||
~RGBPlot() {}
|
||||
void setPixel(int r, int g, int b)
|
||||
{
|
||||
r_->addValue(r);
|
||||
g_->addValue(g);
|
||||
b_->addValue(b);
|
||||
}
|
||||
int x() const {return x_;}
|
||||
int y() const {return y_;}
|
||||
private:
|
||||
int x_;
|
||||
int y_;
|
||||
UPlotCurve * r_;
|
||||
UPlotCurve * g_;
|
||||
UPlotCurve * b_;
|
||||
};
|
||||
|
||||
class ImageViewQt : public QWidget
|
||||
{
|
||||
Q_OBJECT;
|
||||
public:
|
||||
ImageViewQt(QWidget * parent = 0) : QWidget(parent)
|
||||
{
|
||||
this->setMouseTracking(true);
|
||||
}
|
||||
~ImageViewQt() {}
|
||||
|
||||
public slots:
|
||||
void setImage(const QImage & image)
|
||||
{
|
||||
if(pixmap_.width() != image.width() || pixmap_.height() != image.height())
|
||||
{
|
||||
for(QMap<QPair<int,int>, RGBPlot*>::iterator iter = pixelMap_.begin(); iter!=pixelMap_.end();)
|
||||
{
|
||||
RGBPlot * plot = *iter;
|
||||
iter = pixelMap_.erase(iter);
|
||||
delete plot;
|
||||
}
|
||||
this->setMinimumSize(image.width(), image.height());
|
||||
this->setGeometry(this->geometry().x(), this->geometry().y(), image.width(), image.height());
|
||||
}
|
||||
else
|
||||
{
|
||||
for(QMap<QPair<int,int>, RGBPlot*>::iterator iter = pixelMap_.begin(); iter!=pixelMap_.end();++iter)
|
||||
{
|
||||
QRgb rgb = image.pixel((*iter)->x(), (*iter)->y());
|
||||
(*iter)->setPixel(qRed(rgb), qGreen(rgb), qBlue(rgb));
|
||||
}
|
||||
}
|
||||
pixmap_ = QPixmap::fromImage(image);
|
||||
this->update();
|
||||
}
|
||||
|
||||
private:
|
||||
void computeScaleOffsets(float & scale, float & offsetX, float & offsetY)
|
||||
{
|
||||
scale = 1.0f;
|
||||
offsetX = 0.0f;
|
||||
offsetY = 0.0f;
|
||||
|
||||
if(!pixmap_.isNull())
|
||||
{
|
||||
float w = pixmap_.width();
|
||||
float h = pixmap_.height();
|
||||
float widthRatio = float(this->rect().width()) / w;
|
||||
float heightRatio = float(this->rect().height()) / h;
|
||||
|
||||
if(widthRatio < heightRatio)
|
||||
{
|
||||
scale = widthRatio;
|
||||
}
|
||||
else
|
||||
{
|
||||
scale = heightRatio;
|
||||
}
|
||||
|
||||
w *= scale;
|
||||
h *= scale;
|
||||
|
||||
if(w < this->rect().width())
|
||||
{
|
||||
offsetX = (this->rect().width() - w)/2.0f;
|
||||
}
|
||||
if(h < this->rect().height())
|
||||
{
|
||||
offsetY = (this->rect().height() - h)/2.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
private slots:
|
||||
void removePlot(QObject * obj)
|
||||
{
|
||||
if(obj)
|
||||
{
|
||||
RGBPlot * plot = (RGBPlot*)obj;
|
||||
pixelMap_.remove(QPair<int,int>(plot->x(), plot->y()));
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void paintEvent(QPaintEvent *event)
|
||||
{
|
||||
if(!pixmap_.isNull())
|
||||
{
|
||||
//Scale
|
||||
float ratio, offsetX, offsetY;
|
||||
this->computeScaleOffsets(ratio, offsetX, offsetY);
|
||||
QPainter painter(this);
|
||||
painter.translate(offsetX, offsetY);
|
||||
painter.scale(ratio, ratio);
|
||||
painter.drawPixmap(QPoint(0,0), pixmap_);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void mouseMoveEvent(QMouseEvent * event)
|
||||
{
|
||||
if(!pixmap_.isNull())
|
||||
{
|
||||
QPoint pos = this->mapFromGlobal(event->globalPos());
|
||||
|
||||
float ratio, offsetX, offsetY;
|
||||
computeScaleOffsets(ratio, offsetX, offsetY);
|
||||
pos.rx()-=offsetX;
|
||||
pos.ry()-=offsetY;
|
||||
pos.rx()/=ratio;
|
||||
pos.ry()/=ratio;
|
||||
|
||||
if(pos.x()>=0 && pos.x()<pixmap_.width() &&
|
||||
pos.y()>=0 && pos.y()<pixmap_.height())
|
||||
{
|
||||
QToolTip::showText(event->globalPos(),
|
||||
QString("[%1,%2]")
|
||||
.arg(pos.x())
|
||||
.arg(pos.y()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
virtual void contextMenuEvent(QContextMenuEvent * event)
|
||||
{
|
||||
if(!pixmap_.isNull())
|
||||
{
|
||||
QPoint pos = this->mapFromGlobal(event->globalPos());
|
||||
|
||||
float ratio, offsetX, offsetY;
|
||||
computeScaleOffsets(ratio, offsetX, offsetY);
|
||||
pos.rx()-=offsetX;
|
||||
pos.ry()-=offsetY;
|
||||
pos.rx()/=ratio;
|
||||
pos.ry()/=ratio;
|
||||
|
||||
if(pos.x()>=0 && pos.x()<pixmap_.width() &&
|
||||
pos.y()>=0 && pos.y()<pixmap_.height())
|
||||
{
|
||||
QMenu menu;
|
||||
QAction * a = menu.addAction(tr("Plot pixel (%1,%2) variation").arg(pos.x()).arg(pos.y()));
|
||||
QAction * b = menu.exec(event->globalPos());
|
||||
if(b == a)
|
||||
{
|
||||
if(!pixelMap_.contains(QPair<int,int>(pos.x(), pos.y())))
|
||||
{
|
||||
RGBPlot * plot = new RGBPlot(pos.x(), pos.y(), this);
|
||||
plot->setWindowTitle(tr("Pixel (%1,%2)").arg(pos.x()).arg(pos.y()));
|
||||
connect(plot, SIGNAL(destroyed(QObject *)), this, SLOT(removePlot(QObject *)));
|
||||
plot->setMaxVisibleItems(100);
|
||||
plot->show();
|
||||
pixelMap_.insert(QPair<int,int>(pos.x(), pos.y()), plot);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
QPixmap pixmap_;
|
||||
QMap<QPair<int,int>, RGBPlot*> pixelMap_;
|
||||
};
|
||||
|
||||
|
||||
#endif /* IMAGEVIEWQT_HPP_ */
|
||||
@@ -0,0 +1,153 @@
|
||||
/*
|
||||
* CameraNodeReceiver.cpp
|
||||
*
|
||||
* Created on: 2 févr. 2010
|
||||
* Author: labm2414
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <utilite/UDirectory.h>
|
||||
#include <utilite/UConversion.h>
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
#include "ImageViewQt.hpp"
|
||||
|
||||
ImageViewQt * view = 0;
|
||||
bool imagesSaved = false;
|
||||
int i = 0;
|
||||
|
||||
// assume bgr
|
||||
QImage cvtCvMat2QImage(const cv::Mat & image, bool isBgr = true)
|
||||
{
|
||||
QImage qtemp;
|
||||
if(!image.empty() && image.depth() == CV_8U)
|
||||
{
|
||||
const unsigned char * data = image.data;
|
||||
qtemp = QImage(image.cols, image.rows, QImage::Format_RGB32);
|
||||
for(int y = 0; y < image.rows; ++y, data += image.cols*image.elemSize())
|
||||
{
|
||||
for(int x = 0; x < image.cols; ++x)
|
||||
{
|
||||
QRgb * p = ((QRgb*)qtemp.scanLine (y)) + x;
|
||||
if(isBgr)
|
||||
{
|
||||
*p = qRgb(data[x * image.channels()+2], data[x * image.channels()+1], data[x * image.channels()]);
|
||||
}
|
||||
else
|
||||
{
|
||||
*p = qRgb(data[x * image.channels()], data[x * image.channels()+1], data[x * image.channels()+2]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(!image.empty() && image.depth() != CV_8U)
|
||||
{
|
||||
printf("Wrong image format, must be 8_bits\n");
|
||||
}
|
||||
return qtemp;
|
||||
}
|
||||
|
||||
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||
{
|
||||
if(msg->data.size())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
||||
|
||||
//ROS_INFO("Received an image size=(%d,%d)", ptr->image.cols, ptr->image.rows);
|
||||
|
||||
if(imagesSaved)
|
||||
{
|
||||
std::string path = "./imagesSaved";
|
||||
if(!UDirectory::exists(path))
|
||||
{
|
||||
if(!UDirectory::makeDir(path))
|
||||
{
|
||||
ROS_ERROR("Cannot make dir %s", path.c_str());
|
||||
}
|
||||
}
|
||||
path.append("/");
|
||||
path.append(uNumber2Str(i++));
|
||||
path.append(".bmp");
|
||||
if(!cv::imwrite(path.c_str(), ptr->image))
|
||||
{
|
||||
ROS_ERROR("Cannot save image to %s", path.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_INFO("Saved image %s", path.c_str());
|
||||
}
|
||||
}
|
||||
else if(view && view->isVisible())
|
||||
{
|
||||
if(ptr->encoding.compare("bgr8") == 0)
|
||||
{
|
||||
// Process image in Qt thread...
|
||||
QMetaObject::invokeMethod(view, "setImage", Q_ARG(const QImage &, cvtCvMat2QImage(ptr->image)));
|
||||
}
|
||||
else if(ptr->encoding.compare("rgb8") == 0)
|
||||
{
|
||||
// Process image in Qt thread...
|
||||
QMetaObject::invokeMethod(view, "setImage", Q_ARG(const QImage &, cvtCvMat2QImage(ptr->image, false)));
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Encoding \"%s\" is not supported yet (try \"bgr8\" or \"rgb8\")", ptr->encoding.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void my_handler(int s){
|
||||
QApplication::closeAllWindows();
|
||||
QApplication::exit();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "image_view_qt");
|
||||
ros::NodeHandle pn("~");
|
||||
|
||||
pn.param("images_saved", imagesSaved, imagesSaved);
|
||||
ROS_INFO("images_saved=%d", imagesSaved?1:0);
|
||||
|
||||
ros::NodeHandle n;
|
||||
image_transport::ImageTransport it(n);
|
||||
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
||||
|
||||
if(!imagesSaved)
|
||||
{
|
||||
QApplication app(argc, argv);
|
||||
view = new ImageViewQt();
|
||||
view->setWindowTitle(ros::this_node::getName().c_str());
|
||||
view->show();
|
||||
|
||||
// Catch ctrl-c to close the gui
|
||||
// (Place this after QApplication's constructor)
|
||||
struct sigaction sigIntHandler;
|
||||
sigIntHandler.sa_handler = my_handler;
|
||||
sigemptyset(&sigIntHandler.sa_mask);
|
||||
sigIntHandler.sa_flags = 0;
|
||||
sigaction(SIGINT, &sigIntHandler, NULL);
|
||||
|
||||
ROS_INFO("Waiting for images...");
|
||||
ros::AsyncSpinner spinner(1); // Use 1 thread
|
||||
spinner.start();
|
||||
app.exec();
|
||||
spinner.stop();
|
||||
|
||||
delete view;
|
||||
}
|
||||
else
|
||||
{
|
||||
ros::spin();
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* RGB2IndexedNode.cpp
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <signal.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rtabmap_image/motionFilterConfig.h>
|
||||
|
||||
image_transport::Publisher rosPublisher;
|
||||
cv::Mat previousImage;
|
||||
double ratio = 0.2;
|
||||
|
||||
void callback(rtabmap_image::motionFilterConfig &config, uint32_t level)
|
||||
{
|
||||
ratio = config.ratio;
|
||||
}
|
||||
|
||||
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||
{
|
||||
if(rosPublisher.getNumSubscribers() && msg->data.size())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
||||
if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3)
|
||||
{
|
||||
cv::Mat motion = ptr->image.clone();
|
||||
if(previousImage.cols == motion.cols && previousImage.rows == motion.rows)
|
||||
{
|
||||
unsigned char * imageData = (unsigned char *)motion.data;
|
||||
unsigned char * previous_imageData = (unsigned char *)previousImage.data;
|
||||
int widthStep = motion.cols * motion.elemSize();
|
||||
for(int j=0; j<motion.rows; ++j)
|
||||
{
|
||||
for(int i=0; i<motion.cols; ++i)
|
||||
{
|
||||
float b = (float)imageData[j*widthStep+i*3+0];
|
||||
float g = (float)imageData[j*widthStep+i*3+1];
|
||||
float r = (float)imageData[j*widthStep+i*3+2];
|
||||
float previous_b = (float)previous_imageData[j*widthStep+i*3+0];
|
||||
float previous_g = (float)previous_imageData[j*widthStep+i*3+1];
|
||||
float previous_r = (float)previous_imageData[j*widthStep+i*3+2];
|
||||
|
||||
if(!(fabs(b-previous_b)/256.0f>=ratio || fabs(g-previous_g)/256.0f >= ratio || fabs(r-previous_r)/256.0f >= ratio))
|
||||
{
|
||||
imageData[j*widthStep+i*3+0] = 0;
|
||||
imageData[j*widthStep+i*3+1] = 0;
|
||||
imageData[j*widthStep+i*3+2] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
previousImage = ptr->image.clone();
|
||||
|
||||
cv_bridge::CvImage img;
|
||||
img.header.stamp = ros::Time::now();
|
||||
img.header.frame_id = ptr->header.frame_id;
|
||||
img.encoding = ptr->encoding;
|
||||
img.image = motion;
|
||||
rosPublisher.publish(img.toImageMsg());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
ros::init(argc, argv, "motion_filter");
|
||||
|
||||
ros::NodeHandle n;
|
||||
image_transport::ImageTransport it(n);
|
||||
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
||||
rosPublisher = it.advertise("image_motion_filtered", 1);
|
||||
|
||||
dynamic_reconfigure::Server<rtabmap_image::motionFilterConfig> server;
|
||||
dynamic_reconfigure::Server<rtabmap_image::motionFilterConfig>::CallbackType f;
|
||||
f = boost::bind(&callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* RGB2IndexedNode.cpp
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <signal.h>
|
||||
#include <rtabmap/core/ColorTable.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rtabmap_image/rgb2indConfig.h>
|
||||
|
||||
image_transport::Publisher rosPublisher;
|
||||
rtabmap::ColorTable colorTable(rtabmap::ColorTable::kSize1024);
|
||||
|
||||
void callback(rtabmap_image::rgb2indConfig &config, uint32_t level)
|
||||
{
|
||||
if(config.color_table_size == 8)
|
||||
{
|
||||
colorTable = rtabmap::ColorTable(rtabmap::ColorTable::kSize65536);
|
||||
}
|
||||
else
|
||||
{
|
||||
colorTable = rtabmap::ColorTable(1<<(config.color_table_size+3));
|
||||
}
|
||||
}
|
||||
|
||||
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||
{
|
||||
if(rosPublisher.getNumSubscribers() && msg->data.size())
|
||||
{
|
||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
||||
|
||||
if(ptr->image.depth() == CV_8U && ptr->image.channels() == 3)
|
||||
{
|
||||
cv::Mat ind = ptr->image.clone();
|
||||
unsigned char * imageData = (unsigned char *)ind.data;
|
||||
int widthStep = ind.cols * ind.elemSize();
|
||||
for(int i=0; i<ind.rows; ++i)
|
||||
{
|
||||
for(int j=0; j<ind.cols; ++j)
|
||||
{
|
||||
unsigned char & b = imageData[i*widthStep+j*3+0];
|
||||
unsigned char & g = imageData[i*widthStep+j*3+1];
|
||||
unsigned char & r = imageData[i*widthStep+j*3+2];
|
||||
int index = (int)colorTable.getIndex(r, g, b);
|
||||
colorTable.getRgb(index, r, g , b);
|
||||
}
|
||||
}
|
||||
cv_bridge::CvImage img;
|
||||
img.header.stamp = ros::Time::now();
|
||||
img.header.frame_id = ptr->header.frame_id;
|
||||
img.encoding = ptr->encoding;
|
||||
img.image = ind;
|
||||
rosPublisher.publish(img.toImageMsg());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
ros::init(argc, argv, "rgb2ind");
|
||||
|
||||
ros::NodeHandle n;
|
||||
image_transport::ImageTransport it(n);
|
||||
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
||||
rosPublisher = it.advertise("image_indexed", 1);
|
||||
|
||||
dynamic_reconfigure::Server<rtabmap_image::rgb2indConfig> server;
|
||||
dynamic_reconfigure::Server<rtabmap_image::rgb2indConfig>::CallbackType f;
|
||||
f = boost::bind(&callback, _1, _2);
|
||||
server.setCallback(f);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user