mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +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
@@ -1,6 +0,0 @@
|
|||||||
sqlite3:
|
|
||||||
ubuntu: libsqlite3-dev sqlite3
|
|
||||||
debian: libsqlite3-dev sqlite3
|
|
||||||
fftw3:
|
|
||||||
ubuntu: libfftw3-dev
|
|
||||||
debian: libfftw3-dev
|
|
||||||
+214
-207
@@ -2,211 +2,218 @@
|
|||||||
<?fileVersion 4.0.0?>
|
<?fileVersion 4.0.0?>
|
||||||
|
|
||||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||||
<cconfiguration id="0.250647335">
|
<cconfiguration id="0.250647335">
|
||||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.250647335" moduleId="org.eclipse.cdt.core.settings" name="Default">
|
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.250647335" moduleId="org.eclipse.cdt.core.settings" name="Default">
|
||||||
<externalSettings/>
|
<externalSettings/>
|
||||||
<extensions>
|
<extensions>
|
||||||
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||||
<extension id="org.eclipse.cdt.core.MakeErrorParser" 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.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.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||||
</extensions>
|
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||||
</storageModule>
|
</extensions>
|
||||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
</storageModule>
|
||||||
<configuration artifactName="rtabmap-pkg" buildProperties="" description="" id="0.250647335" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
|
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||||
<folderInfo id="0.250647335." name="/" resourcePath="">
|
<configuration artifactName="rtabmap-pkg" buildProperties="" description="" id="0.250647335" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
|
||||||
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
|
<folderInfo id="0.250647335." name="/" resourcePath="">
|
||||||
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931.1819283928" name=""/>
|
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
|
||||||
<builder arguments="-C ${ProjDirPath}/build VERBOSE=true" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.425794023" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
|
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931.1819283928" name=""/>
|
||||||
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1445630059" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
|
<builder arguments="VERBOSE=true" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.425794023" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
|
||||||
<tool id="org.eclipse.cdt.build.core.settings.holder.1764610277" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
|
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1445630059" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
|
||||||
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1671812416" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
|
<tool id="org.eclipse.cdt.build.core.settings.holder.1764610277" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
|
||||||
</tool>
|
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1671812416" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
|
||||||
<tool id="org.eclipse.cdt.build.core.settings.holder.1195149217" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
|
</tool>
|
||||||
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.747334269" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
|
<tool id="org.eclipse.cdt.build.core.settings.holder.1195149217" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
|
||||||
</tool>
|
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.747334269" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
|
||||||
<tool id="org.eclipse.cdt.build.core.settings.holder.1906811302" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
|
</tool>
|
||||||
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.537988677" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
|
<tool id="org.eclipse.cdt.build.core.settings.holder.1906811302" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
|
||||||
</tool>
|
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.537988677" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
|
||||||
</toolChain>
|
</tool>
|
||||||
</folderInfo>
|
</toolChain>
|
||||||
</configuration>
|
</folderInfo>
|
||||||
</storageModule>
|
<sourceEntries>
|
||||||
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
|
<entry excluding="build" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
|
||||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
|
</sourceEntries>
|
||||||
<storageModule moduleId="scannerConfiguration">
|
</configuration>
|
||||||
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
|
</storageModule>
|
||||||
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
|
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
|
||||||
<buildOutputProvider>
|
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
|
||||||
<openAction enabled="true" filePath=""/>
|
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
|
||||||
<parser enabled="true"/>
|
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
|
||||||
</buildOutputProvider>
|
</cconfiguration>
|
||||||
<scannerInfoProvider id="specsFile">
|
</storageModule>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||||
<parser enabled="true"/>
|
<project id="rtabmap-pkg.null.595292537" name="rtabmap-pkg"/>
|
||||||
</scannerInfoProvider>
|
</storageModule>
|
||||||
</profile>
|
<storageModule moduleId="scannerConfiguration">
|
||||||
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
|
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="makefileGenerator">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
<scannerInfoProvider id="makefileGenerator">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<scannerConfigBuildInfo instanceId="0.250647335">
|
</profile>
|
||||||
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
|
||||||
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
|
<buildOutputProvider>
|
||||||
<buildOutputProvider>
|
<openAction enabled="true" filePath=""/>
|
||||||
<openAction enabled="true" filePath=""/>
|
<parser enabled="true"/>
|
||||||
<parser enabled="true"/>
|
</buildOutputProvider>
|
||||||
</buildOutputProvider>
|
<scannerInfoProvider id="specsFile">
|
||||||
<scannerInfoProvider id="specsFile">
|
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
<parser enabled="true"/>
|
||||||
<parser enabled="true"/>
|
</scannerInfoProvider>
|
||||||
</scannerInfoProvider>
|
</profile>
|
||||||
</profile>
|
<scannerConfigBuildInfo instanceId="0.250647335">
|
||||||
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
|
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="makefileGenerator">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
<scannerInfoProvider id="makefileGenerator">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-f ${project_name}_scd.mk" command="make" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
|
</profile>
|
||||||
<buildOutputProvider>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
|
||||||
<openAction enabled="true" filePath=""/>
|
<buildOutputProvider>
|
||||||
<parser enabled="true"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</buildOutputProvider>
|
<parser enabled="true"/>
|
||||||
<scannerInfoProvider id="specsFile">
|
</buildOutputProvider>
|
||||||
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/>
|
<scannerInfoProvider id="specsFile">
|
||||||
<parser enabled="true"/>
|
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/>
|
||||||
</scannerInfoProvider>
|
<parser enabled="true"/>
|
||||||
</profile>
|
</scannerInfoProvider>
|
||||||
</scannerConfigBuildInfo>
|
</profile>
|
||||||
</storageModule>
|
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
|
||||||
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
|
<buildOutputProvider>
|
||||||
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
|
<openAction enabled="true" filePath=""/>
|
||||||
</cconfiguration>
|
<parser enabled="true"/>
|
||||||
</storageModule>
|
</buildOutputProvider>
|
||||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
<scannerInfoProvider id="specsFile">
|
||||||
<project id="rtabmap-pkg.null.595292537" name="rtabmap-pkg"/>
|
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/>
|
||||||
</storageModule>
|
<parser enabled="true"/>
|
||||||
|
</scannerInfoProvider>
|
||||||
|
</profile>
|
||||||
|
</scannerConfigBuildInfo>
|
||||||
|
</storageModule>
|
||||||
|
<storageModule moduleId="refreshScope" versionNumber="1">
|
||||||
|
<resource resourceType="PROJECT" workspacePath="/rtabmap-pkg"/>
|
||||||
|
</storageModule>
|
||||||
</cproject>
|
</cproject>
|
||||||
|
|||||||
+1
-1
@@ -23,7 +23,7 @@
|
|||||||
</dictionary>
|
</dictionary>
|
||||||
<dictionary>
|
<dictionary>
|
||||||
<key>org.eclipse.cdt.make.core.buildArguments</key>
|
<key>org.eclipse.cdt.make.core.buildArguments</key>
|
||||||
<value>-C ${ProjDirPath}/build VERBOSE=true</value>
|
<value>VERBOSE=true</value>
|
||||||
</dictionary>
|
</dictionary>
|
||||||
<dictionary>
|
<dictionary>
|
||||||
<key>org.eclipse.cdt.make.core.buildCommand</key>
|
<key>org.eclipse.cdt.make.core.buildCommand</key>
|
||||||
|
|||||||
+21
-12
@@ -37,29 +37,38 @@ rosbuild_gensrv()
|
|||||||
#target_link_libraries(example ${PROJECT_NAME})
|
#target_link_libraries(example ${PROJECT_NAME})
|
||||||
|
|
||||||
find_package(OpenCV REQUIRED)
|
find_package(OpenCV REQUIRED)
|
||||||
rosbuild_add_executable(camera src/CameraNode.cpp src/CameraWrapper.cpp)
|
|
||||||
target_link_libraries(camera ${OpenCV_LIBS})
|
|
||||||
rosbuild_add_executable(camera_receiver src/CameraNodeReceiver.cpp)
|
|
||||||
target_link_libraries(camera_receiver ${OpenCV_LIBS})
|
|
||||||
rosbuild_add_executable(camera_receiver_sms src/CameraNodeReceiverSM.cpp)
|
|
||||||
target_link_libraries(camera_receiver_sms ${OpenCV_LIBS})
|
|
||||||
rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
|
rosbuild_add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
|
||||||
target_link_libraries(rtabmap ${OpenCV_LIBS})
|
target_link_libraries(rtabmap ${OpenCV_LIBS})
|
||||||
rosbuild_add_executable(rtabmap_in src/InputNode.cpp)
|
|
||||||
target_link_libraries(rtabmap_in ${OpenCV_LIBS})
|
|
||||||
rosbuild_add_executable(rtabmap_out src/OutputNode.cpp)
|
rosbuild_add_executable(rtabmap_out src/OutputNode.cpp)
|
||||||
rosbuild_add_executable(abtr_velocity src/AbtrVelocityNode.cpp)
|
rosbuild_add_executable(abtr_velocity src/AbtrVelocityNode.cpp)
|
||||||
rosbuild_add_executable(image_to_sms src/ImageToSensorimotorStateNode.cpp)
|
|
||||||
target_link_libraries(image_to_sms ${OpenCV_LIBS})
|
|
||||||
rosbuild_add_executable(twist_to_sms src/TwistToSensorimotorStateNode.cpp)
|
|
||||||
rosbuild_add_executable(twist_to_turtle_vel src/CmdVelToTurtleVelNode.cpp)
|
rosbuild_add_executable(twist_to_turtle_vel src/CmdVelToTurtleVelNode.cpp)
|
||||||
rosbuild_add_executable(twist_to_poses src/TwistToPoses.cpp)
|
rosbuild_add_executable(twist_to_pose src/Twist2Pose.cpp)
|
||||||
|
rosbuild_add_executable(twist_to_twist_stamped src/Twist2TwistStamped.cpp)
|
||||||
|
|
||||||
|
# Input nodes
|
||||||
|
rosbuild_add_boost_directories()
|
||||||
|
rosbuild_add_executable(input_image_audio_node src/ImageAudioInputNode.cpp)
|
||||||
|
target_link_libraries(input_image_audio_node ${OpenCV_LIBS})
|
||||||
|
rosbuild_link_boost(input_image_audio_node signals)
|
||||||
|
|
||||||
|
rosbuild_add_executable(input_image_audio_twist_node src/ImageAudioTwistInputNode.cpp)
|
||||||
|
target_link_libraries(input_image_audio_twist_node ${OpenCV_LIBS})
|
||||||
|
rosbuild_link_boost(input_image_audio_twist_node signals)
|
||||||
|
|
||||||
|
rosbuild_add_executable(input_image_twist_node src/ImageTwistInputNode.cpp)
|
||||||
|
target_link_libraries(input_image_twist_node ${OpenCV_LIBS})
|
||||||
|
rosbuild_link_boost(input_image_twist_node signals)
|
||||||
|
|
||||||
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
|
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui)
|
||||||
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
|
IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
|
||||||
INCLUDE(${QT_USE_FILE})
|
INCLUDE(${QT_USE_FILE})
|
||||||
rosbuild_add_executable(rtabmap_gui src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
|
rosbuild_add_executable(rtabmap_gui src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
|
||||||
target_link_libraries(rtabmap_gui ${QT_LIBRARIES} ${OpenCV_LIBS} "-lrtabmap_gui")
|
target_link_libraries(rtabmap_gui ${QT_LIBRARIES} ${OpenCV_LIBS} "-lrtabmap_gui")
|
||||||
|
|
||||||
|
rosbuild_add_executable(visual_attention src/VisualAttentionNode.cpp)
|
||||||
|
target_link_libraries(visual_attention ${QT_LIBRARIES} ${OpenCV_LIBS})
|
||||||
ELSE()
|
ELSE()
|
||||||
MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...")
|
MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI node will not be compiled...")
|
||||||
ENDIF()
|
ENDIF()
|
||||||
|
|||||||
@@ -0,0 +1,11 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
export GDK_NATIVE_WINDOWS=1
|
||||||
|
|
||||||
|
## Source ROS setup.sh (adjust to your version : boxturtle, cturtle, diamondback, e...)
|
||||||
|
source /opt/ros/fuerte/setup.bash
|
||||||
|
|
||||||
|
## Setup ROS_PACKAGE_PATH
|
||||||
|
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/workspace/ros-pkg
|
||||||
|
|
||||||
|
## Start eclipse
|
||||||
|
/usr/bin/eclipse
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
[Desktop Entry]
|
||||||
|
Name=Eclipse
|
||||||
|
GenericName=eclipse
|
||||||
|
Comment=eclipse
|
||||||
|
Keywords=eclipse
|
||||||
|
Exec=/PATH/TO/THIS/DIRECTORY/eclipse-launch.sh
|
||||||
|
Terminal=false
|
||||||
|
Type=Application
|
||||||
|
StartupNotify=true
|
||||||
|
Icon=/PATH/TO/THIS/DIRECTORY/eclipse48.png
|
||||||
Binary file not shown.
|
After Width: | Height: | Size: 3.0 KiB |
@@ -0,0 +1,36 @@
|
|||||||
|
<launch>
|
||||||
|
|
||||||
|
<!-- RTAB-MAP LOOP CLOSURE DETECTION VERSION : with sensorimotor memory type-->
|
||||||
|
<!-- WARNING : Database is automatically deleted on each startup -->
|
||||||
|
<!-- See "delete_db_on_start" option below... -->
|
||||||
|
|
||||||
|
<!-- args: "delete_db_on_start" and "udebug" -->
|
||||||
|
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
|
||||||
|
<remap from="/image" to="/image_not_used"/> <!-- Just to make sure that rtabmap does not subscribe
|
||||||
|
to camera image, here we use sensorimotor input -->
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="rtabmap_gui" pkg="rtabmap" type="rtabmap_gui" output="screen"/>
|
||||||
|
|
||||||
|
<node name="camera" pkg="rtabmap" type="camera" output="screen">
|
||||||
|
<remap from="/camera/image" to="/image"/>
|
||||||
|
<param name="device_id" value="0" type="int"/>
|
||||||
|
<param name="image_hz" value="5.0" type="double"/>
|
||||||
|
<param name="image_width" value="80" type="int"/>
|
||||||
|
<param name="image_height" value="60" type="int"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="my_audio_recorder" pkg="rtabmap" type="audio_recorder" output="screen">
|
||||||
|
<param name="device_id" value="0" type="int"/>
|
||||||
|
<param name="file_name" value="" type="string"/> <!-- use the micro -->
|
||||||
|
<param name="frame_length" value="4800" type="int"/> <!-- Must match with image_hz of the camera
|
||||||
|
(5 Hz at 24000 fs = 4800 samples / frame) -->
|
||||||
|
<param name="fs" value="24000" type="int"/>
|
||||||
|
<param name="sample_size" value="2" type="int"/> <!-- 16bits/sample -->
|
||||||
|
<param name="channels" value="1" type="int"/> <!-- mono -->
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- This node will synchronize images and audio frames, and
|
||||||
|
transform them to a Sensorimotor topic used by RTAB-Map. -->
|
||||||
|
<node name="my_input_node" pkg="rtabmap" type="input_image_audio_node" />
|
||||||
|
</launch>
|
||||||
@@ -0,0 +1,41 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- teleop -->
|
||||||
|
<node pkg="pr2_teleop" type="teleop_pr2_keyboard" name="spawn_teleop_keyboard" output="screen">
|
||||||
|
<remap from="cmd_vel" to="user/cmd_vel" />
|
||||||
|
|
||||||
|
<param name="walk_vel" value="0.5" />
|
||||||
|
<param name="run_vel" value="1.0" />
|
||||||
|
<param name="yaw_rate" value="1.0" />
|
||||||
|
<param name="yaw_run_rate" value="1.5" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- camera -->
|
||||||
|
<node name="camera" pkg="rtabmap" type="camera" output="screen">
|
||||||
|
<remap from="camera/image" to="image"/>
|
||||||
|
<param name="device_id" value="0" type="int"/>
|
||||||
|
<param name="image_hz" value="10.0" type="double"/>
|
||||||
|
<param name="image_width" value="80" type="int"/>
|
||||||
|
<param name="image_height" value="60" type="int"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- mic -->
|
||||||
|
<node name="my_audio_recorder" pkg="rtabmap" type="audio_recorder" output="screen">
|
||||||
|
<param name="device_id" value="0" type="int"/>
|
||||||
|
<param name="file_name" value="" type="string"/> <!-- use the micro -->
|
||||||
|
<param name="frame_length" value="2400" type="int"/> <!-- Must match with image_hz of the camera
|
||||||
|
(10 Hz at 24000 fs = 2400 samples / frame) -->
|
||||||
|
<param name="fs" value="24000" type="int"/>
|
||||||
|
<param name="sample_size" value="2" type="int"/> <!-- 16bits/sample -->
|
||||||
|
<param name="channels" value="1" type="int"/> <!-- mono -->
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- Lasers -->
|
||||||
|
<!-- ls -l /dev/ttyACM* -->
|
||||||
|
<!-- sudo chmod a+rw /dev/ttyACM* -->
|
||||||
|
|
||||||
|
<node pkg="hokuyo_node" type="hokuyo_node" name="laser_top_publisher" ns="laser_top">
|
||||||
|
<param name="frame_id" type="string" value="laser_top"/>
|
||||||
|
<param name="port" type="string" value="/dev/ttyACM0"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
</launch>
|
||||||
@@ -1,7 +1,7 @@
|
|||||||
<launch>
|
<launch>
|
||||||
<!-- TELEOP JOYSTICK -->
|
<!-- TELEOP JOYSTICK -->
|
||||||
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
|
<node name="teleop_pr2" pkg="pr2_teleop" type="teleop_pr2" args="--deadman_no_publish">
|
||||||
<remap from="cmd_vel" to="user/cmd_vel" />
|
<remap from="cmd_vel" to="tr/cmd_vel" />
|
||||||
</node>
|
</node>
|
||||||
<node name="joystick" pkg="joy" type="joy_node">
|
<node name="joystick" pkg="joy" type="joy_node">
|
||||||
<param name="autorepeat_rate" value="20" type="double" />
|
<param name="autorepeat_rate" value="20" type="double" />
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
<launch>
|
<launch>
|
||||||
<node pkg="pr2_teleop" type="teleop_pr2_keyboard" name="spawn_teleop_keyboard" output="screen">
|
<node pkg="pr2_teleop" type="teleop_pr2_keyboard" name="spawn_teleop_keyboard" output="screen">
|
||||||
<remap from="cmd_vel" to="user/cmd_vel" />
|
<remap from="cmd_vel" to="tr/cmd_vel" />
|
||||||
|
|
||||||
<param name="walk_vel" value="0.5" />
|
<param name="walk_vel" value="0.5" />
|
||||||
<param name="run_vel" value="1.0" />
|
<param name="run_vel" value="1.0" />
|
||||||
|
|||||||
@@ -0,0 +1,57 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- launch teleop with teleop_keyboard.launch -->
|
||||||
|
|
||||||
|
<!-- args: "delete_db_on_start" and "udebug" -->
|
||||||
|
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
|
||||||
|
<remap from="/image" to="/image_not_used"/> <!-- Just to make sure that rtabmap does not subscribe
|
||||||
|
to camera image, here we use sensorimotor input -->
|
||||||
|
<remap from="sensorimotor" to="sensorimotor" />
|
||||||
|
<remap from="rtabmap/info" to="rtabmap/info" />
|
||||||
|
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
|
||||||
|
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
|
||||||
|
<remap from="rtabmap/info" to="rtabmap/info" />
|
||||||
|
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- Arbitration -->
|
||||||
|
<!-- cmd_vel_a has priority on cmd_vel_b -->
|
||||||
|
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
|
||||||
|
<param name="commands_hz" value="10.0" type="double"/>
|
||||||
|
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
|
||||||
|
<param name="cmd_vel_b_buffered" value="true" type="bool"/>
|
||||||
|
<param name="stats_logged" value="false" type="bool"/>
|
||||||
|
<remap from="cmd_vel_a" to="user/cmd_vel" />
|
||||||
|
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
|
||||||
|
<remap from="cmd_vel" to="cmd_vel" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- INPUT NODE -->
|
||||||
|
<node name="my_input_node" pkg="rtabmap" type="input_image_audio_twist_node">
|
||||||
|
<remap from="cmd_vel" to="cmd_vel"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- SENSORS -->
|
||||||
|
<!-- camera -->
|
||||||
|
<node name="camera" pkg="rtabmap" type="camera" output="screen">
|
||||||
|
<remap from="camera/image" to="image"/>
|
||||||
|
<param name="device_id" value="0" type="int"/>
|
||||||
|
<param name="image_hz" value="10.0" type="double"/>
|
||||||
|
<param name="image_width" value="80" type="int"/>
|
||||||
|
<param name="image_height" value="60" type="int"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- mic -->
|
||||||
|
<node name="my_audio_recorder" pkg="rtabmap" type="audio_recorder" output="screen">
|
||||||
|
<param name="device_id" value="0" type="int"/>
|
||||||
|
<param name="file_name" value="" type="string"/> <!-- use the micro -->
|
||||||
|
<param name="frame_length" value="2400" type="int"/> <!-- Must match with image_hz of the camera
|
||||||
|
(10 Hz at 24000 fs = 2400 samples / frame) -->
|
||||||
|
<param name="fs" value="24000" type="int"/>
|
||||||
|
<param name="sample_size" value="2" type="int"/> <!-- 16bits/sample -->
|
||||||
|
<param name="channels" value="1" type="int"/> <!-- mono -->
|
||||||
|
</node>
|
||||||
|
|
||||||
|
</launch>
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- when rosbag ... "rosbag play -.-clock my.bag"-->
|
||||||
|
<param name="use_sim_time" type="bool" value="True"/>
|
||||||
|
|
||||||
|
<!-- args: "delete_db_on_start" and "udebug" -->
|
||||||
|
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
|
||||||
|
<remap from="/image" to="/image_not_used"/> <!-- Just to make sure that rtabmap does not subscribe
|
||||||
|
to camera image, here we use sensorimotor input -->
|
||||||
|
<remap from="sensorimotor" to="sensorimotor" />
|
||||||
|
<remap from="rtabmap/info" to="rtabmap/info" />
|
||||||
|
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="rtabmap_out" pkg="rtabmap" type="rtabmap_out">
|
||||||
|
<remap from="rtabmap/cmd_vel" to="rtabmap/cmd_vel" />
|
||||||
|
<remap from="rtabmap/info" to="rtabmap/info" />
|
||||||
|
<remap from="rtabmap/info_x" to="rtabmap/info_x" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- Arbitration -->
|
||||||
|
<!-- cmd_vel_a has priority on cmd_vel_b -->
|
||||||
|
<node name="abtr_velocity" pkg="rtabmap" type="abtr_velocity">
|
||||||
|
<param name="commands_hz" value="10.0" type="double"/>
|
||||||
|
<param name="cmd_vel_a_buffered" value="false" type="bool"/>
|
||||||
|
<param name="cmd_vel_b_buffered" value="true" type="bool"/>
|
||||||
|
<param name="stats_logged" value="false" type="bool"/>
|
||||||
|
<remap from="cmd_vel_a" to="user/cmd_vel" />
|
||||||
|
<remap from="cmd_vel_b" to="rtabmap/cmd_vel" />
|
||||||
|
<remap from="cmd_vel" to="cmd_vel" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<!-- INPUT NODE -->
|
||||||
|
<node name="my_input_node" pkg="rtabmap" type="input_image_twist_node">
|
||||||
|
<remap from="cmd_vel" to="cmd_vel"/>
|
||||||
|
<remap from="image" to="image_local_polar_reconstructed"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
</launch>
|
||||||
@@ -0,0 +1,34 @@
|
|||||||
|
<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="visual_attention" pkg="rtabmap" type="visual_attention" output="screen"/>
|
||||||
|
|
||||||
|
<!--
|
||||||
|
<node name="image" pkg="rtabmap_image" type="image_view_qt" output="screen">
|
||||||
|
<remap from="image" to="image"/>
|
||||||
|
</node>
|
||||||
|
-->
|
||||||
|
<node name="image_motion" pkg="rtabmap_image" type="image_view_qt" output="screen">
|
||||||
|
<remap from="image" to="image_motion"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="image_motion_local" pkg="rtabmap_image" type="image_view_qt" output="screen">
|
||||||
|
<remap from="image" to="image_motion_local"/>
|
||||||
|
</node>
|
||||||
|
<node name="image_local" pkg="rtabmap_image" type="image_view_qt" output="screen">
|
||||||
|
<remap from="image" to="image_local"/>
|
||||||
|
</node>
|
||||||
|
<node name="image_motion_local_polar" pkg="rtabmap_image" type="image_view_qt" output="screen">
|
||||||
|
<remap from="image" to="image_motion_local_polar"/>
|
||||||
|
</node>
|
||||||
|
<node name="image_motion_local_polar_reconstructed" pkg="rtabmap_image" type="image_view_qt" output="screen">
|
||||||
|
<remap from="image" to="image_motion_local_polar_reconstructed"/>
|
||||||
|
</node>
|
||||||
|
</launch>
|
||||||
@@ -7,7 +7,7 @@
|
|||||||
<author>Mathieu Labbe</author>
|
<author>Mathieu Labbe</author>
|
||||||
<license>GPL</license>
|
<license>GPL</license>
|
||||||
<review status="unreviewed" notes=""/>
|
<review status="unreviewed" notes=""/>
|
||||||
<url>http://rtabmap-ros-pkg.googlecode.com</url>
|
<url>http://rtabmap.googlecode.com</url>
|
||||||
<depend package="std_msgs"/>
|
<depend package="std_msgs"/>
|
||||||
<depend package="roscpp"/>
|
<depend package="roscpp"/>
|
||||||
<depend package="image_transport"/>
|
<depend package="image_transport"/>
|
||||||
@@ -17,8 +17,8 @@
|
|||||||
<depend package="turtlesim"/>
|
<depend package="turtlesim"/>
|
||||||
<depend package="tf"/>
|
<depend package="tf"/>
|
||||||
<depend package="rtabmap_lib"/>
|
<depend package="rtabmap_lib"/>
|
||||||
|
<depend package="rtabmap_audio"/>
|
||||||
<rosdep name="opencv2.3"/>
|
<depend package="rtabmap_image"/>
|
||||||
|
|
||||||
</package>
|
</package>
|
||||||
|
|
||||||
|
|||||||
@@ -0,0 +1,6 @@
|
|||||||
|
########################################
|
||||||
|
# Actuator
|
||||||
|
########################################
|
||||||
|
|
||||||
|
uint32 type # Actuator type, refer to rtabmap::Actuator::Type enum)
|
||||||
|
rtabmap/CvMatMsg matrix
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
|
||||||
|
########################################
|
||||||
|
# OpenCV Matrix description
|
||||||
|
########################################
|
||||||
|
|
||||||
|
uint32 width # Matrix width
|
||||||
|
uint32 height # Matrix height
|
||||||
|
uint32 dataType # Matrix data type (refer to OpenCV type, e.g. CV_8UC3 for a RGB image 8bits)
|
||||||
|
uint8[] data # Matrix data
|
||||||
|
uint8 compressed # if data is compressed (in case of an image for example)
|
||||||
@@ -11,7 +11,6 @@ Header header
|
|||||||
int32 refId
|
int32 refId
|
||||||
int32 loopClosureId
|
int32 loopClosureId
|
||||||
|
|
||||||
uint32 actuatorStep
|
rtabmap/ActuatorMsg[] actuators
|
||||||
float32[] actuators
|
|
||||||
|
|
||||||
rtabmap/RtabmapInfoEx infoEx
|
rtabmap/RtabmapInfoEx infoEx
|
||||||
@@ -7,37 +7,32 @@
|
|||||||
|
|
||||||
Header header
|
Header header
|
||||||
|
|
||||||
sensor_msgs/CompressedImage refImage
|
|
||||||
int32 refChild
|
int32 refChild
|
||||||
|
|
||||||
sensor_msgs/CompressedImage loopClosureImage
|
# std::map<int, float> posterior;
|
||||||
|
|
||||||
#std::map<int, float> posterior;
|
|
||||||
int32[] posteriorKeys
|
int32[] posteriorKeys
|
||||||
float32[] posteriorValues
|
float32[] posteriorValues
|
||||||
|
|
||||||
#std::map<int, float> likelihood;
|
# std::map<int, float> likelihood;
|
||||||
int32[] likelihoodKeys
|
int32[] likelihoodKeys
|
||||||
float32[] likelihoodValues
|
float32[] likelihoodValues
|
||||||
|
|
||||||
#std::map<int, int> weights;
|
# std::map<int, int> weights;
|
||||||
int32[] weightsKeys
|
int32[] weightsKeys
|
||||||
int32[] weightsValues
|
int32[] weightsValues
|
||||||
|
|
||||||
#std::map<std::string, float> stats
|
# std::map<std::string, float> stats
|
||||||
string[] statsKeys
|
string[] statsKeys
|
||||||
float32[] statsValues
|
float32[] statsValues
|
||||||
|
|
||||||
##Keypoints##
|
rtabmap/SensorMsg[] refRawData
|
||||||
#std::multimap<int, cv::KeyPoint> refWords
|
rtabmap/SensorMsg[] loopRawData
|
||||||
|
|
||||||
|
#
|
||||||
|
# For features2d : std::multimap<int, cv::Keypoint> words
|
||||||
|
#
|
||||||
int32[] refWordsKeys
|
int32[] refWordsKeys
|
||||||
rtabmap/KeyPoint[] refWordsValues
|
rtabmap/KeyPoint[] refWordsValues
|
||||||
|
|
||||||
#std::multimap<int, cv::KeyPoint> loopWords
|
|
||||||
int32[] loopWordsKeys
|
int32[] loopWordsKeys
|
||||||
rtabmap/KeyPoint[] loopWordsValues
|
rtabmap/KeyPoint[] loopWordsValues
|
||||||
|
|
||||||
##SM masks##
|
|
||||||
uint8[] refMotionMask
|
|
||||||
uint8[] loopMotionMask
|
|
||||||
|
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
########################################
|
||||||
|
# Sensor
|
||||||
|
########################################
|
||||||
|
|
||||||
|
uint32 type # Sensor type (e.g. for sensor: image, audio),
|
||||||
|
# refer to rtabmap::Sensor::Type).
|
||||||
|
rtabmap/CvMatMsg matrix
|
||||||
|
|
||||||
@@ -0,0 +1,4 @@
|
|||||||
|
Header header
|
||||||
|
|
||||||
|
rtabmap/SensorMsg[] sensors
|
||||||
|
rtabmap/ActuatorMsg[] actuators
|
||||||
@@ -1,14 +0,0 @@
|
|||||||
|
|
||||||
# Wrap the rtabmap::SMState class
|
|
||||||
|
|
||||||
Header header
|
|
||||||
|
|
||||||
uint32 sensorStep
|
|
||||||
float32[] sensors
|
|
||||||
|
|
||||||
uint32 actuatorStep
|
|
||||||
float32[] actuators
|
|
||||||
|
|
||||||
# Optional fields
|
|
||||||
sensor_msgs/Image image
|
|
||||||
rtabmap/KeyPoint[] keypoints
|
|
||||||
@@ -6,6 +6,7 @@
|
|||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <geometry_msgs/Twist.h>
|
#include <geometry_msgs/Twist.h>
|
||||||
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
#include <utilite/UMutex.h>
|
#include <utilite/UMutex.h>
|
||||||
#include <queue>
|
#include <queue>
|
||||||
#include <utilite/ULogger.h>
|
#include <utilite/ULogger.h>
|
||||||
@@ -103,7 +104,7 @@ int main(int argc, char** argv)
|
|||||||
velBTopic = nh.subscribe("cmd_vel_b", 1, velocityBReceivedCallback);
|
velBTopic = nh.subscribe("cmd_vel_b", 1, velocityBReceivedCallback);
|
||||||
}
|
}
|
||||||
|
|
||||||
rosPublisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
rosPublisher = nh.advertise<geometry_msgs::TwistStamped>("cmd_vel", 1);
|
||||||
|
|
||||||
int index = 1;
|
int index = 1;
|
||||||
ros::Rate loop_rate(commandsHz); // 10 Hz
|
ros::Rate loop_rate(commandsHz); // 10 Hz
|
||||||
@@ -111,24 +112,28 @@ int main(int argc, char** argv)
|
|||||||
{
|
{
|
||||||
loop_rate.sleep();
|
loop_rate.sleep();
|
||||||
ros::spinOnce();
|
ros::spinOnce();
|
||||||
|
|
||||||
|
geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped());
|
||||||
|
vel->header.frame_id = "base_link";
|
||||||
|
vel->header.stamp = ros::Time::now();
|
||||||
|
|
||||||
// priority for commandsA
|
// priority for commandsA
|
||||||
if(commandsA.size())
|
if(commandsA.size())
|
||||||
{
|
{
|
||||||
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
|
vel->twist.linear.x = commandsA.front();
|
||||||
vel->linear.x = commandsA.front();
|
|
||||||
commandsA.pop();
|
commandsA.pop();
|
||||||
vel->linear.y = commandsA.front();
|
vel->twist.linear.y = commandsA.front();
|
||||||
commandsA.pop();
|
commandsA.pop();
|
||||||
vel->linear.z = commandsA.front();
|
vel->twist.linear.z = commandsA.front();
|
||||||
commandsA.pop();
|
commandsA.pop();
|
||||||
vel->angular.x = commandsA.front();
|
vel->twist.angular.x = commandsA.front();
|
||||||
commandsA.pop();
|
commandsA.pop();
|
||||||
vel->angular.y = commandsA.front();
|
vel->twist.angular.y = commandsA.front();
|
||||||
commandsA.pop();
|
commandsA.pop();
|
||||||
vel->angular.z = commandsA.front();
|
vel->twist.angular.z = commandsA.front();
|
||||||
commandsA.pop();
|
commandsA.pop();
|
||||||
rosPublisher.publish(vel);
|
rosPublisher.publish(vel);
|
||||||
UINFO("%d A %f %f %f", index, vel->linear.x, vel->linear.y, vel->angular.z);
|
UINFO("%d A %f %f %f", index, vel->twist.linear.x, vel->twist.linear.y, vel->twist.angular.z);
|
||||||
if(!cmdABuffered)
|
if(!cmdABuffered)
|
||||||
{
|
{
|
||||||
commandsA = std::queue<float>();
|
commandsA = std::queue<float>();
|
||||||
@@ -145,20 +150,19 @@ int main(int argc, char** argv)
|
|||||||
}
|
}
|
||||||
else if(commandsB.size())
|
else if(commandsB.size())
|
||||||
{
|
{
|
||||||
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
|
vel->twist.linear.x = commandsB.front();
|
||||||
vel->linear.x = commandsB.front();
|
|
||||||
commandsB.pop();
|
commandsB.pop();
|
||||||
vel->linear.y = commandsB.front();
|
vel->twist.linear.y = commandsB.front();
|
||||||
commandsB.pop();
|
commandsB.pop();
|
||||||
vel->linear.z = commandsB.front();
|
vel->twist.linear.z = commandsB.front();
|
||||||
commandsB.pop();
|
commandsB.pop();
|
||||||
vel->angular.x = commandsB.front();
|
vel->twist.angular.x = commandsB.front();
|
||||||
commandsB.pop();
|
commandsB.pop();
|
||||||
vel->angular.y = commandsB.front();
|
vel->twist.angular.y = commandsB.front();
|
||||||
commandsB.pop();
|
commandsB.pop();
|
||||||
vel->angular.z = commandsB.front();
|
vel->twist.angular.z = commandsB.front();
|
||||||
commandsB.pop();
|
commandsB.pop();
|
||||||
UINFO("%d B %f %f %f", index, vel->linear.x, vel->linear.y, vel->angular.z);
|
UINFO("%d B %f %f %f", index, vel->twist.linear.x, vel->twist.linear.y, vel->twist.angular.z);
|
||||||
rosPublisher.publish(vel);
|
rosPublisher.publish(vel);
|
||||||
if(!cmdBBuffered)
|
if(!cmdBBuffered)
|
||||||
{
|
{
|
||||||
@@ -172,13 +176,12 @@ int main(int argc, char** argv)
|
|||||||
{
|
{
|
||||||
// Republish the last command one more time
|
// Republish the last command one more time
|
||||||
// (if the sender cannot reach commandsHz for an iteration)
|
// (if the sender cannot reach commandsHz for an iteration)
|
||||||
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
|
vel->twist.linear.x = lastCommandsB[0];
|
||||||
vel->linear.x = lastCommandsB[0];
|
vel->twist.linear.y = lastCommandsB[1];
|
||||||
vel->linear.y = lastCommandsB[1];
|
vel->twist.linear.z = lastCommandsB[2];
|
||||||
vel->linear.z = lastCommandsB[2];
|
vel->twist.angular.x = lastCommandsB[3];
|
||||||
vel->angular.x = lastCommandsB[3];
|
vel->twist.angular.y = lastCommandsB[4];
|
||||||
vel->angular.y = lastCommandsB[4];
|
vel->twist.angular.z = lastCommandsB[5];
|
||||||
vel->angular.z = lastCommandsB[5];
|
|
||||||
rosPublisher.publish(vel);
|
rosPublisher.publish(vel);
|
||||||
lastCommandsB = std::vector<float>();
|
lastCommandsB = std::vector<float>();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,72 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraNode.cpp
|
|
||||||
*
|
|
||||||
* Created on: 1 févr. 2010
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <sensor_msgs/Image.h>
|
|
||||||
#include "CameraWrapper.h"
|
|
||||||
#include <rtabmap/core/Camera.h>
|
|
||||||
#include <utilite/ULogger.h>
|
|
||||||
|
|
||||||
// See the launch file to change the camera values
|
|
||||||
#define DEFAULT_DEVICE_ID 0
|
|
||||||
#define DEFAULT_IMG_RATE 1.0 //Hz
|
|
||||||
#define DEFAULT_AUTO_RESTART 0
|
|
||||||
#define DEFAULT_IMG_WIDTH 0
|
|
||||||
#define DEFAULT_IMG_HEIGHT 0
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "camera");
|
|
||||||
CameraWrapper * camera = 0;
|
|
||||||
ros::NodeHandle nh("~");
|
|
||||||
|
|
||||||
int deviceId = DEFAULT_DEVICE_ID;
|
|
||||||
double imgRate = DEFAULT_IMG_RATE;
|
|
||||||
int autoRestart = DEFAULT_AUTO_RESTART;
|
|
||||||
int imgWidth = DEFAULT_IMG_WIDTH;
|
|
||||||
int imgHeight = DEFAULT_IMG_HEIGHT;
|
|
||||||
|
|
||||||
nh.param("device_id", deviceId, deviceId);
|
|
||||||
nh.param("image_hz", imgRate, imgRate);
|
|
||||||
nh.param("auto_restart", autoRestart, autoRestart);
|
|
||||||
nh.param("image_width", imgWidth, imgWidth);
|
|
||||||
nh.param("image_height", imgHeight, imgHeight);
|
|
||||||
|
|
||||||
ROS_INFO("device_id=%d", deviceId);
|
|
||||||
ROS_INFO("image_hz=%f", imgRate);
|
|
||||||
ROS_INFO("auto_restart=%d", autoRestart);
|
|
||||||
ROS_INFO("image_width=%d", imgWidth);
|
|
||||||
ROS_INFO("image_height=%d", imgHeight);
|
|
||||||
|
|
||||||
nh.setParam("device_id", deviceId);
|
|
||||||
nh.setParam("image_hz", imgRate);
|
|
||||||
nh.setParam("auto_restart", autoRestart);
|
|
||||||
nh.setParam("image_width", imgWidth);
|
|
||||||
nh.setParam("image_height", imgHeight);
|
|
||||||
|
|
||||||
camera = new CameraVideoWrapper(deviceId, float(imgRate), autoRestart, 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...");
|
|
||||||
ros::spin();
|
|
||||||
}
|
|
||||||
|
|
||||||
//cleanup
|
|
||||||
if(camera)
|
|
||||||
{
|
|
||||||
delete camera;
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,85 +0,0 @@
|
|||||||
/*
|
|
||||||
* 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 <highgui.h>
|
|
||||||
#include <utilite/UDirectory.h>
|
|
||||||
#include <utilite/UConversion.h>
|
|
||||||
|
|
||||||
bool imagesSaved = false;
|
|
||||||
int i = 0;
|
|
||||||
|
|
||||||
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
|
||||||
{
|
|
||||||
if(msg->data.size())
|
|
||||||
{
|
|
||||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
|
||||||
IplImage image = ptr->image;
|
|
||||||
|
|
||||||
ROS_INFO("Received an image size=(%d,%d)", image.width, image.height);
|
|
||||||
|
|
||||||
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(!cvSaveImage(path.c_str(), &image))
|
|
||||||
{
|
|
||||||
ROS_ERROR("Cannot save image to %s", path.c_str());
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ROS_INFO("Saved image %s", path.c_str());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
cvShowImage( "ImageReceived", &image);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "camera_receiver");
|
|
||||||
ros::NodeHandle pn("~");
|
|
||||||
|
|
||||||
pn.param("images_saved", imagesSaved, imagesSaved);
|
|
||||||
ROS_INFO("images_saved=%d", imagesSaved?1:0);
|
|
||||||
|
|
||||||
if(!imagesSaved)
|
|
||||||
{
|
|
||||||
cvStartWindowThread();
|
|
||||||
cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
|
|
||||||
cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
|
|
||||||
}
|
|
||||||
|
|
||||||
ros::NodeHandle n;
|
|
||||||
image_transport::ImageTransport it(n);
|
|
||||||
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
|
||||||
|
|
||||||
ROS_INFO("Waiting for images...");
|
|
||||||
|
|
||||||
ros::spin();
|
|
||||||
|
|
||||||
if(!imagesSaved)
|
|
||||||
{
|
|
||||||
cvDestroyWindow("ImageReceived");
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,43 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraNodeReceiver.cpp
|
|
||||||
*
|
|
||||||
* Created on: 2 févr. 2010
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include "rtabmap/SensoryMotorState.h"
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
#include <highgui.h>
|
|
||||||
|
|
||||||
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
|
|
||||||
{
|
|
||||||
if( msg->image.data.size())
|
|
||||||
{
|
|
||||||
boost::shared_ptr<sensor_msgs::Image> tracked_object;
|
|
||||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg->image, tracked_object);
|
|
||||||
IplImage image = ptr->image;
|
|
||||||
|
|
||||||
ROS_INFO("Received an image size=(%d,%d)", image.width, image.height);
|
|
||||||
cvShowImage( "ImageReceived", &image);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
cvStartWindowThread();
|
|
||||||
cvNamedWindow("ImageReceived", CV_WINDOW_AUTOSIZE);
|
|
||||||
cvMoveWindow("ImageReceived", 100, 100); // offset from the UL corner of the screen
|
|
||||||
|
|
||||||
ros::init(argc, argv, "camera_receiver_sms");
|
|
||||||
ros::NodeHandle n;
|
|
||||||
ros::Subscriber image_sub = n.subscribe("/sm_state", 1, smReceivedCallback);
|
|
||||||
|
|
||||||
ROS_INFO("Waiting for Sensorimotor states (containing images)...");
|
|
||||||
|
|
||||||
ros::spin();
|
|
||||||
|
|
||||||
cvDestroyWindow("ImageReceived");
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,141 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraWrapper.cpp
|
|
||||||
*
|
|
||||||
* Created on: 2 févr. 2010
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "CameraWrapper.h"
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
#include <sensor_msgs/image_encodings.h>
|
|
||||||
#include <sensor_msgs/Image.h>
|
|
||||||
#include <rtabmap/core/CameraEvent.h>
|
|
||||||
#include <rtabmap/core/SMState.h>
|
|
||||||
#include <utilite/UEventsManager.h>
|
|
||||||
#include <rtabmap/core/Camera.h>
|
|
||||||
#include <rtabmap/core/Parameters.h>
|
|
||||||
|
|
||||||
//msgs
|
|
||||||
#include "rtabmap/SensoryMotorState.h"
|
|
||||||
|
|
||||||
CameraWrapper::CameraWrapper() :
|
|
||||||
camera_(0)
|
|
||||||
{
|
|
||||||
ros::NodeHandle nh("~");
|
|
||||||
image_transport::ImageTransport it(nh);
|
|
||||||
rosPublisher_ = it.advertise("image", 1);
|
|
||||||
changeCameraImgRateSrv_ = nh.advertiseService("changeImgRate", &CameraWrapper::changeCameraImgRateCallback, this);
|
|
||||||
UEventsManager::addHandler(this);
|
|
||||||
}
|
|
||||||
|
|
||||||
CameraWrapper::~CameraWrapper()
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void CameraWrapper::setCamera(rtabmap::Camera * camera)
|
|
||||||
{
|
|
||||||
if(camera)
|
|
||||||
{
|
|
||||||
if(camera_)
|
|
||||||
{
|
|
||||||
delete camera_;
|
|
||||||
camera_ = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
camera_ = camera;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// ownership is transferred
|
|
||||||
void CameraWrapper::setPostThreatement(rtabmap::CamPostTreatment * strategy)
|
|
||||||
{
|
|
||||||
if(camera_ && strategy)
|
|
||||||
{
|
|
||||||
camera_->setPostThreatement(strategy);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CameraWrapper::init()
|
|
||||||
{
|
|
||||||
if(camera_)
|
|
||||||
{
|
|
||||||
return camera_->init();
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CameraWrapper::start()
|
|
||||||
{
|
|
||||||
if(camera_)
|
|
||||||
{
|
|
||||||
return camera_->start();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request & request, rtabmap::ChangeCameraImgRate::Response & response)
|
|
||||||
{
|
|
||||||
ros::NodeHandle nh("~");
|
|
||||||
nh.setParam("image_hz", request.imgRate);
|
|
||||||
camera_->setImageRate(request.imgRate);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CameraWrapper::handleEvent(UEvent* anEvent)
|
|
||||||
{
|
|
||||||
if(anEvent->getClassName().compare("SMStateEvent") == 0)
|
|
||||||
{
|
|
||||||
rtabmap::SMStateEvent * e = (rtabmap::SMStateEvent*)anEvent;
|
|
||||||
const rtabmap::SMState * smState = e->getSMState();
|
|
||||||
if(smState && smState->getImage())
|
|
||||||
{
|
|
||||||
cv_bridge::CvImage img;
|
|
||||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
|
||||||
img.image = smState->getImage();
|
|
||||||
rosPublisher_.publish(img.toImageMsg());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Camera Image wrapper
|
|
||||||
CameraImagesWrapper::CameraImagesWrapper(
|
|
||||||
const std::string & path,
|
|
||||||
int startAt,
|
|
||||||
bool refreshDir,
|
|
||||||
float imageRate,
|
|
||||||
bool autoRestart,
|
|
||||||
unsigned int imageWidth,
|
|
||||||
unsigned int imageHeight)
|
|
||||||
{
|
|
||||||
this->setCamera(new rtabmap::CameraImages(path, startAt, refreshDir, imageRate, autoRestart, imageWidth, imageHeight));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Camera Video wrapper
|
|
||||||
CameraVideoWrapper::CameraVideoWrapper(int usbDevice,
|
|
||||||
float imageRate,
|
|
||||||
bool autoRestart,
|
|
||||||
unsigned int imageWidth,
|
|
||||||
unsigned int imageHeight)
|
|
||||||
{
|
|
||||||
this->setCamera(new rtabmap::CameraVideo(usbDevice, imageRate, autoRestart, imageWidth, imageHeight));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
CameraVideoWrapper::CameraVideoWrapper(const std::string & fileName,
|
|
||||||
float imageRate,
|
|
||||||
bool autoRestart,
|
|
||||||
unsigned int imageWidth,
|
|
||||||
unsigned int imageHeight)
|
|
||||||
{
|
|
||||||
this->setCamera(new rtabmap::CameraVideo(fileName, imageRate, autoRestart, imageWidth, imageHeight));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Camera database wrapper
|
|
||||||
CameraDatabaseWrapper::CameraDatabaseWrapper(const std::string & path,
|
|
||||||
bool actionsLoaded,
|
|
||||||
float imageRate,
|
|
||||||
bool autoRestart,
|
|
||||||
unsigned int imageWidth,
|
|
||||||
unsigned int imageHeight)
|
|
||||||
{
|
|
||||||
this->setCamera(new rtabmap::CameraDatabase(path, actionsLoaded, imageRate, autoRestart, imageWidth, imageHeight));
|
|
||||||
}
|
|
||||||
@@ -1,106 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraWrapper.h
|
|
||||||
*
|
|
||||||
* Created on: 2 févr. 2010
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef CAMERAWRAPPER_H_
|
|
||||||
#define CAMERAWRAPPER_H_
|
|
||||||
|
|
||||||
#include "utilite/UEventsHandler.h"
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include "rtabmap/ChangeCameraImgRate.h"
|
|
||||||
#include <std_msgs/Empty.h>
|
|
||||||
#include <image_transport/image_transport.h>
|
|
||||||
|
|
||||||
namespace Util
|
|
||||||
{
|
|
||||||
class Event;
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace rtabmap
|
|
||||||
{
|
|
||||||
class Camera;
|
|
||||||
class CamPostTreatment;
|
|
||||||
}
|
|
||||||
|
|
||||||
class CameraWrapper : public UEventsHandler
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
virtual ~CameraWrapper();
|
|
||||||
|
|
||||||
bool init();
|
|
||||||
void start();
|
|
||||||
|
|
||||||
void setPostThreatement(rtabmap::CamPostTreatment * strategy); // ownership is transferred
|
|
||||||
void updateParameters();
|
|
||||||
|
|
||||||
protected:
|
|
||||||
CameraWrapper();
|
|
||||||
virtual void handleEvent(UEvent * anEvent);
|
|
||||||
void setCamera(rtabmap::Camera * camera);
|
|
||||||
|
|
||||||
private:
|
|
||||||
bool changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Request&, rtabmap::ChangeCameraImgRate::Response&);
|
|
||||||
|
|
||||||
private:
|
|
||||||
image_transport::Publisher rosPublisher_;
|
|
||||||
rtabmap::Camera * camera_;
|
|
||||||
ros::ServiceServer changeCameraImgRateSrv_;
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
// Specialized wrappers
|
|
||||||
|
|
||||||
//Image
|
|
||||||
class CameraImagesWrapper : public CameraWrapper
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
CameraImagesWrapper(const std::string & path,
|
|
||||||
int startAt = 1,
|
|
||||||
bool refreshDir = false,
|
|
||||||
float imageRate = 0,
|
|
||||||
bool autoRestart = false,
|
|
||||||
unsigned int imageWidth = 0,
|
|
||||||
unsigned int imageHeight = 0);
|
|
||||||
virtual ~CameraImagesWrapper() {}
|
|
||||||
};
|
|
||||||
|
|
||||||
// Video
|
|
||||||
class CameraVideoWrapper : public CameraWrapper
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
// Usb device like a Webcam
|
|
||||||
CameraVideoWrapper(int usbDevice = 0,
|
|
||||||
float imageRate = 0,
|
|
||||||
bool autoRestart = false,
|
|
||||||
unsigned int imageWidth = 0,
|
|
||||||
unsigned int imageHeight = 0);
|
|
||||||
|
|
||||||
// for a video file (AVI)
|
|
||||||
CameraVideoWrapper(const std::string & fileName,
|
|
||||||
float imageRate = 0,
|
|
||||||
bool autoRestart = false,
|
|
||||||
unsigned int imageWidth = 0,
|
|
||||||
unsigned int imageHeight = 0);
|
|
||||||
|
|
||||||
virtual ~CameraVideoWrapper() {}
|
|
||||||
};
|
|
||||||
|
|
||||||
// Database
|
|
||||||
class CameraDatabaseWrapper : public CameraWrapper
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
// Usb device like a Webcam
|
|
||||||
CameraDatabaseWrapper(const std::string & path,
|
|
||||||
bool actionsLoaded,
|
|
||||||
float imageRate = 0,
|
|
||||||
bool autoRestart = false,
|
|
||||||
unsigned int imageWidth = 0,
|
|
||||||
unsigned int imageHeight = 0);
|
|
||||||
|
|
||||||
virtual ~CameraDatabaseWrapper() {}
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif /* CAMERAWRAPPER_H_ */
|
|
||||||
@@ -6,7 +6,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "CoreWrapper.h"
|
#include "CoreWrapper.h"
|
||||||
#include "utilite/ULogger.h"
|
#include <utilite/ULogger.h>
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
int main(int argc, char** argv)
|
||||||
{
|
{
|
||||||
|
|||||||
+117
-150
@@ -6,18 +6,23 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "CoreWrapper.h"
|
#include "CoreWrapper.h"
|
||||||
#include <rtabmap/core/CameraEvent.h>
|
#include "MsgConversion.h"
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <rtabmap/core/RtabmapEvent.h>
|
#include <rtabmap/core/RtabmapEvent.h>
|
||||||
#include <rtabmap/core/Rtabmap.h>
|
#include <rtabmap/core/Rtabmap.h>
|
||||||
|
#include <rtabmap/core/Camera.h>
|
||||||
#include <rtabmap/core/Parameters.h>
|
#include <rtabmap/core/Parameters.h>
|
||||||
#include <rtabmap/core/SMState.h>
|
#include <rtabmap/core/SensorimotorEvent.h>
|
||||||
#include "utilite/UtiLite.h"
|
#include <utilite/UEventsManager.h>
|
||||||
#include <highgui.h>
|
#include <utilite/ULogger.h>
|
||||||
|
#include <utilite/UFile.h>
|
||||||
|
#include <utilite/UStl.h>
|
||||||
|
#include <opencv2/highgui/highgui.hpp>
|
||||||
|
|
||||||
//msgs
|
//msgs
|
||||||
#include "rtabmap/RtabmapInfo.h"
|
#include "rtabmap/RtabmapInfo.h"
|
||||||
#include "rtabmap/RtabmapInfoEx.h"
|
#include "rtabmap/RtabmapInfoEx.h"
|
||||||
|
#include "rtabmap/CvMatMsg.h"
|
||||||
|
|
||||||
using namespace rtabmap;
|
using namespace rtabmap;
|
||||||
|
|
||||||
@@ -26,9 +31,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
|||||||
{
|
{
|
||||||
ros::NodeHandle nh("~");
|
ros::NodeHandle nh("~");
|
||||||
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
|
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
|
||||||
|
infoPubEx_ = nh.advertise<rtabmap::RtabmapInfo>("infoEx", 1);
|
||||||
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
|
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
|
||||||
|
|
||||||
rtabmap_ = new Rtabmap();
|
rtabmap_ = new Rtabmap();
|
||||||
|
UEventsManager::addHandler(rtabmap_);
|
||||||
loadNodeParameters(rtabmap_->getIniFilePath());
|
loadNodeParameters(rtabmap_->getIniFilePath());
|
||||||
|
|
||||||
if(deleteDbOnStart)
|
if(deleteDbOnStart)
|
||||||
@@ -44,8 +51,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
|||||||
dumpPredictionSrv_ = nh.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
|
dumpPredictionSrv_ = nh.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);
|
||||||
|
|
||||||
nh = ros::NodeHandle();
|
nh = ros::NodeHandle();
|
||||||
smStateTopic_ = nh.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
|
|
||||||
parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
|
parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
|
||||||
|
sensorimotorTopic_ = nh.subscribe("sensorimotor", 1, &CoreWrapper::sensorimotorReceivedCallback, this);
|
||||||
|
|
||||||
image_transport::ImageTransport it(nh);
|
image_transport::ImageTransport it(nh);
|
||||||
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
|
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
|
||||||
@@ -109,44 +116,38 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
|
|||||||
ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str());
|
ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void CoreWrapper::sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg)
|
||||||
void CoreWrapper::smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
|
|
||||||
{
|
{
|
||||||
IplImage * image = 0;
|
std::list<Sensor> sensors;
|
||||||
std::vector<cv::KeyPoint> keypoints(msg->keypoints.size());
|
std::list<Actuator> actuators;
|
||||||
|
|
||||||
if(msg->image.data.size())
|
for(unsigned int i=0; i<msg->sensors.size(); ++i)
|
||||||
{
|
{
|
||||||
boost::shared_ptr<sensor_msgs::Image> tracked_object;
|
sensors.push_back(Sensor(fromCvMatMsgToCvMat(msg->sensors[i].matrix), (Sensor::Type)msg->sensors[i].type));
|
||||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg->image, tracked_object);
|
}
|
||||||
IplImage img = ptr->image;
|
for(unsigned int i=0; i<msg->actuators.size(); ++i)
|
||||||
image = cvCloneImage(&img);
|
{
|
||||||
|
actuators.push_back(Actuator(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type));
|
||||||
}
|
}
|
||||||
|
|
||||||
for(unsigned int i=0; i<msg->keypoints.size() && i<msg->keypoints.size(); i++)
|
if(!sensors.size() && !actuators.size())
|
||||||
{
|
{
|
||||||
keypoints[i].angle = msg->keypoints.at(i).angle;
|
ROS_ERROR("Sensorimotor received is empty...");
|
||||||
keypoints[i].response = msg->keypoints.at(i).response;
|
}
|
||||||
keypoints[i].pt.x = msg->keypoints.at(i).ptx;
|
else
|
||||||
keypoints[i].pt.y = msg->keypoints.at(i).pty;
|
{
|
||||||
keypoints[i].size = msg->keypoints.at(i).size;
|
ROS_INFO("Received sensorimotor (%d sensors %d actuators).", sensors.size(), actuators.size());
|
||||||
|
UEventsManager::post(new SensorimotorEvent(sensors, actuators));
|
||||||
}
|
}
|
||||||
rtabmap::SMState * smState = new rtabmap::SMState(msg->sensors, msg->sensorStep, msg->actuators, msg->actuatorStep);
|
|
||||||
smState->setImage(image);
|
|
||||||
smState->setKeypoints(keypoints);
|
|
||||||
UEventsManager::post(new SMStateEvent(smState));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||||
{
|
{
|
||||||
if(msg->data.size())
|
if(msg->data.size())
|
||||||
{
|
{
|
||||||
boost::shared_ptr<sensor_msgs::Image> tracked_object;
|
ROS_INFO("Received image.");
|
||||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
|
||||||
IplImage imgTmp = ptr->image;
|
UEventsManager::post(new CameraEvent(ptr->image.clone()));
|
||||||
IplImage * image = &imgTmp;
|
|
||||||
rtabmap::SMState * smState = new rtabmap::SMState(cvCloneImage(image));
|
|
||||||
UEventsManager::post(new SMStateEvent(smState));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -194,140 +195,106 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
|
|||||||
{
|
{
|
||||||
if(anEvent->getClassName().compare("RtabmapEvent") == 0)
|
if(anEvent->getClassName().compare("RtabmapEvent") == 0)
|
||||||
{
|
{
|
||||||
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
|
if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
|
||||||
const Statistics & stat = rtabmapEvent->getStats();
|
|
||||||
|
|
||||||
//prepare ros message
|
|
||||||
|
|
||||||
if(stat.extended())
|
|
||||||
{
|
{
|
||||||
|
ROS_INFO("Sending RtabmapInfo msg...");
|
||||||
|
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
|
||||||
|
const Statistics & stat = rtabmapEvent->getStats();
|
||||||
|
|
||||||
rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
|
rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
|
||||||
|
|
||||||
|
// General info
|
||||||
msg->refId = stat.refImageId();
|
msg->refId = stat.refImageId();
|
||||||
if(stat.refImage())
|
|
||||||
{
|
|
||||||
int params[3] = {0};
|
|
||||||
|
|
||||||
//JPEG compression
|
|
||||||
std::string format = "jpeg";
|
|
||||||
params[0] = CV_IMWRITE_JPEG_QUALITY;
|
|
||||||
params[1] = 80; // default: 80% quality
|
|
||||||
|
|
||||||
//PNG compression
|
|
||||||
//std::string format = "png";
|
|
||||||
//params[0] = CV_IMWRITE_PNG_COMPRESSION;
|
|
||||||
//params[1] = 9; // default: maximum compression
|
|
||||||
|
|
||||||
std::string extension = '.' + format;
|
|
||||||
|
|
||||||
// Compress image
|
|
||||||
const IplImage* image = stat.refImage();
|
|
||||||
CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
|
|
||||||
|
|
||||||
// Set up message and publish
|
|
||||||
sensor_msgs::CompressedImage compressed;
|
|
||||||
compressed.format = format;
|
|
||||||
compressed.data.resize(buf->width);
|
|
||||||
memcpy(&compressed.data[0], buf->data.ptr, buf->width);
|
|
||||||
cvReleaseMat(&buf);
|
|
||||||
|
|
||||||
msg->infoEx.refImage = compressed;
|
|
||||||
}
|
|
||||||
msg->loopClosureId = stat.loopClosureId();
|
msg->loopClosureId = stat.loopClosureId();
|
||||||
if(stat.loopClosureImage())
|
|
||||||
|
msg->actuators.resize(stat.getActuators().size());
|
||||||
|
int i=0;
|
||||||
|
for(std::list<Actuator>::const_iterator iter = stat.getActuators().begin(); iter!=stat.getActuators().end(); ++iter)
|
||||||
{
|
{
|
||||||
int params[3] = {0};
|
msg->actuators[i].type = iter->type();
|
||||||
|
fromCvMatToCvMatMsg(msg->actuators[i++].matrix, iter->data());
|
||||||
//JPEG compression
|
|
||||||
std::string format = "jpeg";
|
|
||||||
params[0] = CV_IMWRITE_JPEG_QUALITY;
|
|
||||||
params[1] = 80; // default: 80% quality
|
|
||||||
|
|
||||||
//PNG compression
|
|
||||||
//std::string format = "png";
|
|
||||||
//params[0] = CV_IMWRITE_PNG_COMPRESSION;
|
|
||||||
//params[1] = 9; // default: maximum compression
|
|
||||||
|
|
||||||
std::string extension = '.' + format;
|
|
||||||
|
|
||||||
// Compress image
|
|
||||||
const IplImage* image = stat.loopClosureImage();
|
|
||||||
CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
|
|
||||||
|
|
||||||
// Set up message and publish
|
|
||||||
sensor_msgs::CompressedImage compressed;
|
|
||||||
compressed.format = format;
|
|
||||||
compressed.data.resize(buf->width);
|
|
||||||
memcpy(&compressed.data[0], buf->data.ptr, buf->width);
|
|
||||||
cvReleaseMat(&buf);
|
|
||||||
|
|
||||||
msg->infoEx.loopClosureImage = compressed;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const std::list<std::vector<float> > & actuators = stat.getActions();
|
if(infoPub_.getNumSubscribers())
|
||||||
if(actuators.size())
|
|
||||||
{
|
{
|
||||||
msg->actuatorStep = actuators.front().size();
|
infoPub_.publish(msg);
|
||||||
}
|
}
|
||||||
for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
|
|
||||||
|
if(infoPubEx_.getNumSubscribers())
|
||||||
{
|
{
|
||||||
if((iter->size() == 0 && msg->actuatorStep > 0) || msg->actuatorStep % iter->size() != 0)
|
// Detailed info
|
||||||
|
if(stat.extended())
|
||||||
{
|
{
|
||||||
ROS_ERROR("Actuators must have all the same length.");
|
if(stat.refRawData().size())
|
||||||
|
{
|
||||||
|
msg->infoEx.refRawData.resize(stat.refRawData().size());
|
||||||
|
i=0;
|
||||||
|
for(std::list<Sensor>::const_iterator iter = stat.refRawData().begin(); iter!=stat.refRawData().end(); ++iter)
|
||||||
|
{
|
||||||
|
msg->infoEx.refRawData[i].type = iter->type();
|
||||||
|
fromCvMatToCvMatMsg(msg->infoEx.refRawData[i++].matrix, iter->data());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(stat.loopClosureRawData().size())
|
||||||
|
{
|
||||||
|
msg->infoEx.loopRawData.resize(stat.loopClosureRawData().size());
|
||||||
|
i=0;
|
||||||
|
for(std::list<Sensor>::const_iterator iter = stat.loopClosureRawData().begin(); iter!=stat.loopClosureRawData().end(); ++iter)
|
||||||
|
{
|
||||||
|
msg->infoEx.loopRawData[i].type = iter->type();
|
||||||
|
fromCvMatToCvMatMsg(msg->infoEx.loopRawData[i++].matrix, iter->data());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//Posterior, likelihood, childCount
|
||||||
|
msg->infoEx.posteriorKeys = uKeys(stat.posterior());
|
||||||
|
msg->infoEx.posteriorValues = uValues(stat.posterior());
|
||||||
|
msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
|
||||||
|
msg->infoEx.likelihoodValues = uValues(stat.likelihood());
|
||||||
|
msg->infoEx.weightsKeys = uKeys(stat.weights());
|
||||||
|
msg->infoEx.weightsValues = uValues(stat.weights());
|
||||||
|
|
||||||
|
//Features stuff...
|
||||||
|
msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
|
||||||
|
msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
|
||||||
|
int index = 0;
|
||||||
|
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
|
||||||
|
i!=stat.refWords().end();
|
||||||
|
++i)
|
||||||
|
{
|
||||||
|
msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
|
||||||
|
msg->infoEx.refWordsValues.at(index).response = i->second.response;
|
||||||
|
msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
|
||||||
|
msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
|
||||||
|
msg->infoEx.refWordsValues.at(index).size = i->second.size;
|
||||||
|
msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
|
||||||
|
msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
|
||||||
|
++index;
|
||||||
|
}
|
||||||
|
|
||||||
|
msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
|
||||||
|
msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
|
||||||
|
index = 0;
|
||||||
|
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
|
||||||
|
i!=stat.loopWords().end();
|
||||||
|
++i)
|
||||||
|
{
|
||||||
|
msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
|
||||||
|
msg->infoEx.loopWordsValues.at(index).response = i->second.response;
|
||||||
|
msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
|
||||||
|
msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
|
||||||
|
msg->infoEx.loopWordsValues.at(index).size = i->second.size;
|
||||||
|
msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
|
||||||
|
msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
|
||||||
|
++index;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Statistics data
|
||||||
|
msg->infoEx.statsKeys = uKeys(stat.data());
|
||||||
|
msg->infoEx.statsValues = uValues(stat.data());
|
||||||
}
|
}
|
||||||
msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end());
|
infoPubEx_.publish(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
//Posterior, likelihood, childCount
|
|
||||||
msg->infoEx.posteriorKeys = uKeys(stat.posterior());
|
|
||||||
msg->infoEx.posteriorValues = uValues(stat.posterior());
|
|
||||||
msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
|
|
||||||
msg->infoEx.likelihoodValues = uValues(stat.likelihood());
|
|
||||||
msg->infoEx.weightsKeys = uKeys(stat.weights());
|
|
||||||
msg->infoEx.weightsValues = uValues(stat.weights());
|
|
||||||
|
|
||||||
//SURF stuff...
|
|
||||||
msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
|
|
||||||
msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
|
|
||||||
int index = 0;
|
|
||||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
|
|
||||||
i!=stat.refWords().end();
|
|
||||||
++i)
|
|
||||||
{
|
|
||||||
msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
|
|
||||||
msg->infoEx.refWordsValues.at(index).response = i->second.response;
|
|
||||||
msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
|
|
||||||
msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
|
|
||||||
msg->infoEx.refWordsValues.at(index).size = i->second.size;
|
|
||||||
msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
|
|
||||||
msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
|
|
||||||
++index;
|
|
||||||
}
|
|
||||||
msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
|
|
||||||
msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
|
|
||||||
index = 0;
|
|
||||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
|
|
||||||
i!=stat.loopWords().end();
|
|
||||||
++i)
|
|
||||||
{
|
|
||||||
msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
|
|
||||||
msg->infoEx.loopWordsValues.at(index).response = i->second.response;
|
|
||||||
msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
|
|
||||||
msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
|
|
||||||
msg->infoEx.loopWordsValues.at(index).size = i->second.size;
|
|
||||||
msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
|
|
||||||
msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
|
|
||||||
++index;
|
|
||||||
}
|
|
||||||
|
|
||||||
// SM masks
|
|
||||||
msg->infoEx.refMotionMask = stat.refMotionMask();
|
|
||||||
msg->infoEx.loopMotionMask = stat.loopMotionMask();
|
|
||||||
|
|
||||||
// Statistics data
|
|
||||||
msg->infoEx.statsKeys = uKeys(stat.data());
|
|
||||||
msg->infoEx.statsValues = uValues(stat.data());
|
|
||||||
|
|
||||||
infoPub_.publish(msg);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -16,8 +16,9 @@
|
|||||||
#include "utilite/UEventsHandler.h"
|
#include "utilite/UEventsHandler.h"
|
||||||
#include <rtabmap/core/RtabmapEvent.h>
|
#include <rtabmap/core/RtabmapEvent.h>
|
||||||
#include <sensor_msgs/Image.h>
|
#include <sensor_msgs/Image.h>
|
||||||
#include "rtabmap/SensoryMotorState.h"
|
#include <geometry_msgs/Twist.h>
|
||||||
#include <image_transport/image_transport.h>
|
#include <image_transport/image_transport.h>
|
||||||
|
#include "rtabmap/Sensorimotor.h"
|
||||||
|
|
||||||
namespace rtabmap
|
namespace rtabmap
|
||||||
{
|
{
|
||||||
@@ -33,8 +34,9 @@ public:
|
|||||||
void start();
|
void start();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg);
|
void sensorimotorReceivedCallback(const rtabmap::SensorimotorConstPtr & msg);
|
||||||
void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
|
void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
|
||||||
|
void twistCallback(const geometry_msgs::TwistConstPtr & msg);
|
||||||
void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
|
void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg);
|
||||||
|
|
||||||
bool resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
|
bool resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
|
||||||
@@ -49,10 +51,13 @@ private:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
rtabmap::Rtabmap * rtabmap_;
|
rtabmap::Rtabmap * rtabmap_;
|
||||||
ros::Subscriber smStateTopic_;
|
ros::Subscriber sensorimotorTopic_;
|
||||||
image_transport::Subscriber imageTopic_;
|
image_transport::Subscriber imageTopic_;
|
||||||
|
ros::Subscriber audioFrameFreqSqrdMagnTopic_;
|
||||||
|
ros::Subscriber twistTopic_;
|
||||||
ros::Subscriber parametersUpdatedTopic_;
|
ros::Subscriber parametersUpdatedTopic_;
|
||||||
ros::Publisher infoPub_;
|
ros::Publisher infoPub_;
|
||||||
|
ros::Publisher infoPubEx_;
|
||||||
ros::Publisher parametersLoadedPub_;
|
ros::Publisher parametersLoadedPub_;
|
||||||
std::string configFile_;
|
std::string configFile_;
|
||||||
|
|
||||||
|
|||||||
+54
-54
@@ -6,27 +6,33 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "GuiWrapper.h"
|
#include "GuiWrapper.h"
|
||||||
#include <rtabmap/gui/MainWindow.h>
|
#include "MsgConversion.h"
|
||||||
#include "PreferencesDialogROS.h"
|
|
||||||
#include <QtGui/QApplication>
|
#include <QtGui/QApplication>
|
||||||
#include <rtabmap/core/RtabmapEvent.h>
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
#include <cv_bridge/cv_bridge.h>
|
||||||
#include "utilite/UEventsManager.h"
|
#include <std_srvs/Empty.h>
|
||||||
#include "std_srvs/Empty.h"
|
#include <std_msgs/Empty.h>
|
||||||
#include "std_msgs/Empty.h"
|
|
||||||
|
#include <utilite/UEventsManager.h>
|
||||||
|
|
||||||
|
#include <opencv2/highgui/highgui.hpp>
|
||||||
|
|
||||||
|
#include <rtabmap/gui/MainWindow.h>
|
||||||
|
#include <rtabmap/core/RtabmapEvent.h>
|
||||||
#include <rtabmap/core/Parameters.h>
|
#include <rtabmap/core/Parameters.h>
|
||||||
#include <highgui.h>
|
#include <rtabmap/core/Camera.h>
|
||||||
#include <rtabmap/core/CameraEvent.h>
|
#include <rtabmap/core/Sensor.h>
|
||||||
|
#include <rtabmap/core/SensorimotorEvent.h>
|
||||||
|
|
||||||
|
#include "PreferencesDialogROS.h"
|
||||||
#include "rtabmap/ChangeCameraImgRate.h"
|
#include "rtabmap/ChangeCameraImgRate.h"
|
||||||
#include "rtabmap/core/SMState.h"
|
|
||||||
|
|
||||||
using namespace rtabmap;
|
using namespace rtabmap;
|
||||||
|
|
||||||
GuiWrapper::GuiWrapper(int & argc, char** argv) :
|
GuiWrapper::GuiWrapper(int & argc, char** argv)
|
||||||
nbCommands_(2)
|
|
||||||
{
|
{
|
||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
infoTopic_ = nh.subscribe("rtabmap/info", 1, &GuiWrapper::infoReceivedCallback, this);
|
infoTopic_ = nh.subscribe("rtabmap/infoEx", 1, &GuiWrapper::infoReceivedCallback, this);
|
||||||
velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
|
velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
|
||||||
app_ = new QApplication(argc, argv);
|
app_ = new QApplication(argc, argv);
|
||||||
mainWindow_ = new MainWindow(new PreferencesDialogROS());
|
mainWindow_ = new MainWindow(new PreferencesDialogROS());
|
||||||
@@ -42,8 +48,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
|
|||||||
|
|
||||||
nh = ros::NodeHandle("~");
|
nh = ros::NodeHandle("~");
|
||||||
parametersUpdatedPub_ = nh.advertise<std_msgs::Empty>("parameters_updated", 1);
|
parametersUpdatedPub_ = nh.advertise<std_msgs::Empty>("parameters_updated", 1);
|
||||||
nh.param("nb_commands", nbCommands_, nbCommands_);
|
|
||||||
ROS_INFO("nb_commands=%d", nbCommands_);
|
|
||||||
|
|
||||||
UEventsManager::addHandler(this);
|
UEventsManager::addHandler(this);
|
||||||
UEventsManager::addHandler(mainWindow_);
|
UEventsManager::addHandler(mainWindow_);
|
||||||
@@ -70,21 +74,28 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
|||||||
stat->setExtended(true); // Extended
|
stat->setExtended(true); // Extended
|
||||||
|
|
||||||
stat->setRefImageId(msg->refId);
|
stat->setRefImageId(msg->refId);
|
||||||
if(msg->infoEx.refImage.data.size() > 0)
|
std::list<Sensor> sensors;
|
||||||
|
for(unsigned int i=0; i<msg->infoEx.refRawData.size(); ++i)
|
||||||
{
|
{
|
||||||
// Decompress
|
Sensor s(fromCvMatMsgToCvMat(msg->infoEx.refRawData[i].matrix), (Sensor::Type)msg->infoEx.refRawData[i].type);
|
||||||
const CvMat compressed = cvMat(1, msg->infoEx.refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.refImage.data[0]));
|
if(s.data().total())
|
||||||
IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
|
{
|
||||||
stat->setRefImage(&decompressed);
|
sensors.push_back(s);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
stat->setRefRawData(sensors);
|
||||||
|
|
||||||
stat->setLoopClosureId(msg->loopClosureId);
|
stat->setLoopClosureId(msg->loopClosureId);
|
||||||
if(msg->infoEx.loopClosureImage.data.size() > 0)
|
sensors.clear();
|
||||||
|
for(unsigned int i=0; i<msg->infoEx.loopRawData.size(); ++i)
|
||||||
{
|
{
|
||||||
// Decompress
|
Sensor s(fromCvMatMsgToCvMat(msg->infoEx.loopRawData[i].matrix), (Sensor::Type)msg->infoEx.loopRawData[i].type);
|
||||||
const CvMat compressed = cvMat(1, msg->infoEx.loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.loopClosureImage.data[0]));
|
if(s.data().total())
|
||||||
IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
|
{
|
||||||
stat->setLoopClosureImage(&decompressed);
|
sensors.push_back(s);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
stat->setLoopClosureRawData(sensors);
|
||||||
|
|
||||||
//Posterior, likelihood, childCount
|
//Posterior, likelihood, childCount
|
||||||
std::map<int, float> mapIntFloat;
|
std::map<int, float> mapIntFloat;
|
||||||
@@ -108,12 +119,11 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
|||||||
|
|
||||||
//SURF stuff...
|
//SURF stuff...
|
||||||
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
|
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
|
||||||
for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); i++)
|
for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); ++i)
|
||||||
{
|
{
|
||||||
cv::KeyPoint pt;
|
cv::KeyPoint pt;
|
||||||
pt.angle = msg->infoEx.refWordsValues.at(i).angle;
|
pt.angle = msg->infoEx.refWordsValues.at(i).angle;
|
||||||
pt.response = msg->infoEx.refWordsValues.at(i).response;
|
pt.response = msg->infoEx.refWordsValues.at(i).response;
|
||||||
//pt.laplacian = msg->refWordsValues.at(i).laplacian;
|
|
||||||
pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx;
|
pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx;
|
||||||
pt.pt.y = msg->infoEx.refWordsValues.at(i).pty;
|
pt.pt.y = msg->infoEx.refWordsValues.at(i).pty;
|
||||||
pt.size = msg->infoEx.refWordsValues.at(i).size;
|
pt.size = msg->infoEx.refWordsValues.at(i).size;
|
||||||
@@ -121,12 +131,11 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
|||||||
}
|
}
|
||||||
stat->setRefWords(mapIntKeypoint);
|
stat->setRefWords(mapIntKeypoint);
|
||||||
mapIntKeypoint.clear();
|
mapIntKeypoint.clear();
|
||||||
for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); i++)
|
for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); ++i)
|
||||||
{
|
{
|
||||||
cv::KeyPoint pt;
|
cv::KeyPoint pt;
|
||||||
pt.angle = msg->infoEx.loopWordsValues.at(i).angle;
|
pt.angle = msg->infoEx.loopWordsValues.at(i).angle;
|
||||||
pt.response = msg->infoEx.loopWordsValues.at(i).response;
|
pt.response = msg->infoEx.loopWordsValues.at(i).response;
|
||||||
//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
|
|
||||||
pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx;
|
pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx;
|
||||||
pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty;
|
pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty;
|
||||||
pt.size = msg->infoEx.loopWordsValues.at(i).size;
|
pt.size = msg->infoEx.loopWordsValues.at(i).size;
|
||||||
@@ -134,22 +143,17 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
|||||||
}
|
}
|
||||||
stat->setLoopWords(mapIntKeypoint);
|
stat->setLoopWords(mapIntKeypoint);
|
||||||
|
|
||||||
//SM stuff
|
|
||||||
stat->setRefMotionMask(msg->infoEx.refMotionMask);
|
|
||||||
stat->setLoopMotionMask(msg->infoEx.loopMotionMask);
|
|
||||||
|
|
||||||
//Actions
|
//Actions
|
||||||
std::list<std::vector<float> > actions;
|
std::list<Actuator> actuators;
|
||||||
for(unsigned int i=0; i<msg->actuators.size(); i+=msg->actuatorStep)
|
for(unsigned int i=0; i<msg->actuators.size(); ++i)
|
||||||
{
|
{
|
||||||
std::vector<float> a(msg->actuatorStep);
|
Actuator a(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type);
|
||||||
for(unsigned int j=0; j<a.size(); ++j)
|
if(a.data().total())
|
||||||
{
|
{
|
||||||
a[j] = msg->actuators[i+j];
|
actuators.push_back(a);
|
||||||
}
|
}
|
||||||
actions.push_back(a);
|
|
||||||
}
|
}
|
||||||
stat->setActions(actions);
|
stat->setActuators(actuators);
|
||||||
|
|
||||||
// Statistics data
|
// Statistics data
|
||||||
for(unsigned int i=0; i<msg->infoEx.statsKeys.size() && i<msg->infoEx.statsValues.size(); i++)
|
for(unsigned int i=0; i<msg->infoEx.statsKeys.size() && i<msg->infoEx.statsValues.size(); i++)
|
||||||
@@ -161,23 +165,19 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
|||||||
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
|
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
|
||||||
}
|
}
|
||||||
|
|
||||||
void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
|
void GuiWrapper::velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg)
|
||||||
{
|
{
|
||||||
std::vector<float> v(6);
|
cv::Mat data = cv::Mat(1, 6, CV_32F);
|
||||||
v[0] = msg->linear.x;
|
data.at<float>(0) = (float)msg->twist.linear.x;
|
||||||
v[1] = msg->linear.y;
|
data.at<float>(1) = (float)msg->twist.linear.y;
|
||||||
v[2] = msg->linear.z;
|
data.at<float>(2) = (float)msg->twist.linear.z;
|
||||||
v[3] = msg->angular.x;
|
data.at<float>(3) = (float)msg->twist.angular.x;
|
||||||
v[4] = msg->angular.y;
|
data.at<float>(4) = (float)msg->twist.angular.y;
|
||||||
v[5] = msg->angular.z;
|
data.at<float>(5) = (float)msg->twist.angular.z;
|
||||||
|
|
||||||
commands_.push_back(v);
|
std::list<Actuator> actuators;
|
||||||
|
actuators.push_back(Actuator(data, rtabmap::Actuator::kTypeTwist));
|
||||||
if(commands_.size() == (unsigned int)nbCommands_)
|
this->post(new rtabmap::SensorimotorEvent(std::list<Sensor>(), actuators));
|
||||||
{
|
|
||||||
this->post(new SMStateEvent(new SMState(cv::Mat(), commands_)));
|
|
||||||
commands_.clear();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GuiWrapper::handleEvent(UEvent * anEvent)
|
void GuiWrapper::handleEvent(UEvent * anEvent)
|
||||||
|
|||||||
@@ -12,7 +12,7 @@
|
|||||||
#include "rtabmap/RtabmapInfo.h"
|
#include "rtabmap/RtabmapInfo.h"
|
||||||
#include "rtabmap/RtabmapInfoEx.h"
|
#include "rtabmap/RtabmapInfoEx.h"
|
||||||
#include "utilite/UEventsHandler.h"
|
#include "utilite/UEventsHandler.h"
|
||||||
#include <geometry_msgs/Twist.h>
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
|
|
||||||
namespace rtabmap
|
namespace rtabmap
|
||||||
{
|
{
|
||||||
@@ -34,7 +34,7 @@ protected:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
|
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
|
||||||
void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg);
|
void velocityReceivedCallback(const geometry_msgs::TwistStampedConstPtr & msg);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
ros::Subscriber infoTopic_;
|
ros::Subscriber infoTopic_;
|
||||||
@@ -49,9 +49,6 @@ private:
|
|||||||
ros::ServiceClient dumpPredictionClient_;
|
ros::ServiceClient dumpPredictionClient_;
|
||||||
|
|
||||||
ros::Publisher parametersUpdatedPub_;
|
ros::Publisher parametersUpdatedPub_;
|
||||||
|
|
||||||
int nbCommands_;
|
|
||||||
std::list<std::vector<float> > commands_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* GUIWRAPPER_H_ */
|
#endif /* GUIWRAPPER_H_ */
|
||||||
|
|||||||
@@ -0,0 +1,90 @@
|
|||||||
|
/*
|
||||||
|
* InputNode.cpp
|
||||||
|
*
|
||||||
|
* Created on: 2012-05-27
|
||||||
|
* Author: mathieu
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <message_filters/subscriber.h>
|
||||||
|
#include <message_filters/sync_policies/approximate_time.h>
|
||||||
|
#include <sensor_msgs/Image.h>
|
||||||
|
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
|
||||||
|
#include "rtabmap/Sensorimotor.h"
|
||||||
|
#include <opencv2/core/core.hpp>
|
||||||
|
#include <cv_bridge/cv_bridge.h>
|
||||||
|
#include "MsgConversion.h"
|
||||||
|
|
||||||
|
class ImageAudioInput
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ImageAudioInput(ros::NodeHandle n) :
|
||||||
|
n_(n),
|
||||||
|
image_sub(n_, "image", 1),
|
||||||
|
audio_sub(n_, "audioFrameFreqSqrdMagn", 1),
|
||||||
|
sync(MySyncPolicy(10), image_sub, audio_sub)
|
||||||
|
{
|
||||||
|
sync.registerCallback(boost::bind(&ImageAudioInput::imageAudioCallback, this, _1, _2));
|
||||||
|
sensorimotor_pub_ = n_.advertise<rtabmap::Sensorimotor>("sensorimotor",1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void imageAudioCallback(
|
||||||
|
const sensor_msgs::ImageConstPtr& image,
|
||||||
|
const rtabmap_audio::AudioFrameFreqSqrdMagnConstPtr & audio)
|
||||||
|
{
|
||||||
|
if(!image->data.size())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Image is empty...");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if(!audio->data.size())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Audio is empty...");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create a sensorimotor msg
|
||||||
|
rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor());
|
||||||
|
sm->header.stamp = ros::Time::now();
|
||||||
|
sm->sensors.resize(2);
|
||||||
|
|
||||||
|
//image
|
||||||
|
cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
|
||||||
|
sm->sensors[0].type = rtabmap::Sensor::kTypeImage;
|
||||||
|
fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
|
||||||
|
|
||||||
|
//audio
|
||||||
|
cv::Mat dataMat(audio->nChannels, audio->frameLength, CV_32F);
|
||||||
|
memcpy(dataMat.data, audio->data.data(), audio->data.size()*sizeof(float));
|
||||||
|
sm->sensors[1].type = rtabmap::Sensor::kTypeAudioFreqSqrdMagn;
|
||||||
|
fromCvMatToCvMatMsg(sm->sensors[1].matrix, dataMat);
|
||||||
|
|
||||||
|
sensorimotor_pub_.publish(sm);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
ros::NodeHandle n_;
|
||||||
|
|
||||||
|
//inputs
|
||||||
|
message_filters::Subscriber<sensor_msgs::Image> image_sub;
|
||||||
|
message_filters::Subscriber<rtabmap_audio::AudioFrameFreqSqrdMagn > audio_sub;
|
||||||
|
|
||||||
|
//synchronization stuff
|
||||||
|
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, rtabmap_audio::AudioFrameFreqSqrdMagn> MySyncPolicy;
|
||||||
|
message_filters::Synchronizer<MySyncPolicy> sync;
|
||||||
|
|
||||||
|
ros::Publisher sensorimotor_pub_;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "image_audio_input");
|
||||||
|
ros::NodeHandle n;
|
||||||
|
ImageAudioInput iai(n);
|
||||||
|
|
||||||
|
ros::spin();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,107 @@
|
|||||||
|
/*
|
||||||
|
* InputNode.cpp
|
||||||
|
*
|
||||||
|
* Created on: 2012-05-27
|
||||||
|
* Author: mathieu
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <message_filters/subscriber.h>
|
||||||
|
#include <message_filters/sync_policies/approximate_time.h>
|
||||||
|
#include <sensor_msgs/Image.h>
|
||||||
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
|
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
|
||||||
|
#include "rtabmap/Sensorimotor.h"
|
||||||
|
#include <opencv2/core/core.hpp>
|
||||||
|
#include <cv_bridge/cv_bridge.h>
|
||||||
|
#include "MsgConversion.h"
|
||||||
|
|
||||||
|
class ImageAudioTwistInput
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ImageAudioTwistInput(ros::NodeHandle n) :
|
||||||
|
n_(n),
|
||||||
|
image_sub(n_, "image", 1),
|
||||||
|
audio_sub(n_, "audioFrameFreqSqrdMagn", 1),
|
||||||
|
twist_sub(n_, "cmd_vel", 1),
|
||||||
|
sync(MySyncPolicy(10), image_sub, audio_sub, twist_sub)
|
||||||
|
{
|
||||||
|
sync.registerCallback(boost::bind(&ImageAudioTwistInput::callback, this, _1, _2, _3));
|
||||||
|
sensorimotor_pub_ = n_.advertise<rtabmap::Sensorimotor>("sensorimotor",1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void callback(
|
||||||
|
const sensor_msgs::ImageConstPtr& image,
|
||||||
|
const rtabmap_audio::AudioFrameFreqSqrdMagnConstPtr & audio,
|
||||||
|
const geometry_msgs::TwistStampedConstPtr & twist)
|
||||||
|
{
|
||||||
|
if(!image->data.size())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Image is empty...");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if(!audio->data.size())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Audio is empty...");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create a sensorimotor msg
|
||||||
|
cv::Mat data;
|
||||||
|
rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor());
|
||||||
|
sm->header.stamp = ros::Time::now();
|
||||||
|
sm->sensors.resize(2);
|
||||||
|
|
||||||
|
//image
|
||||||
|
cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
|
||||||
|
sm->sensors[0].type = rtabmap::Sensor::kTypeImage;
|
||||||
|
fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
|
||||||
|
|
||||||
|
//audio
|
||||||
|
data = cv::Mat(audio->nChannels, audio->frameLength, CV_32F);
|
||||||
|
memcpy(data.data, audio->data.data(), audio->data.size()*sizeof(float));
|
||||||
|
sm->sensors[1].type = rtabmap::Sensor::kTypeAudioFreqSqrdMagn;
|
||||||
|
fromCvMatToCvMatMsg(sm->sensors[1].matrix, data);
|
||||||
|
|
||||||
|
//twist
|
||||||
|
sm->actuators.resize(1);
|
||||||
|
data = cv::Mat(1, 6, CV_32F);
|
||||||
|
data.at<float>(0) = (float)twist->twist.linear.x;
|
||||||
|
data.at<float>(1) = (float)twist->twist.linear.y;
|
||||||
|
data.at<float>(2) = (float)twist->twist.linear.z;
|
||||||
|
data.at<float>(3) = (float)twist->twist.angular.x;
|
||||||
|
data.at<float>(4) = (float)twist->twist.angular.y;
|
||||||
|
data.at<float>(5) = (float)twist->twist.angular.z;
|
||||||
|
sm->actuators[0].type = rtabmap::Actuator::kTypeTwist;
|
||||||
|
fromCvMatToCvMatMsg(sm->actuators[0].matrix, data);
|
||||||
|
|
||||||
|
sensorimotor_pub_.publish(sm);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
ros::NodeHandle n_;
|
||||||
|
|
||||||
|
//inputs
|
||||||
|
message_filters::Subscriber<sensor_msgs::Image> image_sub;
|
||||||
|
message_filters::Subscriber<rtabmap_audio::AudioFrameFreqSqrdMagn> audio_sub;
|
||||||
|
message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub;
|
||||||
|
|
||||||
|
//synchronization stuff
|
||||||
|
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, rtabmap_audio::AudioFrameFreqSqrdMagn, geometry_msgs::TwistStamped> MySyncPolicy;
|
||||||
|
message_filters::Synchronizer<MySyncPolicy> sync;
|
||||||
|
|
||||||
|
ros::Publisher sensorimotor_pub_;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "image_audio_twist_input");
|
||||||
|
ros::NodeHandle n;
|
||||||
|
ImageAudioTwistInput iati(n);
|
||||||
|
|
||||||
|
ros::spin();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -1,188 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraNode.cpp
|
|
||||||
*
|
|
||||||
* Created on: 1 févr. 2010
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <sensor_msgs/Image.h>
|
|
||||||
#include <rtabmap/core/Camera.h>
|
|
||||||
#include <rtabmap/core/SMState.h>
|
|
||||||
#include <utilite/ULogger.h>
|
|
||||||
#include <utilite/UTimer.h>
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
#include <image_transport/image_transport.h>
|
|
||||||
#include <sensor_msgs/image_encodings.h>
|
|
||||||
#include "rtabmap/SensoryMotorState.h"
|
|
||||||
#include <std_msgs/Empty.h>
|
|
||||||
#include <opencv2/imgproc/imgproc_c.h>
|
|
||||||
|
|
||||||
rtabmap::CamKeypointTreatment kpThreatment;
|
|
||||||
ros::Publisher rosPublisher;
|
|
||||||
|
|
||||||
int imgWidth = 0;
|
|
||||||
int imgHeight = 0;
|
|
||||||
double imgRate = 0.0;
|
|
||||||
int keypointsExtracted = 0;
|
|
||||||
|
|
||||||
UTimer timer;
|
|
||||||
|
|
||||||
void updateParameters()
|
|
||||||
{
|
|
||||||
ros::NodeHandle nh;
|
|
||||||
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
|
|
||||||
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
|
|
||||||
{
|
|
||||||
std::string value;
|
|
||||||
if(nh.getParam(iter->first, value))
|
|
||||||
{
|
|
||||||
iter->second = value;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
ROS_INFO("Updating parameters");
|
|
||||||
kpThreatment.parseParameters(parameters);
|
|
||||||
}
|
|
||||||
|
|
||||||
void parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
|
|
||||||
{
|
|
||||||
updateParameters();
|
|
||||||
}
|
|
||||||
|
|
||||||
void parametersLoadedCallback(const std_msgs::EmptyConstPtr & msg)
|
|
||||||
{
|
|
||||||
updateParameters();
|
|
||||||
}
|
|
||||||
|
|
||||||
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & imgMsg)
|
|
||||||
{
|
|
||||||
double period = 0.0;
|
|
||||||
if(imgRate > 0)
|
|
||||||
{
|
|
||||||
period = 1.0/imgRate;
|
|
||||||
period -= 0.01 * period; // 1% error
|
|
||||||
}
|
|
||||||
double elapsed = timer.getElapsedTime();
|
|
||||||
if(imgRate == 0.0 || elapsed > period)
|
|
||||||
{
|
|
||||||
timer.start();
|
|
||||||
|
|
||||||
if(imgMsg->data.size())
|
|
||||||
{
|
|
||||||
boost::shared_ptr<sensor_msgs::Image> tracked_object;
|
|
||||||
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(imgMsg);
|
|
||||||
IplImage imgTmp = ptr->image;
|
|
||||||
IplImage * image = &imgTmp;
|
|
||||||
|
|
||||||
bool resized = false;
|
|
||||||
if(imgWidth &&
|
|
||||||
imgHeight &&
|
|
||||||
imgWidth != image->width &&
|
|
||||||
imgHeight != image->height)
|
|
||||||
{
|
|
||||||
// declare a destination IplImage object with correct size, depth and channels
|
|
||||||
IplImage * resampledImg = cvCreateImage( cvSize((int)(imgWidth) ,
|
|
||||||
(int)(imgHeight) ),
|
|
||||||
image->depth, image->nChannels );
|
|
||||||
|
|
||||||
//use cvResize to resize source to a destination image (linear interpolation)
|
|
||||||
cvResize(image, resampledImg);
|
|
||||||
image = resampledImg;
|
|
||||||
resized = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
if(image)
|
|
||||||
{
|
|
||||||
rtabmap::SMState * smState = new rtabmap::SMState(image);
|
|
||||||
if(keypointsExtracted)
|
|
||||||
{
|
|
||||||
kpThreatment.process(smState);
|
|
||||||
}
|
|
||||||
if(smState)
|
|
||||||
{
|
|
||||||
rtabmap::SensoryMotorStatePtr msg(new rtabmap::SensoryMotorState);
|
|
||||||
|
|
||||||
std::vector<float> sensors;
|
|
||||||
int sensorStep = 0;
|
|
||||||
smState->getSensorsMerged(sensors, sensorStep);
|
|
||||||
msg->sensors = sensors;
|
|
||||||
msg->sensorStep = sensorStep;
|
|
||||||
|
|
||||||
std::vector<float> actuators;
|
|
||||||
int actuatorStep = 0;
|
|
||||||
smState->getActuatorsMerged(actuators, actuatorStep);
|
|
||||||
msg->actuators = actuators;
|
|
||||||
msg->actuatorStep = actuatorStep;
|
|
||||||
|
|
||||||
if(!resized)
|
|
||||||
{
|
|
||||||
msg->image = *imgMsg;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
cv_bridge::CvImage img;
|
|
||||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
|
||||||
img.image = image;
|
|
||||||
msg->image = *img.toImageMsg();
|
|
||||||
}
|
|
||||||
const std::vector<cv::KeyPoint> & keypoints = smState->getKeypoints();
|
|
||||||
msg->keypoints = std::vector<rtabmap::KeyPoint>(keypoints.size());
|
|
||||||
int i=0;
|
|
||||||
for(std::vector<cv::KeyPoint>::const_iterator iter = keypoints.begin(); iter!=keypoints.end(); ++iter)
|
|
||||||
{
|
|
||||||
msg->keypoints.at(i).angle = iter->angle;
|
|
||||||
msg->keypoints.at(i).octave = iter->octave;
|
|
||||||
msg->keypoints.at(i).ptx = iter->pt.x;
|
|
||||||
msg->keypoints.at(i).pty = iter->pt.y;
|
|
||||||
msg->keypoints.at(i).response = iter->response;
|
|
||||||
msg->keypoints.at(i).size = iter->size;
|
|
||||||
msg->keypoints.at(i).class_id = iter->class_id;
|
|
||||||
++i;
|
|
||||||
}
|
|
||||||
|
|
||||||
rosPublisher.publish(msg);
|
|
||||||
}
|
|
||||||
if(resized)
|
|
||||||
{
|
|
||||||
cvReleaseImage(&image);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
//ROS_INFO("Ignored frame...");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "image_to_sms");
|
|
||||||
//ULogger::setType(ULogger::kTypeConsole);
|
|
||||||
//ULogger::setLevel(ULogger::kDebug);
|
|
||||||
|
|
||||||
ros::NodeHandle np("~");
|
|
||||||
|
|
||||||
np.param("image_hz", imgRate, imgRate);
|
|
||||||
np.param("resize_image_width", imgWidth, imgWidth);
|
|
||||||
np.param("resize_image_height", imgHeight, imgHeight);
|
|
||||||
np.param("keypoints_extracted", keypointsExtracted, keypointsExtracted);
|
|
||||||
|
|
||||||
ROS_INFO("image_hz=%f\nresize_image_width=%d\nresize_image_height=%d\nkeypointsExtracted=%d", imgRate, imgWidth, imgHeight, keypointsExtracted);
|
|
||||||
|
|
||||||
ros::NodeHandle nh;
|
|
||||||
ros::Subscriber parametersUpdatedTopic = nh.subscribe("rtabmap_gui/parameters_updated", 1, parametersUpdatedCallback);
|
|
||||||
ros::Subscriber parametersLoadedTopic = nh.subscribe("rtabmap/parameters_loaded", 1, parametersLoadedCallback);
|
|
||||||
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
|
|
||||||
|
|
||||||
image_transport::ImageTransport it(nh);
|
|
||||||
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
|
||||||
|
|
||||||
updateParameters();
|
|
||||||
|
|
||||||
timer.start();
|
|
||||||
|
|
||||||
ros::spin();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,94 @@
|
|||||||
|
/*
|
||||||
|
* InputNode.cpp
|
||||||
|
*
|
||||||
|
* Created on: 2012-05-27
|
||||||
|
* Author: mathieu
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <message_filters/subscriber.h>
|
||||||
|
#include <message_filters/sync_policies/approximate_time.h>
|
||||||
|
#include <sensor_msgs/Image.h>
|
||||||
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
|
#include "rtabmap/Sensorimotor.h"
|
||||||
|
#include <opencv2/core/core.hpp>
|
||||||
|
#include <cv_bridge/cv_bridge.h>
|
||||||
|
#include "MsgConversion.h"
|
||||||
|
|
||||||
|
class ImageAudioTwistInput
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ImageAudioTwistInput(ros::NodeHandle n) :
|
||||||
|
n_(n),
|
||||||
|
image_sub(n_, "image", 1),
|
||||||
|
twist_sub(n_, "cmd_vel", 1),
|
||||||
|
sync(MySyncPolicy(10), image_sub, twist_sub)
|
||||||
|
{
|
||||||
|
sync.registerCallback(boost::bind(&ImageAudioTwistInput::callback, this, _1, _2));
|
||||||
|
sensorimotor_pub_ = n_.advertise<rtabmap::Sensorimotor>("sensorimotor",1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void callback(
|
||||||
|
const sensor_msgs::ImageConstPtr& image,
|
||||||
|
const geometry_msgs::TwistStampedConstPtr & twist)
|
||||||
|
{
|
||||||
|
if(!image->data.size())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Image is empty...");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create a sensorimotor msg
|
||||||
|
cv::Mat data;
|
||||||
|
rtabmap::SensorimotorPtr sm(new rtabmap::Sensorimotor());
|
||||||
|
sm->header.stamp = ros::Time::now();
|
||||||
|
sm->sensors.resize(2);
|
||||||
|
|
||||||
|
//image
|
||||||
|
cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
|
||||||
|
sm->sensors[0].type = rtabmap::Sensor::kTypeImage;
|
||||||
|
fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
|
||||||
|
|
||||||
|
//twist
|
||||||
|
sm->actuators.resize(1);
|
||||||
|
data = cv::Mat(1, 6, CV_32F);
|
||||||
|
data.at<float>(0) = (float)twist->twist.linear.x;
|
||||||
|
data.at<float>(1) = (float)twist->twist.linear.y;
|
||||||
|
data.at<float>(2) = (float)twist->twist.linear.z;
|
||||||
|
data.at<float>(3) = (float)twist->twist.angular.x;
|
||||||
|
data.at<float>(4) = (float)twist->twist.angular.y;
|
||||||
|
data.at<float>(5) = (float)twist->twist.angular.z;
|
||||||
|
sm->actuators[0].type = rtabmap::Actuator::kTypeTwist;
|
||||||
|
fromCvMatToCvMatMsg(sm->actuators[0].matrix, data);
|
||||||
|
sm->sensors[1].type = rtabmap::Sensor::kTypeTwist;
|
||||||
|
sm->sensors[1].matrix = sm->actuators[0].matrix;
|
||||||
|
|
||||||
|
sensorimotor_pub_.publish(sm);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
ros::NodeHandle n_;
|
||||||
|
|
||||||
|
//inputs
|
||||||
|
message_filters::Subscriber<sensor_msgs::Image> image_sub;
|
||||||
|
message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub;
|
||||||
|
|
||||||
|
//synchronization stuff
|
||||||
|
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::TwistStamped> MySyncPolicy;
|
||||||
|
message_filters::Synchronizer<MySyncPolicy> sync;
|
||||||
|
|
||||||
|
ros::Publisher sensorimotor_pub_;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "image_twist_input");
|
||||||
|
ros::NodeHandle n;
|
||||||
|
ImageAudioTwistInput iati(n);
|
||||||
|
|
||||||
|
ros::spin();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -1,138 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraNode.cpp
|
|
||||||
*
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include "rtabmap/SensoryMotorState.h"
|
|
||||||
#include <geometry_msgs/Twist.h>
|
|
||||||
#include <utilite/UMutex.h>
|
|
||||||
#include <utilite/UTimer.h>
|
|
||||||
#include <utilite/ULogger.h>
|
|
||||||
|
|
||||||
UMutex commandMutex;
|
|
||||||
std::list<std::vector<float> > commands;
|
|
||||||
ros::Publisher rosPublisher;
|
|
||||||
UTimer timer;
|
|
||||||
rtabmap::SensoryMotorStatePtr state;
|
|
||||||
|
|
||||||
bool stateUpdated = false;
|
|
||||||
bool actionsUpdated = false;
|
|
||||||
int nbCommands = 10;
|
|
||||||
bool statsLogged = true;
|
|
||||||
const char * statsFileName = "InputStats.txt";
|
|
||||||
int index2 = 1;
|
|
||||||
|
|
||||||
void publish()
|
|
||||||
{
|
|
||||||
if(stateUpdated && actionsUpdated)
|
|
||||||
{
|
|
||||||
std::vector<float> actions;
|
|
||||||
int sizeActions = -1;
|
|
||||||
for(std::list<std::vector<float> >::iterator iter = commands.begin(); iter!=commands.end();++iter)
|
|
||||||
{
|
|
||||||
UINFO("%d %f %f %f", index2, iter->at(0), iter->at(1), iter->at(5));
|
|
||||||
actions.insert(actions.end(), iter->begin(), iter->end());
|
|
||||||
}
|
|
||||||
sizeActions = commands.size();
|
|
||||||
commands.clear();
|
|
||||||
|
|
||||||
state->actuators = actions;
|
|
||||||
|
|
||||||
rosPublisher.publish(state);
|
|
||||||
float elapsed = timer.ticks();
|
|
||||||
ROS_INFO("Sensorimotor state sent (sizeActions=%d, %f Hz)", sizeActions, elapsed>0?1/elapsed:0);
|
|
||||||
stateUpdated = false;
|
|
||||||
actionsUpdated = false;
|
|
||||||
++index2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void smReceivedCallback(const rtabmap::SensoryMotorStateConstPtr & msg)
|
|
||||||
{
|
|
||||||
state = rtabmap::SensoryMotorStatePtr(new rtabmap::SensoryMotorState);
|
|
||||||
state->sensors = msg->sensors;
|
|
||||||
state->sensorStep = msg->sensorStep;
|
|
||||||
state->actuatorStep = 6;
|
|
||||||
state->image = msg->image;
|
|
||||||
state->keypoints = msg->keypoints;
|
|
||||||
|
|
||||||
stateUpdated = true;
|
|
||||||
publish();
|
|
||||||
}
|
|
||||||
|
|
||||||
void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
|
|
||||||
{
|
|
||||||
ROS_INFO("Received command velocity linear=(%f,%f,%f) angular=(%f,%f,%f)",
|
|
||||||
msg->linear.x,
|
|
||||||
msg->linear.y,
|
|
||||||
msg->linear.z,
|
|
||||||
msg->angular.x,
|
|
||||||
msg->angular.y,
|
|
||||||
msg->angular.z);
|
|
||||||
|
|
||||||
std::vector<float> v(6);
|
|
||||||
v[0] = msg->linear.x;
|
|
||||||
v[1] = msg->linear.y;
|
|
||||||
v[2] = msg->linear.z;
|
|
||||||
v[3] = msg->angular.x;
|
|
||||||
v[4] = msg->angular.y;
|
|
||||||
v[5] = msg->angular.z;
|
|
||||||
|
|
||||||
commands.push_back(v);
|
|
||||||
|
|
||||||
// 10 Hz max
|
|
||||||
while(commands.size() > (unsigned int)nbCommands)
|
|
||||||
{
|
|
||||||
UINFO("Ignored %f %f %f", commands.front().at(0), commands.front().at(1), commands.front().at(5));
|
|
||||||
ROS_WARN("Too many commands (%zu) > %d, removing the oldest...", commands.size(), nbCommands);
|
|
||||||
//remove the oldest
|
|
||||||
commands.pop_front();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(commands.size() == (unsigned int)nbCommands)
|
|
||||||
{
|
|
||||||
actionsUpdated = true;
|
|
||||||
publish();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "rtabmap_in");
|
|
||||||
ros::NodeHandle nh("~");
|
|
||||||
|
|
||||||
nh.param("nb_commands", nbCommands, nbCommands);
|
|
||||||
ROS_INFO("nb_commands=%d", nbCommands);
|
|
||||||
|
|
||||||
if(statsLogged)
|
|
||||||
{
|
|
||||||
ULogger::setPrintWhere(false);
|
|
||||||
ULogger::setBuffered(true);
|
|
||||||
ULogger::setPrintLevel(false);
|
|
||||||
ULogger::setPrintTime(false);
|
|
||||||
ULogger::setType(ULogger::kTypeFile, statsFileName, false);
|
|
||||||
ROS_INFO("stats log file = \"%s\"", statsFileName);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ULogger::setLevel(ULogger::kError);
|
|
||||||
}
|
|
||||||
|
|
||||||
nh = ros::NodeHandle();
|
|
||||||
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
|
|
||||||
ros::Subscriber image_sub = nh.subscribe("sensor_data", 1, smReceivedCallback);
|
|
||||||
ros::Subscriber velocity_sub = nh.subscribe("cmd_vel", 1, velocityReceivedCallback);
|
|
||||||
|
|
||||||
timer.start();
|
|
||||||
|
|
||||||
ros::spin();
|
|
||||||
|
|
||||||
if(statsLogged)
|
|
||||||
{
|
|
||||||
ULogger::flush();
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,68 @@
|
|||||||
|
/*
|
||||||
|
* MsgConversion.h
|
||||||
|
*
|
||||||
|
* Created on: 2012-05-27
|
||||||
|
* Author: mathieu
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef MSGCONVERSION_H_
|
||||||
|
#define MSGCONVERSION_H_
|
||||||
|
|
||||||
|
#include <rtabmap/core/Sensor.h>
|
||||||
|
#include <rtabmap/core/Actuator.h>
|
||||||
|
#include "rtabmap/CvMatMsg.h"
|
||||||
|
#include "rtabmap/SensorMsg.h"
|
||||||
|
#include "rtabmap/ActuatorMsg.h"
|
||||||
|
#include <opencv2/highgui/highgui.hpp>
|
||||||
|
|
||||||
|
using namespace rtabmap;
|
||||||
|
|
||||||
|
cv::Mat fromCvMatMsgToCvMat(const CvMatMsg & matrixMsg)
|
||||||
|
{
|
||||||
|
cv::Mat data;
|
||||||
|
if(matrixMsg.compressed)
|
||||||
|
{
|
||||||
|
data = cv::imdecode(matrixMsg.data, -1);
|
||||||
|
if(data.cols != (int)matrixMsg.width || data.rows != (int)matrixMsg.height)
|
||||||
|
{
|
||||||
|
ROS_ERROR("Uncompressed size (%d/%d) is not %d/%d", data.cols, data.rows, matrixMsg.width, matrixMsg.height);
|
||||||
|
data = cv::Mat();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
data = cv::Mat(matrixMsg.height, matrixMsg.width, matrixMsg.dataType);
|
||||||
|
if(data.total() * data.elemSize() != matrixMsg.data.size())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Size attributes (total size=%d) is not equal to actual data size (%d)", data.total() * data.elemSize(), matrixMsg.data.size());
|
||||||
|
data = cv::Mat();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
memcpy(data.data, matrixMsg.data.data(), matrixMsg.data.size());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void fromCvMatToCvMatMsg(CvMatMsg & matrixMsg, const cv::Mat & matrix, bool compressImage = false)
|
||||||
|
{
|
||||||
|
matrixMsg.width = matrix.cols;
|
||||||
|
matrixMsg.height = matrix.rows;
|
||||||
|
matrixMsg.dataType = matrix.type();
|
||||||
|
if(compressImage)
|
||||||
|
{
|
||||||
|
// compress images
|
||||||
|
matrixMsg.compressed = true;
|
||||||
|
cv::imencode(".png", matrix, matrixMsg.data);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
matrixMsg.data.resize(matrix.total()*matrix.elemSize());
|
||||||
|
memcpy(matrixMsg.data.data(), matrix.data, matrixMsg.data.size());
|
||||||
|
matrixMsg.compressed = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* MSGCONVERSION_H_ */
|
||||||
+24
-23
@@ -5,6 +5,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
#include <rtabmap/core/Actuator.h>
|
||||||
#include "rtabmap/RtabmapInfo.h"
|
#include "rtabmap/RtabmapInfo.h"
|
||||||
#include "rtabmap/RtabmapInfoEx.h"
|
#include "rtabmap/RtabmapInfoEx.h"
|
||||||
#include <geometry_msgs/Twist.h>
|
#include <geometry_msgs/Twist.h>
|
||||||
@@ -14,32 +15,32 @@
|
|||||||
ros::Publisher rosPublisher;
|
ros::Publisher rosPublisher;
|
||||||
bool statsLogged = true;
|
bool statsLogged = true;
|
||||||
const char * statsFileName = "OuputStats.txt";
|
const char * statsFileName = "OuputStats.txt";
|
||||||
int index2 = 1;
|
|
||||||
|
|
||||||
void publishCommands(const std::vector<float> & commands, int commandSize)
|
|
||||||
{
|
|
||||||
if(commandSize && commandSize%6 == 0)
|
|
||||||
{
|
|
||||||
unsigned int commandIndex = 0;
|
|
||||||
while(commandIndex < commands.size())
|
|
||||||
{
|
|
||||||
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
|
|
||||||
vel->linear.x = commands[commandIndex++];
|
|
||||||
vel->linear.y = commands[commandIndex++];
|
|
||||||
vel->linear.z = commands[commandIndex++];
|
|
||||||
vel->angular.x = commands[commandIndex++];
|
|
||||||
vel->angular.y = commands[commandIndex++];
|
|
||||||
vel->angular.z = commands[commandIndex++];
|
|
||||||
UINFO("%d %f %f %f", index2, vel->linear.x, vel->linear.y, vel->angular.z);
|
|
||||||
rosPublisher.publish(vel);
|
|
||||||
}
|
|
||||||
++index2;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
||||||
{
|
{
|
||||||
publishCommands(msg->actuators, msg->actuatorStep);
|
for(unsigned int i=0; i<msg->actuators.size(); ++i)
|
||||||
|
{
|
||||||
|
if(msg->actuators[i].type == rtabmap::Actuator::kTypeTwist)
|
||||||
|
{
|
||||||
|
if((msg->actuators[i].matrix.dataType & CV_32F) && msg->actuators[i].matrix.data.size()/sizeof(float) == 6)
|
||||||
|
{
|
||||||
|
geometry_msgs::TwistPtr vel(new geometry_msgs::Twist());
|
||||||
|
float * twist = (float *)msg->actuators[i].matrix.data.data();
|
||||||
|
vel->linear.x = twist[0];
|
||||||
|
vel->linear.y = twist[1];
|
||||||
|
vel->linear.z = twist[2];
|
||||||
|
vel->angular.x = twist[3];
|
||||||
|
vel->angular.y = twist[4];
|
||||||
|
vel->angular.z = twist[5];
|
||||||
|
UINFO("%f %f %f", vel->linear.x, vel->linear.y, vel->angular.z);
|
||||||
|
rosPublisher.publish(vel);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_ERROR("Twist format is wrong...");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
int main(int argc, char** argv)
|
||||||
|
|||||||
@@ -32,7 +32,7 @@ void PreferencesDialogROS::readCameraSettings(const QString & filePath)
|
|||||||
double imgRate = 0;
|
double imgRate = 0;
|
||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
nh.getParam("camera/image_hz", imgRate);
|
nh.getParam("camera/image_hz", imgRate);
|
||||||
this->setImgRate(imgRate);
|
this->setInputRate(imgRate);
|
||||||
}
|
}
|
||||||
|
|
||||||
QString PreferencesDialogROS::getParamMessage()
|
QString PreferencesDialogROS::getParamMessage()
|
||||||
|
|||||||
@@ -44,12 +44,12 @@ void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
|
|||||||
}
|
}
|
||||||
msgLinear.header.frame_id = "/base_link";
|
msgLinear.header.frame_id = "/base_link";
|
||||||
msgAngular.header.frame_id = "/base_link";
|
msgAngular.header.frame_id = "/base_link";
|
||||||
|
msgLinear.header.stamp = ros::Time::now();
|
||||||
|
msgAngular.header.stamp = ros::Time::now();
|
||||||
rosPublisherLinear.publish(msgLinear);
|
rosPublisherLinear.publish(msgLinear);
|
||||||
rosPublisherAngular.publish(msgAngular);
|
rosPublisherAngular.publish(msgAngular);
|
||||||
}
|
}
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
int main(int argc, char * argv[])
|
int main(int argc, char * argv[])
|
||||||
{
|
{
|
||||||
ros::init(argc, argv, "twist_to_poses");
|
ros::init(argc, argv, "twist_to_poses");
|
||||||
@@ -57,7 +57,7 @@ int main(int argc, char * argv[])
|
|||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
rosPublisherLinear = nh.advertise<geometry_msgs::PoseStamped>("pose_linear", 1);
|
rosPublisherLinear = nh.advertise<geometry_msgs::PoseStamped>("pose_linear", 1);
|
||||||
rosPublisherAngular = nh.advertise<geometry_msgs::PoseStamped>("pose_angular", 1);
|
rosPublisherAngular = nh.advertise<geometry_msgs::PoseStamped>("pose_angular", 1);
|
||||||
ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
|
ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
|
||||||
|
|
||||||
ros::spin();
|
ros::spin();
|
||||||
|
|
||||||
@@ -0,0 +1,35 @@
|
|||||||
|
/*
|
||||||
|
* TwistToPoses.cpp
|
||||||
|
*
|
||||||
|
* Created on: 2011-11-30
|
||||||
|
* Author: matlab
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <geometry_msgs/Twist.h>
|
||||||
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
|
|
||||||
|
ros::Publisher pub;
|
||||||
|
|
||||||
|
void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
|
||||||
|
{
|
||||||
|
geometry_msgs::TwistStamped twistStamped;
|
||||||
|
|
||||||
|
twistStamped.twist = *msg;
|
||||||
|
twistStamped.header.frame_id = "/base_link";
|
||||||
|
twistStamped.header.stamp = ros::Time::now();
|
||||||
|
pub.publish(twistStamped);
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char * argv[])
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "twist_to_twist_stamped");
|
||||||
|
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
pub = nh.advertise<geometry_msgs::TwistStamped>("cmd_vel_stamped", 1);
|
||||||
|
ros::Subscriber twist_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
|
||||||
|
|
||||||
|
ros::spin();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -1,41 +0,0 @@
|
|||||||
/*
|
|
||||||
* CameraNode.cpp
|
|
||||||
*
|
|
||||||
* Created on: 1 févr. 2010
|
|
||||||
* Author: labm2414
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include "rtabmap/SensoryMotorState.h"
|
|
||||||
#include <geometry_msgs/Twist.h>
|
|
||||||
|
|
||||||
ros::Publisher rosPublisher;
|
|
||||||
|
|
||||||
void twistReceivedCallback(const geometry_msgs::TwistConstPtr & msg)
|
|
||||||
{
|
|
||||||
std::vector<float> v(6);
|
|
||||||
v[0] = msg->linear.x*100;
|
|
||||||
v[1] = msg->linear.y*100;
|
|
||||||
v[2] = msg->linear.z*100;
|
|
||||||
v[3] = msg->angular.x*100;
|
|
||||||
v[4] = msg->angular.y*100;
|
|
||||||
v[5] = msg->angular.z*100;
|
|
||||||
|
|
||||||
rtabmap::SensoryMotorStatePtr smMsg(new rtabmap::SensoryMotorState);
|
|
||||||
smMsg->sensors = v;
|
|
||||||
smMsg->sensorStep = v.size();
|
|
||||||
rosPublisher.publish(smMsg);
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "twist_to_sms");
|
|
||||||
|
|
||||||
ros::NodeHandle nh;
|
|
||||||
rosPublisher = nh.advertise<rtabmap::SensoryMotorState>("sm_state", 1);
|
|
||||||
ros::Subscriber image_sub = nh.subscribe("cmd_vel", 1, twistReceivedCallback);
|
|
||||||
|
|
||||||
ros::spin();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,507 @@
|
|||||||
|
/*
|
||||||
|
* VisualAttentionNode.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 <utilite/UPlot.h>
|
||||||
|
#include <QtGui/QApplication>
|
||||||
|
#include <QtGui/QSpinBox>
|
||||||
|
#include <QtGui/QCheckBox>
|
||||||
|
#include <QtGui/QVBoxLayout>
|
||||||
|
#include <opencv2/imgproc/imgproc_c.h>
|
||||||
|
|
||||||
|
image_transport::Publisher rosPublisherMotionGlobal;
|
||||||
|
image_transport::Publisher rosPublisherMotionLocal;
|
||||||
|
image_transport::Publisher rosPublisherLocal;
|
||||||
|
image_transport::Publisher rosPublisherMotionLocalPolar;
|
||||||
|
image_transport::Publisher rosPublisherMotionLocalPolarReconstructed;
|
||||||
|
image_transport::Publisher rosPublisherLocalPolar;
|
||||||
|
image_transport::Publisher rosPublisherLocalPolarReconstructed;
|
||||||
|
image_transport::Publisher rosPublisherWithRoi;
|
||||||
|
UPlotCurve * g_curveR = 0;
|
||||||
|
UPlotCurve * g_curveG = 0;
|
||||||
|
UPlotCurve * g_curveB = 0;
|
||||||
|
|
||||||
|
UPlotCurve * g_curveRRatio = 0;
|
||||||
|
UPlotCurve * g_curveGRatio = 0;
|
||||||
|
UPlotCurve * g_curveBRatio = 0;
|
||||||
|
|
||||||
|
QSpinBox * spinX = 0;
|
||||||
|
QSpinBox * spinY = 0;
|
||||||
|
QCheckBox * polarCheckBox = 0;
|
||||||
|
|
||||||
|
#define ROI_RATIO 4 // default 8
|
||||||
|
#define POLAR_RAYS 128
|
||||||
|
#define POLAR_RINGS 64
|
||||||
|
cv::Mat previousGlobalImage;
|
||||||
|
cv::Mat previousPolarROI;
|
||||||
|
cv::Rect roi;
|
||||||
|
float ratio = 0.2;
|
||||||
|
bool attentionDisabled = false; // may set ROI_RATIO=1 if true
|
||||||
|
bool localAttention = false;
|
||||||
|
|
||||||
|
void imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||||
|
{
|
||||||
|
if(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(previousGlobalImage.cols == ptr->image.cols && previousGlobalImage.rows == ptr->image.rows)
|
||||||
|
{
|
||||||
|
unsigned char * imageData = (unsigned char *)motion.data;
|
||||||
|
unsigned char * previous_imageData = (unsigned char *)previousGlobalImage.data;
|
||||||
|
int widthStep = motion.cols * motion.elemSize();
|
||||||
|
cv::Point2i centerROILocal(roi.width/2, roi.height/2);
|
||||||
|
cv::Point2i centerROIGlobal(roi.x+roi.width/2, roi.y+roi.height/2);
|
||||||
|
cv::Point2i nearestMovingPixel;
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Motion ROI
|
||||||
|
cv::Mat motionROI = cv::Mat(motion, roi);
|
||||||
|
if(rosPublisherMotionLocal.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = motionROI;
|
||||||
|
rosPublisherMotionLocal.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
/*{
|
||||||
|
int radius = motionROI.cols/2;
|
||||||
|
float M = 64/std::log(radius);
|
||||||
|
cv::Mat tmpPolar(128, 64, CV_8UC3);
|
||||||
|
IplImage iplTmpPolar = tmpPolar;
|
||||||
|
IplImage iplPolar = motionROI;
|
||||||
|
cvLogPolar( &iplPolar, &iplTmpPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
|
||||||
|
cvLogPolar( &iplTmpPolar, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
|
||||||
|
|
||||||
|
if(rosPublisherMotionLocalPolar.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = motionROI;
|
||||||
|
rosPublisherMotionLocalPolar.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
}*/
|
||||||
|
|
||||||
|
|
||||||
|
// Motion ROI polar
|
||||||
|
{
|
||||||
|
cv::Mat imageROI = cv::Mat(ptr->image, roi);
|
||||||
|
int radius = imageROI.cols/2;
|
||||||
|
float M = POLAR_RINGS/std::log(radius);
|
||||||
|
cv::Mat polarROI(POLAR_RAYS, POLAR_RINGS, CV_8UC3);
|
||||||
|
IplImage iplPolar = polarROI;
|
||||||
|
IplImage iplImageROI = imageROI;
|
||||||
|
cvLogPolar( &iplImageROI, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
|
||||||
|
cv::Mat motionPolarROI = polarROI.clone();
|
||||||
|
if(previousPolarROI.cols == motionPolarROI.cols && previousPolarROI.rows == motionPolarROI.rows)
|
||||||
|
{
|
||||||
|
unsigned char * motionPolarData = motionPolarROI.data;
|
||||||
|
unsigned char * previousPolarData = previousPolarROI.data;
|
||||||
|
widthStep = motionPolarROI.cols * motionPolarROI.elemSize();
|
||||||
|
for(int j=0; j<motionPolarROI.rows; ++j)
|
||||||
|
{
|
||||||
|
for(int i=0; i<motionPolarROI.cols; ++i)
|
||||||
|
{
|
||||||
|
float b = (float)motionPolarData[j*widthStep+i*3+0];
|
||||||
|
float g = (float)motionPolarData[j*widthStep+i*3+1];
|
||||||
|
float r = (float)motionPolarData[j*widthStep+i*3+2];
|
||||||
|
float previous_b = (float)previousPolarData[j*widthStep+i*3+0];
|
||||||
|
float previous_g = (float)previousPolarData[j*widthStep+i*3+1];
|
||||||
|
float previous_r = (float)previousPolarData[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))
|
||||||
|
{
|
||||||
|
motionPolarData[j*widthStep+i*3+0] = 0;
|
||||||
|
motionPolarData[j*widthStep+i*3+1] = 0;
|
||||||
|
motionPolarData[j*widthStep+i*3+2] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(rosPublisherLocalPolar.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = polarROI;
|
||||||
|
rosPublisherLocalPolar.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
if(rosPublisherLocalPolarReconstructed.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv::Mat reconstructedROI(imageROI.rows, imageROI.cols, imageROI.type());
|
||||||
|
IplImage iplPolarROI = polarROI;
|
||||||
|
IplImage iplReconstructedROI = reconstructedROI;
|
||||||
|
cvLogPolar( &iplPolarROI, &iplReconstructedROI, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
|
||||||
|
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = reconstructedROI;
|
||||||
|
rosPublisherLocalPolarReconstructed.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
if(rosPublisherMotionLocalPolar.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = motionPolarROI;
|
||||||
|
rosPublisherMotionLocalPolar.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
//reconstruct image
|
||||||
|
cv::Mat reconstructedROI(imageROI.rows, imageROI.cols, imageROI.type());
|
||||||
|
IplImage iplMotionPolarROI = motionPolarROI;
|
||||||
|
IplImage iplReconstructedROI = reconstructedROI;
|
||||||
|
cvLogPolar( &iplMotionPolarROI, &iplReconstructedROI, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
|
||||||
|
|
||||||
|
if(rosPublisherMotionLocalPolarReconstructed.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = reconstructedROI;
|
||||||
|
rosPublisherMotionLocalPolarReconstructed.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
if(localAttention)
|
||||||
|
{
|
||||||
|
float distNearest = 0.0f;
|
||||||
|
unsigned char * reconstructedData = reconstructedROI.data;
|
||||||
|
widthStep = reconstructedROI.cols * reconstructedROI.elemSize();
|
||||||
|
for(int j=0; j<reconstructedROI.rows; ++j)
|
||||||
|
{
|
||||||
|
for(int i=0; i<reconstructedROI.cols; ++i)
|
||||||
|
{
|
||||||
|
if(reconstructedData[j*widthStep+i*3+0] ||
|
||||||
|
reconstructedData[j*widthStep+i*3+1] ||
|
||||||
|
reconstructedData[j*widthStep+i*3+2])
|
||||||
|
{
|
||||||
|
float dist = std::sqrt(float((i-centerROILocal.x)*(i-centerROILocal.x) + (j-centerROILocal.y)*(j-centerROILocal.y)));
|
||||||
|
if((nearestMovingPixel.x == 0 && nearestMovingPixel.y == 0) ||
|
||||||
|
dist < distNearest)
|
||||||
|
{
|
||||||
|
nearestMovingPixel.y = j;
|
||||||
|
nearestMovingPixel.x = i;
|
||||||
|
distNearest = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// PLOT
|
||||||
|
if(g_curveR && g_curveG && g_curveB)
|
||||||
|
{
|
||||||
|
cv::Mat ref = ptr->image;
|
||||||
|
if(polarCheckBox->isChecked())
|
||||||
|
{
|
||||||
|
ref = reconstructedROI;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(spinX->value()<0 || spinX->value() > ref.cols)
|
||||||
|
{
|
||||||
|
spinX->setValue(ref.cols/2);
|
||||||
|
}
|
||||||
|
if(spinY->value()<0 || spinY->value() > ref.rows)
|
||||||
|
{
|
||||||
|
spinY->setValue(ref.rows/2);
|
||||||
|
}
|
||||||
|
|
||||||
|
//take pixel (OpenCV is BGR)
|
||||||
|
float r = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 2);
|
||||||
|
float g = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 1);
|
||||||
|
float b = (float)*(ref.data + (spinX->value())*ref.elemSize() + (spinY->value())*ref.elemSize()*ref.cols + 0);
|
||||||
|
if(g_curveR->itemsSize())
|
||||||
|
{
|
||||||
|
float lastR = g_curveR->getItemData(g_curveR->itemsSize()-1).y();
|
||||||
|
float lastG = g_curveG->getItemData(g_curveG->itemsSize()-1).y();
|
||||||
|
float lastB = g_curveB->getItemData(g_curveB->itemsSize()-1).y();
|
||||||
|
QMetaObject::invokeMethod(g_curveRRatio, "addValue", Q_ARG(float, fabs(r-lastR)/255.0f) );
|
||||||
|
QMetaObject::invokeMethod(g_curveGRatio, "addValue", Q_ARG(float, fabs(g-lastG)/255.0f) );
|
||||||
|
QMetaObject::invokeMethod(g_curveBRatio, "addValue", Q_ARG(float, fabs(b-lastB)/255.0f) );
|
||||||
|
}
|
||||||
|
|
||||||
|
QMetaObject::invokeMethod(g_curveR, "addValue", Q_ARG(float, (float)r));
|
||||||
|
QMetaObject::invokeMethod(g_curveG, "addValue", Q_ARG(float, (float)g));
|
||||||
|
QMetaObject::invokeMethod(g_curveB, "addValue", Q_ARG(float, (float)b));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!localAttention)
|
||||||
|
{
|
||||||
|
//global, may be outside of the ROI
|
||||||
|
float distNearest = 0.0f;
|
||||||
|
unsigned char * data = motion.data;
|
||||||
|
widthStep = motion.cols * motion.elemSize();
|
||||||
|
for(int j=0; j<motion.rows; ++j)
|
||||||
|
{
|
||||||
|
for(int i=0; i<motion.cols; ++i)
|
||||||
|
{
|
||||||
|
if(data[j*widthStep+i*3+0] ||
|
||||||
|
data[j*widthStep+i*3+1] ||
|
||||||
|
data[j*widthStep+i*3+2])
|
||||||
|
{
|
||||||
|
float dist = std::sqrt(float((i-centerROIGlobal.x)*(i-centerROIGlobal.x) + (j-centerROIGlobal.y)*(j-centerROIGlobal.y)));
|
||||||
|
if((nearestMovingPixel.x == 0 && nearestMovingPixel.y == 0) ||
|
||||||
|
dist < distNearest)
|
||||||
|
{
|
||||||
|
nearestMovingPixel.y = j;
|
||||||
|
nearestMovingPixel.x = i;
|
||||||
|
distNearest = dist;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!attentionDisabled && nearestMovingPixel.x && nearestMovingPixel.y)
|
||||||
|
{
|
||||||
|
if(localAttention)
|
||||||
|
{
|
||||||
|
ROS_INFO("nearestMovingPixel center(local:global)=(%d,%d:%d,%d) nearest(local:global)=(%d,%d:%d,%d)",
|
||||||
|
centerROILocal.x,
|
||||||
|
centerROILocal.y,
|
||||||
|
centerROILocal.x+roi.x,
|
||||||
|
centerROILocal.y+roi.y,
|
||||||
|
nearestMovingPixel.x,
|
||||||
|
nearestMovingPixel.y,
|
||||||
|
nearestMovingPixel.x+roi.x,
|
||||||
|
nearestMovingPixel.y+roi.y);
|
||||||
|
nearestMovingPixel.x += roi.x;
|
||||||
|
nearestMovingPixel.y += roi.y;
|
||||||
|
if( nearestMovingPixel.x > roi.x &&
|
||||||
|
nearestMovingPixel.x<roi.x+roi.width &&
|
||||||
|
nearestMovingPixel.x>roi.width/2 &&
|
||||||
|
nearestMovingPixel.x<ptr->image.cols - roi.width/2)
|
||||||
|
{
|
||||||
|
roi.x = nearestMovingPixel.x-roi.width/2;
|
||||||
|
}
|
||||||
|
if( nearestMovingPixel.y>roi.y &&
|
||||||
|
nearestMovingPixel.y<roi.y+roi.height &&
|
||||||
|
nearestMovingPixel.y>roi.height/2 &&
|
||||||
|
nearestMovingPixel.y<ptr->image.rows - roi.height/2)
|
||||||
|
{
|
||||||
|
roi.y = nearestMovingPixel.y-roi.height/2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_INFO("nearestMovingPixel center(global)=(%d,%d) nearest(global)=(%d,%d)",
|
||||||
|
centerROIGlobal.x,
|
||||||
|
centerROIGlobal.y,
|
||||||
|
nearestMovingPixel.x,
|
||||||
|
nearestMovingPixel.y);
|
||||||
|
if( nearestMovingPixel.x>roi.width/2 &&
|
||||||
|
nearestMovingPixel.x<ptr->image.cols - roi.width/2)
|
||||||
|
{
|
||||||
|
roi.x = nearestMovingPixel.x-roi.width/2;
|
||||||
|
}
|
||||||
|
else if(nearestMovingPixel.x>=ptr->image.cols - roi.width/2)
|
||||||
|
{
|
||||||
|
roi.x = ptr->image.cols - roi.width - 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
roi.x =0;
|
||||||
|
}
|
||||||
|
if( nearestMovingPixel.y>roi.height/2 &&
|
||||||
|
nearestMovingPixel.y<ptr->image.rows - roi.height/2)
|
||||||
|
{
|
||||||
|
roi.y = nearestMovingPixel.y-roi.height/2;
|
||||||
|
}
|
||||||
|
else if(nearestMovingPixel.y>=ptr->image.rows - roi.height/2)
|
||||||
|
{
|
||||||
|
roi.y = ptr->image.rows - roi.height - 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
roi.y = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cv::Mat newImageROI = cv::Mat(ptr->image, roi);
|
||||||
|
int radius = newImageROI.cols/2;
|
||||||
|
float M = POLAR_RINGS/std::log(radius);
|
||||||
|
previousPolarROI = cv::Mat(POLAR_RAYS, POLAR_RINGS, CV_8UC3);
|
||||||
|
IplImage iplPolar = previousPolarROI;
|
||||||
|
IplImage iplImageROI = newImageROI;
|
||||||
|
cvLogPolar( &iplImageROI, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
int size = ptr->image.cols < ptr->image.rows?ptr->image.cols/ROI_RATIO: ptr->image.rows/ROI_RATIO;
|
||||||
|
int x = size<ptr->image.cols?(ptr->image.cols-size)/2:0;
|
||||||
|
int y = size<ptr->image.rows?(ptr->image.rows-size)/2:0;
|
||||||
|
roi = cv::Rect(x, y, size, size);
|
||||||
|
}
|
||||||
|
previousGlobalImage = ptr->image.clone();
|
||||||
|
|
||||||
|
ROS_INFO("polar ROI (%d,%d,%d,%d)", roi.x, roi.y, roi.width, roi.height);
|
||||||
|
cv::Mat localImage = cv::Mat(ptr->image, roi).clone();
|
||||||
|
|
||||||
|
/*int radius = polarImage.cols/2;
|
||||||
|
float M = 64/std::log(radius);
|
||||||
|
ROS_INFO("src size=(%d,%d) radius=%d, M=%f", polarImage.cols, polarImage.rows, radius, M);
|
||||||
|
cv::Mat tmpPolar(128, 64, CV_8UC3);
|
||||||
|
IplImage iplTmpPolar = tmpPolar;
|
||||||
|
IplImage iplPolar = polarImage;
|
||||||
|
cvLogPolar( &iplPolar, &iplTmpPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
|
||||||
|
cvLogPolar( &iplTmpPolar, &iplPolar, cvPoint2D32f(radius, radius), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
|
||||||
|
*/
|
||||||
|
|
||||||
|
if(rosPublisherLocal.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = localImage;
|
||||||
|
rosPublisherLocal.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if(rosPublisherMotionGlobal.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
cv::rectangle(motion, cv::Point2f(roi.x, roi.y), cv::Point2f(roi.x+roi.width, roi.y+roi.height), cv::Scalar(0, 255, 0), 1);
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = motion;
|
||||||
|
rosPublisherMotionGlobal.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
if(rosPublisherWithRoi.getNumSubscribers())
|
||||||
|
{
|
||||||
|
cv::Mat imageWithRoi = ptr->image.clone();
|
||||||
|
cv::rectangle(imageWithRoi, cv::Point2f(roi.x, roi.y), cv::Point2f(roi.x+roi.width, roi.y+roi.height), cv::Scalar(0, 255, 0), 1);
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
img.header.frame_id = msg->header.frame_id;
|
||||||
|
img.header.stamp = msg->header.stamp;
|
||||||
|
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||||
|
img.image = imageWithRoi;
|
||||||
|
rosPublisherWithRoi.publish(img.toImageMsg());
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void my_handler(int s){
|
||||||
|
QApplication::closeAllWindows();
|
||||||
|
QApplication::exit();
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char * argv[])
|
||||||
|
{
|
||||||
|
ros::init(argc, argv, "visual_attention");
|
||||||
|
|
||||||
|
ros::NodeHandle n;
|
||||||
|
image_transport::ImageTransport it(n);
|
||||||
|
image_transport::Subscriber image_sub = it.subscribe("image", 1, imgReceivedCallback);
|
||||||
|
rosPublisherMotionGlobal = it.advertise("image_motion", 1);
|
||||||
|
rosPublisherMotionLocal = it.advertise("image_motion_local", 1);
|
||||||
|
rosPublisherLocal = it.advertise("image_local", 1);
|
||||||
|
rosPublisherMotionLocalPolar = it.advertise("image_motion_local_polar", 1);
|
||||||
|
rosPublisherMotionLocalPolarReconstructed = it.advertise("image_motion_local_polar_reconstructed", 1);
|
||||||
|
rosPublisherLocalPolar = it.advertise("image_local_polar", 1);
|
||||||
|
rosPublisherLocalPolarReconstructed = it.advertise("image_local_polar_reconstructed", 1);
|
||||||
|
rosPublisherWithRoi = it.advertise("image_with_roi", 1);
|
||||||
|
|
||||||
|
bool show_gui = true;
|
||||||
|
if(show_gui)
|
||||||
|
{
|
||||||
|
QApplication app(argc, argv);
|
||||||
|
|
||||||
|
QWidget widget;
|
||||||
|
widget.setLayout(new QVBoxLayout());
|
||||||
|
spinX = new QSpinBox(&widget);
|
||||||
|
spinY = new QSpinBox(&widget);
|
||||||
|
polarCheckBox = new QCheckBox("Polar", &widget);
|
||||||
|
polarCheckBox->setChecked(false);
|
||||||
|
QHBoxLayout * hLayout = new QHBoxLayout();
|
||||||
|
hLayout->addWidget(spinX);
|
||||||
|
hLayout->addWidget(spinY);
|
||||||
|
hLayout->addWidget(polarCheckBox);
|
||||||
|
widget.layout()->addItem(hLayout);
|
||||||
|
UPlot * plot = new UPlot(&widget);
|
||||||
|
plot->setWindowTitle("Pixel value");
|
||||||
|
plot->keepAllData(false);
|
||||||
|
plot->setMaxVisibleItems(50);
|
||||||
|
plot->setXLabel("Time (s)");
|
||||||
|
g_curveR = plot->addCurve("R", Qt::red);
|
||||||
|
g_curveG = plot->addCurve("G", Qt::green);
|
||||||
|
g_curveB = plot->addCurve("B", Qt::blue);
|
||||||
|
plot->setMinimumSize(600, 400);
|
||||||
|
widget.layout()->addWidget(plot);
|
||||||
|
widget.show();
|
||||||
|
|
||||||
|
UPlot plotRatio;
|
||||||
|
plotRatio.setWindowTitle("Pixel ratio");
|
||||||
|
plotRatio.keepAllData(false);
|
||||||
|
plotRatio.setMaxVisibleItems(50);
|
||||||
|
plotRatio.setXLabel("Time (s)");
|
||||||
|
g_curveRRatio = plotRatio.addCurve("R", Qt::red);
|
||||||
|
g_curveGRatio = plotRatio.addCurve("G", Qt::green);
|
||||||
|
g_curveBRatio = plotRatio.addCurve("B", Qt::blue);
|
||||||
|
plotRatio.setMinimumSize(600, 400);
|
||||||
|
plotRatio.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::AsyncSpinner spinner(4); // Use 4 threads
|
||||||
|
spinner.start();
|
||||||
|
app.exec();
|
||||||
|
spinner.stop();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ros::spin();
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -1,2 +0,0 @@
|
|||||||
float32 imgRate
|
|
||||||
---
|
|
||||||
@@ -0,0 +1,389 @@
|
|||||||
|
/*
|
||||||
|
* AudioPlayerNode.cpp
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <fmod.hpp>
|
||||||
|
#include <fmod_errors.h>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <utilite/ULogger.h>
|
||||||
|
#include <utilite/UMath.h>
|
||||||
|
#include <utilite/UThreadNode.h>
|
||||||
|
#include <utilite/UPlot.h>
|
||||||
|
#include <utilite/USpectrogram.h>
|
||||||
|
#include "rtabmap_audio/AudioFrame.h"
|
||||||
|
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
|
||||||
|
|
||||||
|
#include <QApplication>
|
||||||
|
#include <signal.h>
|
||||||
|
|
||||||
|
#define DEFAULT_GUI_USED true
|
||||||
|
#define PLOT_SAMPLING_RATIO 64
|
||||||
|
|
||||||
|
// Ring buffer
|
||||||
|
unsigned int g_readPtr = 0;
|
||||||
|
unsigned int g_writePtr = 0;
|
||||||
|
std::vector<char> g_ringBuffer; // interleaved audio
|
||||||
|
unsigned int g_readingLooped = 0;
|
||||||
|
unsigned int g_writingLooped = 0;
|
||||||
|
bool g_warn = true;
|
||||||
|
|
||||||
|
// Fmod stuff
|
||||||
|
FMOD::System *g_system = 0;
|
||||||
|
FMOD::Sound *g_sound = 0;
|
||||||
|
FMOD::Channel *g_channel = 0;
|
||||||
|
bool initialized = false;
|
||||||
|
|
||||||
|
// Display stuff
|
||||||
|
UPlot * g_plot = 0;
|
||||||
|
UPlotCurve * g_curveReceiving = 0;
|
||||||
|
UPlotCurve * g_curvePlaying = 0;
|
||||||
|
USpectrogram * g_spectrogram = 0;
|
||||||
|
|
||||||
|
void ERRCHECK(FMOD_RESULT result)
|
||||||
|
{
|
||||||
|
if (result != FMOD_OK)
|
||||||
|
{
|
||||||
|
ROS_ERROR("FMOD error! (%d) %s\n", result, FMOD_ErrorString(result));
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void updateCurve(UPlotCurve * curve, void * data, unsigned int dataSize, int channels, int sampleSize)
|
||||||
|
{
|
||||||
|
if(!curve || !data || dataSize == 0)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
//ROS_INFO("channels=%d, bitsPerSample=%d", channels, sampleSize);
|
||||||
|
int downSamplingFactor = PLOT_SAMPLING_RATIO*channels;
|
||||||
|
QVector<int> v(dataSize/downSamplingFactor/sampleSize);
|
||||||
|
//ROS_INFO("dataSize=%d downSamplingFactor = %d, v=%d", dataSize, downSamplingFactor, v.size());
|
||||||
|
if(sampleSize == 1)
|
||||||
|
{
|
||||||
|
char * p = (char*)data;
|
||||||
|
char min,max;
|
||||||
|
for(int i=0; i<v.size(); ++i)
|
||||||
|
{
|
||||||
|
uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
|
||||||
|
v[i] = abs(min) > abs(max) ? min : max;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(sampleSize == 2)
|
||||||
|
{
|
||||||
|
short * p = (short*)data;
|
||||||
|
short min,max;
|
||||||
|
for(int i=0; i<v.size(); ++i)
|
||||||
|
{
|
||||||
|
uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
|
||||||
|
v[i] = abs(min) > abs(max) ? min : max;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(sampleSize == 4)
|
||||||
|
{
|
||||||
|
int * p = (int*)data;
|
||||||
|
int min,max;
|
||||||
|
for(int i=0; i<v.size(); ++i)
|
||||||
|
{
|
||||||
|
uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
|
||||||
|
v[i] = abs(min) > abs(max) ? min : max;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
QMetaObject::invokeMethod(curve, "addValues", Q_ARG(QVector<int>, v));
|
||||||
|
}
|
||||||
|
|
||||||
|
FMOD_RESULT F_CALLBACK pcmreadcallback(FMOD_SOUND *sound, void *data, unsigned int datalen)
|
||||||
|
{
|
||||||
|
//ROS_INFO("datalen=%d, g_readPtr=%d", datalen, g_readPtr);
|
||||||
|
unsigned int count;
|
||||||
|
char * buffer = (char *)data;
|
||||||
|
|
||||||
|
bool okToCpy = false;
|
||||||
|
|
||||||
|
if(g_readPtr < g_writePtr)
|
||||||
|
{
|
||||||
|
okToCpy = true;
|
||||||
|
}
|
||||||
|
else if(g_readingLooped < g_writingLooped)
|
||||||
|
{
|
||||||
|
okToCpy = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(okToCpy)
|
||||||
|
{
|
||||||
|
g_warn = true;
|
||||||
|
unsigned int start = g_readPtr;
|
||||||
|
for(count=0; count<datalen; ++count)
|
||||||
|
{
|
||||||
|
*(buffer++) = g_ringBuffer[g_readPtr++];
|
||||||
|
g_readPtr %= g_ringBuffer.size();
|
||||||
|
}
|
||||||
|
if(g_readPtr < start)
|
||||||
|
{
|
||||||
|
++g_readingLooped;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(g_plot && g_plot->isVisible() && g_curvePlaying)
|
||||||
|
{
|
||||||
|
int channels, bitsPerSample;
|
||||||
|
FMOD_Sound_GetFormat(sound, 0, 0, &channels, &bitsPerSample);
|
||||||
|
updateCurve(g_curvePlaying, &g_ringBuffer[start], datalen, channels, bitsPerSample/8);
|
||||||
|
}
|
||||||
|
//ROS_INFO("datalen=%d, writePtr=%d, readPtr=%d", datalen, g_writePtr, g_readPtr);
|
||||||
|
}
|
||||||
|
else if(g_warn)
|
||||||
|
{
|
||||||
|
g_warn = false;
|
||||||
|
ROS_WARN("Empty buffer : stream down? (this warning is shown only one time)");
|
||||||
|
|
||||||
|
//fill data with zeros
|
||||||
|
for(unsigned int i=0; i<datalen; ++i)
|
||||||
|
{
|
||||||
|
*(buffer++) = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return FMOD_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
FMOD_RESULT F_CALLBACK pcmsetposcallback(FMOD_SOUND *sound, int subsound, unsigned int position, FMOD_TIMEUNIT postype)
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
This is useful if the user calls Sound::setPosition and you want to seek your data accordingly.
|
||||||
|
*/
|
||||||
|
return FMOD_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* decoderBufferSize = in bytes
|
||||||
|
*/
|
||||||
|
bool initAudioPlayer(unsigned int decoderBufferSize, int fs, int channels, int bytesPerSample)
|
||||||
|
{
|
||||||
|
ROS_INFO("Init player with fs=%d, channels=%d, bytesPerSample=%d", fs, channels, bytesPerSample);
|
||||||
|
UASSERT(bytesPerSample == 1 || bytesPerSample == 2 || bytesPerSample == 4);
|
||||||
|
FMOD_RESULT result;
|
||||||
|
FMOD_CREATESOUNDEXINFO createsoundexinfo;
|
||||||
|
unsigned int version;
|
||||||
|
FMOD_MODE mode = FMOD_2D | FMOD_OPENUSER | FMOD_LOOP_NORMAL | FMOD_HARDWARE | FMOD_CREATESTREAM;
|
||||||
|
/*
|
||||||
|
Create a System object and initialize.
|
||||||
|
*/
|
||||||
|
result = FMOD::System_Create(&g_system);
|
||||||
|
ERRCHECK(result);
|
||||||
|
|
||||||
|
result = g_system->getVersion(&version);
|
||||||
|
ERRCHECK(result);
|
||||||
|
|
||||||
|
if (version < FMOD_VERSION)
|
||||||
|
{
|
||||||
|
printf("Error! You are using an old version of FMOD %08x. This program requires %08x\n", version, FMOD_VERSION);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
result = g_system->init(32, FMOD_INIT_NORMAL, 0);
|
||||||
|
ERRCHECK(result);
|
||||||
|
|
||||||
|
memset(&createsoundexinfo, 0, sizeof(FMOD_CREATESOUNDEXINFO));
|
||||||
|
createsoundexinfo.cbsize = sizeof(FMOD_CREATESOUNDEXINFO); /* required. */
|
||||||
|
createsoundexinfo.length = decoderBufferSize; /* Length of PCM data in bytes of whole song (for Sound::getLength) */
|
||||||
|
createsoundexinfo.numchannels = channels; /* Number of channels in the sound. */
|
||||||
|
createsoundexinfo.defaultfrequency = fs; /* Default playback rate of sound. */
|
||||||
|
createsoundexinfo.decodebuffersize = (decoderBufferSize / bytesPerSample) / channels; /* Chunk size of stream update in samples. This will be the amount of data passed to the user callback. */
|
||||||
|
if(bytesPerSample == 1)
|
||||||
|
{
|
||||||
|
createsoundexinfo.format = FMOD_SOUND_FORMAT_PCM8; /* Data format of sound. */
|
||||||
|
}
|
||||||
|
else if(bytesPerSample == 2)
|
||||||
|
{
|
||||||
|
createsoundexinfo.format = FMOD_SOUND_FORMAT_PCM16; /* Data format of sound. */
|
||||||
|
}
|
||||||
|
else if(bytesPerSample == 4)
|
||||||
|
{
|
||||||
|
createsoundexinfo.format = FMOD_SOUND_FORMAT_PCM32; /* Data format of sound. */
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_ERROR("Sample size format (%d) must be 1, 2 or 4", bytesPerSample);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
createsoundexinfo.pcmreadcallback = pcmreadcallback; /* User callback for reading. */
|
||||||
|
createsoundexinfo.pcmsetposcallback = pcmsetposcallback; /* User callback for seeking. */
|
||||||
|
|
||||||
|
result = g_system->createSound(0, mode, &createsoundexinfo, &g_sound);
|
||||||
|
ERRCHECK(result);
|
||||||
|
|
||||||
|
/*
|
||||||
|
Play the sound.
|
||||||
|
*/
|
||||||
|
|
||||||
|
result = g_system->playSound(FMOD_CHANNEL_FREE, g_sound, 0, &g_channel);
|
||||||
|
ERRCHECK(result);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void frameReceivedCallback(const rtabmap_audio::AudioFramePtr & msg)
|
||||||
|
{
|
||||||
|
if(msg->data.size())
|
||||||
|
{
|
||||||
|
if(!g_ringBuffer.size())
|
||||||
|
{
|
||||||
|
g_ringBuffer = std::vector<char>(msg->data.size() * 5 * msg->nChannels * msg->sampleSize, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
//ROS_INFO("Received audio frame size=(%d), writePtr=%d", msg->data.size(), g_writePtr);
|
||||||
|
unsigned int start = g_writePtr;
|
||||||
|
if(g_writePtr + msg->data.size() <= g_ringBuffer.size())
|
||||||
|
{
|
||||||
|
memcpy(g_ringBuffer.data()+g_writePtr, msg->data.data(), msg->data.size());
|
||||||
|
g_writePtr += msg->data.size();
|
||||||
|
g_writePtr %= g_ringBuffer.size();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
unsigned int size2 = (g_writePtr + msg->data.size()) - g_ringBuffer.size();
|
||||||
|
unsigned int size1 = msg->data.size() - size2;
|
||||||
|
memcpy(g_ringBuffer.data()+g_writePtr, msg->data.data(), size1);
|
||||||
|
memcpy(g_ringBuffer.data(), msg->data.data() + size1, size2);
|
||||||
|
g_writePtr = size2;
|
||||||
|
}
|
||||||
|
if(g_writePtr < start)
|
||||||
|
{
|
||||||
|
++g_writingLooped;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!initialized)
|
||||||
|
{
|
||||||
|
//wait for second frame
|
||||||
|
if(g_writePtr > msg->data.size() * 2)
|
||||||
|
{
|
||||||
|
initialized = initAudioPlayer(msg->data.size(), msg->fs, msg->nChannels, msg->sampleSize);
|
||||||
|
if(!initialized)
|
||||||
|
{
|
||||||
|
ROS_ERROR("Cannot initialize the audio player...");
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(g_plot)
|
||||||
|
{
|
||||||
|
if(g_plot->isVisible() && ((msg->data.size() / msg->nChannels) / msg->sampleSize) % PLOT_SAMPLING_RATIO != 0)
|
||||||
|
{
|
||||||
|
g_plot->setVisible(false);
|
||||||
|
ROS_WARN("Frame length (%d) must be a multiple of %d to show audio plot...", ((msg->data.size() / msg->nChannels) / msg->sampleSize), PLOT_SAMPLING_RATIO);
|
||||||
|
}
|
||||||
|
else if(g_curveReceiving && g_curvePlaying)
|
||||||
|
{
|
||||||
|
QMetaObject::invokeMethod(g_curveReceiving, "setXIncrement", Q_ARG(float, float(PLOT_SAMPLING_RATIO)/float(msg->fs)));
|
||||||
|
QMetaObject::invokeMethod(g_curvePlaying, "setXIncrement", Q_ARG(float, float(PLOT_SAMPLING_RATIO)/float(msg->fs)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(g_plot && g_plot->isVisible() && g_curveReceiving)
|
||||||
|
{
|
||||||
|
updateCurve(g_curveReceiving, msg->data.data(), msg->data.size(), msg->nChannels, msg->sampleSize);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void frameFreqSqrdMagnReceivedCallback(const rtabmap_audio::AudioFrameFreqSqrdMagnPtr & msg)
|
||||||
|
{
|
||||||
|
if(msg->data.size() && msg->nChannels && g_spectrogram && g_spectrogram->isVisible())
|
||||||
|
{
|
||||||
|
QMetaObject::invokeMethod(g_spectrogram, "setSamplingRate", Q_ARG(int, msg->fs));
|
||||||
|
std::vector<float> data(msg->data.size()/msg->nChannels);
|
||||||
|
memcpy(data.data(), msg->data.data(), data.size()*sizeof(float)); // Just copy the first channel TODO support more channels...
|
||||||
|
QMetaObject::invokeMethod(g_spectrogram, "push", Q_ARG(std::vector<float>, data));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void my_handler(int s){
|
||||||
|
QApplication::closeAllWindows();
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
ULogger::setType(ULogger::kTypeConsole);
|
||||||
|
ULogger::setLevel(ULogger::kDebug);
|
||||||
|
ros::init(argc, argv, "audioPlayer");
|
||||||
|
|
||||||
|
ros::NodeHandle nh("~");
|
||||||
|
|
||||||
|
//Parameter
|
||||||
|
bool guiUsed = DEFAULT_GUI_USED;
|
||||||
|
nh.param("gui_used", guiUsed, guiUsed);
|
||||||
|
ROS_INFO("gui_used=%s", guiUsed?"true":"false");
|
||||||
|
|
||||||
|
nh = ros::NodeHandle("");
|
||||||
|
ros::Subscriber audioSubs = nh.subscribe("audioFrame", 1, frameReceivedCallback);
|
||||||
|
ros::Subscriber audioFreqSqrdMagnSubs;
|
||||||
|
|
||||||
|
if(guiUsed)
|
||||||
|
{
|
||||||
|
QApplication app(argc, argv);
|
||||||
|
|
||||||
|
qRegisterMetaType<QVector<int> >("QVector<int>");
|
||||||
|
qRegisterMetaType<std::vector<float> >("std::vector<float>");
|
||||||
|
|
||||||
|
g_plot = new UPlot();
|
||||||
|
g_plot->keepAllData(false);
|
||||||
|
g_plot->setMaxVisibleItems(1024);
|
||||||
|
g_plot->setXLabel("Time (s)");
|
||||||
|
g_curveReceiving = g_plot->addCurve("Receiving"); // debugging purpose, to see what is received
|
||||||
|
g_curvePlaying = g_plot->addCurve("Playing");
|
||||||
|
g_plot->setMinimumSize(600, 400);
|
||||||
|
|
||||||
|
g_spectrogram = new USpectrogram();
|
||||||
|
g_spectrogram->setMinimumSize(640, 480);
|
||||||
|
|
||||||
|
g_spectrogram->show();
|
||||||
|
g_plot->show();
|
||||||
|
|
||||||
|
audioFreqSqrdMagnSubs = nh.subscribe("audioFrameFreqSqrdMagn", 1, frameFreqSqrdMagnReceivedCallback);
|
||||||
|
|
||||||
|
// 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::AsyncSpinner spinner(4); // Use 4 threads
|
||||||
|
spinner.start();
|
||||||
|
app.exec();
|
||||||
|
spinner.stop();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ros::spin();
|
||||||
|
}
|
||||||
|
|
||||||
|
uSleep(100); // make sure all subscribers have terminated
|
||||||
|
/*
|
||||||
|
Shut down
|
||||||
|
*/
|
||||||
|
FMOD_RESULT result;
|
||||||
|
if(g_sound)
|
||||||
|
{
|
||||||
|
result = g_sound->release();
|
||||||
|
ERRCHECK(result);
|
||||||
|
}
|
||||||
|
if(g_system)
|
||||||
|
{
|
||||||
|
result = g_system->close();
|
||||||
|
ERRCHECK(result);
|
||||||
|
result = g_system->release();
|
||||||
|
ERRCHECK(result);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(g_plot)
|
||||||
|
{
|
||||||
|
delete g_plot;
|
||||||
|
}
|
||||||
|
if(g_spectrogram)
|
||||||
|
{
|
||||||
|
delete g_spectrogram;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,272 @@
|
|||||||
|
/*
|
||||||
|
* AudioRecorderNode.cpp
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
#include <utilite/ULogger.h>
|
||||||
|
#include <utilite/UFile.h>
|
||||||
|
#include <utilite/UEventsHandler.h>
|
||||||
|
#include <utilite/UEventsManager.h>
|
||||||
|
#include <utilite/UEvent.h>
|
||||||
|
#include <utilite/UThreadNode.h>
|
||||||
|
|
||||||
|
#include <std_msgs/Empty.h>
|
||||||
|
|
||||||
|
#include <rtabmap/core/Micro.h>
|
||||||
|
|
||||||
|
#include "rtabmap_audio/AudioFrame.h"
|
||||||
|
#include "rtabmap_audio/AudioFrameFreq.h"
|
||||||
|
#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
|
||||||
|
|
||||||
|
#define DEFAULT_DEVICE_ID 0
|
||||||
|
#define DEFAULT_FILE_NAME ""
|
||||||
|
#define DEFAULT_FRAME_LENGTH 4800
|
||||||
|
#define DEFAULT_FS 48000
|
||||||
|
#define DEFAULT_SAMPLE_SIZE 2
|
||||||
|
#define DEFAULT_CHANNELS 1
|
||||||
|
|
||||||
|
class EndEvent : public UEvent
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual std::string getClassName() const {return "EndEvent";}
|
||||||
|
};
|
||||||
|
|
||||||
|
class MicroWrapper : public UThreadNode
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
MicroWrapper(int deviceId, int frameLength, int fs, int sampleSize, int nChannels)
|
||||||
|
{
|
||||||
|
micro_ = new rtabmap::Micro(rtabmap::MicroEvent::kTypeFrame, deviceId, fs, frameLength, nChannels, sampleSize, 0);
|
||||||
|
ros::NodeHandle nh("");
|
||||||
|
audioFramePublisher_ = nh.advertise<rtabmap_audio::AudioFrame>("audioFrame", 1);
|
||||||
|
audioFrameFreqPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreq>("audioFrameFreq", 1);
|
||||||
|
audioFrameFreqSqrdMagnPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreqSqrdMagn>("audioFrameFreqSqrdMagn", 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
MicroWrapper(const std::string & fileName, int frameLength)
|
||||||
|
{
|
||||||
|
micro_ = new rtabmap::Micro(rtabmap::MicroEvent::kTypeFrame, fileName, true, frameLength, 0);
|
||||||
|
ros::NodeHandle nh("");
|
||||||
|
audioFramePublisher_ = nh.advertise<rtabmap_audio::AudioFrame>("audioFrame", 1);
|
||||||
|
audioFrameFreqPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreq>("audioFrameFreq", 1);
|
||||||
|
audioFrameFreqSqrdMagnPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreqSqrdMagn>("audioFrameFreqSqrdMagn", 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual ~MicroWrapper()
|
||||||
|
{
|
||||||
|
this->join(true);
|
||||||
|
micro_->join(true);
|
||||||
|
delete micro_;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool init()
|
||||||
|
{
|
||||||
|
if(micro_)
|
||||||
|
{
|
||||||
|
return micro_->init();
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned int freq() const
|
||||||
|
{
|
||||||
|
if(micro_)
|
||||||
|
{
|
||||||
|
return micro_->fs();
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned int sampleSize() const
|
||||||
|
{
|
||||||
|
return micro_->bytesPerSample();
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned int nChannels() const
|
||||||
|
{
|
||||||
|
return micro_->channels();
|
||||||
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
virtual void mainLoopBegin()
|
||||||
|
{
|
||||||
|
micro_->startRecorder();
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual void mainLoop()
|
||||||
|
{
|
||||||
|
if(!micro_)
|
||||||
|
{
|
||||||
|
UERROR("micro_ is not initialized");
|
||||||
|
this->kill();
|
||||||
|
}
|
||||||
|
bool computeFFT = false;
|
||||||
|
if(audioFrameFreqPublisher_.getNumSubscribers() || audioFrameFreqSqrdMagnPublisher_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
computeFFT = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat data;
|
||||||
|
cv::Mat freq;
|
||||||
|
if(!computeFFT)
|
||||||
|
{
|
||||||
|
data = micro_->getFrame();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
data = micro_->getFrame(freq, false);
|
||||||
|
}
|
||||||
|
if(!data.empty())
|
||||||
|
{
|
||||||
|
ros::Time now = ros::Time::now();
|
||||||
|
if(audioFramePublisher_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
rtabmap_audio::AudioFramePtr msg(new rtabmap_audio::AudioFrame);
|
||||||
|
msg->header.frame_id = "micro";
|
||||||
|
msg->header.stamp = now;
|
||||||
|
msg->data.resize(data.total()*data.elemSize());
|
||||||
|
// Interleave the data
|
||||||
|
for(unsigned int i=0; i<msg->data.size(); i+=data.elemSize()*data.rows)
|
||||||
|
{
|
||||||
|
for(int j=0; j<data.rows; ++j)
|
||||||
|
{
|
||||||
|
memcpy(msg->data.data()+i+j*data.elemSize(), data.data + (i/(data.elemSize()*data.rows))*data.elemSize() + j*data.cols*data.elemSize(), data.elemSize());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
msg->frameLength = data.cols;
|
||||||
|
msg->fs = micro_->fs();
|
||||||
|
msg->nChannels = data.rows;
|
||||||
|
msg->sampleSize = data.elemSize();
|
||||||
|
audioFramePublisher_.publish(msg);
|
||||||
|
}
|
||||||
|
if(audioFrameFreqPublisher_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
rtabmap_audio::AudioFrameFreqPtr msg(new rtabmap_audio::AudioFrameFreq);
|
||||||
|
msg->header.frame_id = "micro";
|
||||||
|
msg->header.stamp = now;
|
||||||
|
msg->data.resize(freq.total()*freq.elemSize());
|
||||||
|
memcpy(msg->data.data(), freq.data, msg->data.size());
|
||||||
|
msg->frameLength = freq.cols;
|
||||||
|
msg->fs = micro_->fs();
|
||||||
|
msg->nChannels = freq.rows;
|
||||||
|
audioFrameFreqPublisher_.publish(msg);
|
||||||
|
}
|
||||||
|
if(audioFrameFreqSqrdMagnPublisher_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
rtabmap_audio::AudioFrameFreqSqrdMagnPtr msg(new rtabmap_audio::AudioFrameFreqSqrdMagn);
|
||||||
|
msg->header.frame_id = "micro";
|
||||||
|
msg->header.stamp = now;
|
||||||
|
|
||||||
|
//compute the squared magnitude
|
||||||
|
cv::Mat sqrdMagn(freq.rows, freq.cols/2, CV_32F);
|
||||||
|
//for each channels
|
||||||
|
for(int i=0; i<sqrdMagn.rows; ++i)
|
||||||
|
{
|
||||||
|
cv::Mat rowFreq = freq.row(i);
|
||||||
|
cv::Mat rowSqrdMagn = sqrdMagn.row(i);
|
||||||
|
float re;
|
||||||
|
float im;
|
||||||
|
for(int j=0; j<rowSqrdMagn.cols; ++j)
|
||||||
|
{
|
||||||
|
re = rowFreq.at<float>(0, j*2);
|
||||||
|
im = rowFreq.at<float>(0, j*2+1);
|
||||||
|
rowSqrdMagn.at<float>(0, j) = re*re + im*im;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
msg->data.resize(sqrdMagn.cols * sqrdMagn.rows);
|
||||||
|
memcpy(msg->data.data(), sqrdMagn.data, sqrdMagn.total() * sqrdMagn.elemSize());
|
||||||
|
msg->frameLength = sqrdMagn.cols;
|
||||||
|
msg->fs = micro_->fs();
|
||||||
|
msg->nChannels = sqrdMagn.rows;
|
||||||
|
|
||||||
|
audioFrameFreqSqrdMagnPublisher_.publish(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
UEventsManager::post(new EndEvent());
|
||||||
|
this->kill();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
ros::Publisher audioFramePublisher_;
|
||||||
|
ros::Publisher audioFrameFreqPublisher_;
|
||||||
|
ros::Publisher audioFrameFreqSqrdMagnPublisher_;
|
||||||
|
rtabmap::Micro * micro_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class Handler: public UEventsHandler
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Handler() {UEventsManager::addHandler(this);}
|
||||||
|
virtual ~Handler() {UEventsManager::removeHandler(this);}
|
||||||
|
protected:
|
||||||
|
virtual void handleEvent(UEvent * event)
|
||||||
|
{
|
||||||
|
if(event->getClassName().compare("EndEvent") == 0)
|
||||||
|
{
|
||||||
|
ROS_INFO("End of stream reached... shutting down!");
|
||||||
|
ros::shutdown();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
ULogger::setType(ULogger::kTypeConsole);
|
||||||
|
//ULogger::setLevel(ULogger::kDebug);
|
||||||
|
ros::init(argc, argv, "audioRecorder");
|
||||||
|
ros::NodeHandle nh("~");
|
||||||
|
|
||||||
|
int deviceId = DEFAULT_DEVICE_ID;
|
||||||
|
std::string fileName = DEFAULT_FILE_NAME;
|
||||||
|
int frameLength = DEFAULT_FRAME_LENGTH;
|
||||||
|
int fs = DEFAULT_FS;
|
||||||
|
int sampleSize = DEFAULT_SAMPLE_SIZE;
|
||||||
|
int nChannels = DEFAULT_CHANNELS;
|
||||||
|
|
||||||
|
nh.param("device_id", deviceId, deviceId);
|
||||||
|
nh.param("file_name", fileName, fileName);
|
||||||
|
nh.param("frame_length", frameLength, frameLength);
|
||||||
|
nh.param("fs", fs, fs);
|
||||||
|
nh.param("sample_size", sampleSize, sampleSize);
|
||||||
|
nh.param("channels", nChannels, nChannels);
|
||||||
|
|
||||||
|
MicroWrapper * micro;
|
||||||
|
if(fileName.size() && UFile::exists(fileName))
|
||||||
|
{
|
||||||
|
ROS_INFO("Recording from file %s", fileName.c_str());
|
||||||
|
micro = new MicroWrapper(fileName, frameLength);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_INFO("Recording from microphone %d", deviceId);
|
||||||
|
micro = new MicroWrapper(deviceId, frameLength, fs, sampleSize, nChannels);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!micro->init())
|
||||||
|
{
|
||||||
|
ROS_ERROR("Cannot initiate the audio recorder.");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ROS_INFO("frame_length=%d", frameLength);
|
||||||
|
ROS_INFO("fs=%d", micro->freq());
|
||||||
|
ROS_INFO("sample_size=%d", micro->sampleSize());
|
||||||
|
ROS_INFO("channels=%d", micro->nChannels());
|
||||||
|
|
||||||
|
Handler h;
|
||||||
|
// Start the mic
|
||||||
|
micro->start();
|
||||||
|
ROS_INFO("Audio recorder started...");
|
||||||
|
ros::spin();
|
||||||
|
}
|
||||||
|
|
||||||
|
micro->join(true);
|
||||||
|
delete micro;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,44 @@
|
|||||||
|
cmake_minimum_required(VERSION 2.4.6)
|
||||||
|
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
|
||||||
|
|
||||||
|
project(rtabmap-audio-pkg)
|
||||||
|
|
||||||
|
# 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)
|
||||||
|
|
||||||
|
SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}")
|
||||||
|
|
||||||
|
#uncomment if you have defined messages
|
||||||
|
rosbuild_genmsg()
|
||||||
|
#uncomment if you have defined services
|
||||||
|
#rosbuild_gensrv()
|
||||||
|
|
||||||
|
#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})
|
||||||
|
|
||||||
|
rosbuild_add_executable(audio_recorder AudioRecorderNode.cpp)
|
||||||
|
target_link_libraries(audio_recorder)
|
||||||
|
|
||||||
|
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
|
||||||
|
find_package(Fmodex REQUIRED)
|
||||||
|
INCLUDE(${QT_USE_FILE})
|
||||||
|
|
||||||
|
rosbuild_add_executable(audio_player AudioPlayerNode.cpp ${Fmodex_INCLUDE_DIRS})
|
||||||
|
target_link_libraries(audio_player ${Fmodex_LIBRARIES} ${QT_LIBRARIES})
|
||||||
@@ -0,0 +1,33 @@
|
|||||||
|
# - Find Fmodex
|
||||||
|
# This module finds an installed Fmod package.
|
||||||
|
#
|
||||||
|
# It sets the following variables:
|
||||||
|
# Fmodex_FOUND - Set to false, or undefined, if Fmod isn't found.
|
||||||
|
# Fmodex_INCLUDE_DIRS - The Fmod include directory.
|
||||||
|
# Fmodex_LIBRARIES - The Fmod library to link against.
|
||||||
|
#
|
||||||
|
#
|
||||||
|
|
||||||
|
FIND_PATH(Fmodex_INCLUDE_DIRS fmod.h)
|
||||||
|
|
||||||
|
IF(CMAKE_SIZEOF_VOID_P EQUAL 8)
|
||||||
|
FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex64)
|
||||||
|
ELSE(CMAKE_SIZEOF_VOID_P EQUAL 8)
|
||||||
|
FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex)
|
||||||
|
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8)
|
||||||
|
|
||||||
|
IF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES)
|
||||||
|
SET(Fmodex_FOUND TRUE)
|
||||||
|
ENDIF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES)
|
||||||
|
|
||||||
|
IF (Fmodex_FOUND)
|
||||||
|
# show which Fmod was found only if not quiet
|
||||||
|
IF (NOT Fmodex_FIND_QUIETLY)
|
||||||
|
MESSAGE(STATUS "Found Fmod: ${Fmodex_LIBRARIES}")
|
||||||
|
ENDIF (NOT Fmodex_FIND_QUIETLY)
|
||||||
|
ELSE (Fmodex_FOUND)
|
||||||
|
# fatal error if Fmod is required but not found
|
||||||
|
IF (Fmodex_FIND_REQUIRED)
|
||||||
|
MESSAGE(FATAL_ERROR "Could not find Fmod...")
|
||||||
|
ENDIF (Fmodex_FIND_REQUIRED)
|
||||||
|
ENDIF (Fmodex_FOUND)
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
include $(shell rospack find mk)/cmake.mk
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
/**
|
||||||
|
\mainpage
|
||||||
|
\htmlinclude manifest.html
|
||||||
|
|
||||||
|
\b audio
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Provide an overview of your package.
|
||||||
|
-->
|
||||||
|
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
*/
|
||||||
@@ -0,0 +1,20 @@
|
|||||||
|
<package>
|
||||||
|
<description brief="rtabmap_audio">
|
||||||
|
|
||||||
|
audio
|
||||||
|
|
||||||
|
</description>
|
||||||
|
<author>Mathieu Labbé</author>
|
||||||
|
<license>BSD</license>
|
||||||
|
<review status="unreviewed" notes=""/>
|
||||||
|
<url>http://ros.org/wiki/rtabmap_audio</url>
|
||||||
|
|
||||||
|
<depend package="std_msgs"/>
|
||||||
|
<depend package="roscpp"/>
|
||||||
|
<depend package="rtabmap_lib"/>
|
||||||
|
|
||||||
|
<rosdep name="libqt4-dev"/>
|
||||||
|
|
||||||
|
</package>
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,11 @@
|
|||||||
|
########################################
|
||||||
|
# Audio frame in time domain (raw)
|
||||||
|
########################################
|
||||||
|
|
||||||
|
Header header
|
||||||
|
|
||||||
|
uint32 sampleSize # bytes per sample
|
||||||
|
uint32 frameLength
|
||||||
|
uint32 nChannels
|
||||||
|
uint32 fs
|
||||||
|
uint8[] data
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
########################################
|
||||||
|
# Audio frame in frequency domain (with real and imaginary parts)
|
||||||
|
########################################
|
||||||
|
|
||||||
|
Header header
|
||||||
|
|
||||||
|
uint32 frameLength
|
||||||
|
uint32 nChannels
|
||||||
|
uint32 fs
|
||||||
|
float32[] data
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
########################################
|
||||||
|
# Audio frame in frequency domain (squared magnitude)
|
||||||
|
########################################
|
||||||
|
|
||||||
|
Header header
|
||||||
|
|
||||||
|
uint32 frameLength
|
||||||
|
uint32 nChannels
|
||||||
|
uint32 fs
|
||||||
|
float32[] data
|
||||||
@@ -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"))
|
||||||
@@ -2,7 +2,7 @@
|
|||||||
<!-- Nodes -->
|
<!-- Nodes -->
|
||||||
<node name="camera" pkg="rtabmap" type="camera" output="screen">
|
<node name="camera" pkg="rtabmap" type="camera" output="screen">
|
||||||
<param name="device_id" value="0" type="int"/>
|
<param name="device_id" value="0" type="int"/>
|
||||||
<param name="image_hz" value="5.0" type="double"/>
|
<param name="image_hz" value="0" type="double"/>
|
||||||
<param name="image_width" value="640" type="int"/>
|
<param name="image_width" value="640" type="int"/>
|
||||||
<param name="image_height" value="480" type="int"/>
|
<param name="image_height" value="480" type="int"/>
|
||||||
</node>
|
</node>
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -11,6 +11,8 @@ include $(shell rospack find mk)/svn_checkout.mk
|
|||||||
|
|
||||||
CMAKE = cmake
|
CMAKE = cmake
|
||||||
CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \
|
CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \
|
||||||
|
-D BUILD_LIBS_ONLY=ON \
|
||||||
|
-D BUILD_TESTS=OFF \
|
||||||
-D CMAKE_INSTALL_PREFIX=`rospack find rtabmap_lib`/$(INSTALL_DIR)
|
-D CMAKE_INSTALL_PREFIX=`rospack find rtabmap_lib`/$(INSTALL_DIR)
|
||||||
|
|
||||||
installed: $(SVN_DIR) patched
|
installed: $(SVN_DIR) patched
|
||||||
|
|||||||
@@ -20,9 +20,10 @@
|
|||||||
<depend package="std_srvs"/>
|
<depend package="std_srvs"/>
|
||||||
<depend package="utilite"/>
|
<depend package="utilite"/>
|
||||||
|
|
||||||
|
<rosdep name="libqt4-dev"/>
|
||||||
<rosdep name="sqlite3"/>
|
<rosdep name="sqlite3"/>
|
||||||
<rosdep name="fftw3"/>
|
<rosdep name="libsqlite3-dev"/>
|
||||||
<rosdep name="opencv2.3"/>
|
<rosdep name="libfftw3-dev"/>
|
||||||
|
|
||||||
</package>
|
</package>
|
||||||
|
|
||||||
|
|||||||
@@ -7,7 +7,7 @@
|
|||||||
<author>Mathieu Labbe</author>
|
<author>Mathieu Labbe</author>
|
||||||
<license>GPL</license>
|
<license>GPL</license>
|
||||||
<review status="unreviewed" notes=""/>
|
<review status="unreviewed" notes=""/>
|
||||||
<url>http://rtabmap-ros-pkg.googlecode.com</url>
|
<url>http://rtabmap.googlecode.com</url>
|
||||||
<depend stack="common_msgs" /> <!-- sensor_msgs -->
|
<depend stack="common_msgs" /> <!-- sensor_msgs -->
|
||||||
<depend stack="ros" />
|
<depend stack="ros" />
|
||||||
<depend stack="ros_comm" /> <!-- std_msgs, std_srvs, roscpp -->
|
<depend stack="ros_comm" /> <!-- std_msgs, std_srvs, roscpp -->
|
||||||
|
|||||||
+5
-1
@@ -5,12 +5,16 @@ all: installed
|
|||||||
|
|
||||||
SVN_DIR = build/utilite-svn
|
SVN_DIR = build/utilite-svn
|
||||||
SVN_URL = https://utilite.googlecode.com/svn/trunk/
|
SVN_URL = https://utilite.googlecode.com/svn/trunk/
|
||||||
SVN_REVISION = -rHEAD
|
SVN_REVISION = -r173
|
||||||
#SVN_PATCH = opencvVer.patch
|
#SVN_PATCH = opencvVer.patch
|
||||||
include $(shell rospack find mk)/svn_checkout.mk
|
include $(shell rospack find mk)/svn_checkout.mk
|
||||||
|
|
||||||
CMAKE = cmake
|
CMAKE = cmake
|
||||||
CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \
|
CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \
|
||||||
|
-D BUILD_AUDIO=ON \
|
||||||
|
-D BUILD_QT=ON \
|
||||||
|
-D BUILD_EXAMPLES=OFF \
|
||||||
|
-D BUILD_TESTS=OFF \
|
||||||
-D CMAKE_INSTALL_PREFIX=`rospack find utilite`/$(INSTALL_DIR)
|
-D CMAKE_INSTALL_PREFIX=`rospack find utilite`/$(INSTALL_DIR)
|
||||||
|
|
||||||
installed: $(SVN_DIR) patched
|
installed: $(SVN_DIR) patched
|
||||||
|
|||||||
@@ -9,11 +9,15 @@
|
|||||||
<review status="unreviewed" notes=""/>
|
<review status="unreviewed" notes=""/>
|
||||||
<url>http://utilite.googlecode.com</url>
|
<url>http://utilite.googlecode.com</url>
|
||||||
<export>
|
<export>
|
||||||
<cpp cflags="-I${prefix}/utilite/include" lflags="-L${prefix}/utilite/lib -Wl,-rpath, -lutilite"/>
|
<cpp cflags="-I${prefix}/utilite/include" lflags="-L${prefix}/utilite/lib -Wl,-rpath, -lutilite -lutilite_qt -lutilite_audio"/>
|
||||||
</export>
|
</export>
|
||||||
<versioncontrol type="svn" url="http://utilite.googlecode.com/svn/trunk/"/>
|
<versioncontrol type="svn" url="http://utilite.googlecode.com/svn/trunk/"/>
|
||||||
|
|
||||||
<depend package="roscpp"/>
|
<depend package="roscpp"/>
|
||||||
|
|
||||||
|
<rosdep name="libqt4-dev"/>
|
||||||
|
<rosdep name="libmp3lame-dev"/>
|
||||||
|
<rosdep name="libfftw3-dev"/>
|
||||||
</package>
|
</package>
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user