i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated demo_find_object.launch with save_objects_as_landmarks argument. Added script objects_to_tags.py. Scripts are now installed in binaries. Updated tag conversion to use frame_id/stamp included in detection array instead of parent if set.

Browse Source
This commit is contained in:
matlabbe
2019-08-09 21:52:01 -04:00
parent 18ee9909fc
commit f5a51aa87f
6 changed files with 138 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+8 -4
View File
@@ -428,10 +428,14 @@ set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "p
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
install(PROGRAMS
scripts/patrol.py
scripts/objects_to_tags.py
scripts/point_to_tf.py
scripts/transform_to_tf.py
scripts/yaml_to_camera_info.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables and/or libraries for installation
IF(RTABMAP_GUI)
launch/config/demo_find_object.rviz
+64 -16
View File
@@ -7,7 +7,7 @@ Panels:
- /Global Options1
- /TF1/Frames1
Splitter Ratio: 0.434783012
Tree Height: 590
Tree Height: 592
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -27,6 +27,8 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: LaserScan
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -65,7 +67,7 @@ Visualization Manager:
Max Color: 255; 255; 255
Max Intensity: 2.34176998e-38
Min Color: 0; 0; 0
Min Intensity: 9.21942286e-41
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
@@ -101,6 +103,16 @@ Visualization Manager:
Value: true
map:
Value: true
object_4:
Value: true
object_5:
Value: true
object_6:
Value: true
object_7:
Value: true
object_8:
Value: true
odom:
Value: true
wheelLB_linkWheel_link:
@@ -137,7 +149,16 @@ Visualization Manager:
{}
camera_rgb_frame:
camera_rgb_optical_frame:
{}
object_4:
{}
object_5:
{}
object_6:
{}
object_7:
{}
object_8:
{}
wheelLB_linkWheel_link:
wheelLB_wheel_link:
{}
@@ -155,8 +176,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 4.7127676
Min Value: 0.0444712937
Max Value: -0.662045956
Min Value: -5.20883799
Value: true
Axis: X
Channel Name: intensity
@@ -230,6 +251,7 @@ Visualization Manager:
Channel Name: intensity
Class: rtabmap_ros/MapCloud
Cloud decimation: 4
Cloud from scan: false
Cloud max depth (m): 3
Cloud min depth (m): 0
Cloud voxel size (m): 0.0199999996
@@ -271,6 +293,32 @@ Visualization Manager:
{}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /rtabmap/labels
Name: MarkerArray
Namespaces:
ids: false
labels: false
landmarks: true
Queue Size: 100
Value: true
- Alpha: 1
Arrow Length: 0.300000012
Axes Length: 0.300000012
Axes Radius: 0.100000001
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: true
Head Length: 0.0700000003
Head Radius: 0.0299999993
Name: PoseArray
Shaft Length: 0.230000004
Shaft Radius: 0.00999999978
Shape: Axes
Topic: /rtabmap/landmarks
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -292,35 +340,35 @@ Visualization Manager:
Views:
Current:
Class: rtabmap_ros/OrbitOriented
Distance: 7.89955997
Distance: 7.50918865
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.0199225005
Y: 0.235159993
Z: -0.0896359012
X: 0.0776291937
Y: 0.0206621774
Z: 0.317859411
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.819796979
Pitch: 0.909796953
Target Frame: base_link
Value: OrbitOriented (rtabmap)
Yaw: 3.14374995
Yaw: 3.73874164
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 763
Height: 765
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000d700fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000459000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002dffc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002df000000d700fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002ed000002df00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -329,6 +377,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1551
X: 49
Y: 107
Width: 1187
X: 699
Y: 344
launch/demo/demo_find_object.launch
+9
View File
@@ -6,6 +6,7 @@
<arg name="rtabmapviz" default="false" />
<arg name="save_objects" default="false"/>
<arg name="localization" default="false"/>
<arg name="save_objects_as_landmarks" default="false"/> <!-- apriltag_ros package should be installed and rtabmap_ros built with it -->
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
@@ -32,6 +33,8 @@
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<param name="landmark_linear_variance" type="double" value="0.1"/>
<param name="landmark_angular_variance" type="double" value="0.5"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
@@ -114,4 +117,10 @@
<param name="frame_id" value="base_footprint"/>
</node>
<!-- Convert objects to tags -->
<node if="$(arg save_objects_as_landmarks)" name="objects_to_tags" pkg="rtabmap_ros" type="objects_to_tags.py" output="screen">
<remap from="tag_detections" to="/rtabmap/tag_detections"/>
<param name="distance_max" value="2.0"/>
</node>
</launch>
launch/tests/test_apriltag_ros.launch
+2 -2
View File
@@ -2,11 +2,11 @@
<!-- Print on the file tag_bundle1.png (with GIMP: set size in Image Settings to 160x240mm) -->
<!-- Definition of the tag bundle is tags.yaml, make sure the camera is calibrated or adjust the size of the tag if needed -->
<!-- The TF published by apriltag_ros should match the point cloud created by the camera -->
<!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and tag_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
<!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and landmark_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
<!-- $ roslaunch realsense2_camera rs_camera.launch align_depth:=true -->
<!-- $ roslaunch rtabmap_ros test_apriltag_ros.launch rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info -->
<!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" tag_angular_variance:=9999 -->
<!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" landmark_angular_variance:=9999 -->
<arg name="camera_frame_id" default="camera_color_optical_frame"/>
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
scripts/objects_to_tags.py
Executable
+46
View File
@@ -0,0 +1,46 @@
#!/usr/bin/env python
import rospy
from apriltag_ros.msg import AprilTagDetectionArray
from apriltag_ros.msg import AprilTagDetection
from find_object_2d.msg import ObjectsStamped
import tf
import geometry_msgs.msg
objFramePrefix_ = "object"
distanceMax_ = 0.0
def callback(data):
global objFramePrefix_
global distanceMax_
if len(data.objects.data) > 0:
output = AprilTagDetectionArray()
output.header = data.header
for i in range(0,len(data.objects.data),12):
try:
objId = data.objects.data[i]
(trans,quat) = listener.lookupTransform(data.header.frame_id, objFramePrefix_+'_'+str(int(objId)), data.header.stamp)
tag = AprilTagDetection()
tag.id.append(objId)
tag.pose.pose.pose.position.x = trans[0]
tag.pose.pose.pose.position.y = trans[1]
tag.pose.pose.pose.position.z = trans[2]
tag.pose.pose.pose.orientation.x = quat[0]
tag.pose.pose.pose.orientation.y = quat[1]
tag.pose.pose.pose.orientation.z = quat[2]
tag.pose.pose.pose.orientation.w = quat[3]
tag.pose.header = output.header
if distanceMax_ <= 0.0 or trans[2] < distanceMax_:
output.detections.append(tag)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if len(output.detections) > 0:
pub.publish(output)
if __name__ == '__main__':
pub = rospy.Publisher('tag_detections', AprilTagDetectionArray, queue_size=10)
rospy.init_node('objects_to_tags', anonymous=True)
rospy.Subscriber("objectsStamped", ObjectsStamped, callback)
objFramePrefix_ = rospy.get_param('~object_prefix', objFramePrefix_)
distanceMax_ = rospy.get_param('~distance_max', distanceMax_)
listener = tf.TransformListener()
rospy.spin()
src/CoreWrapper.cpp
+9 -1
View File
@@ -2009,7 +2009,15 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
if(tagDetections.detections[i].id.size() >= 1)
{
geometry_msgs::PoseWithCovarianceStamped p = tagDetections.detections[i].pose;
p.header.frame_id = tagDetections.header.frame_id; // make sure we pass frame_id of parent
p.header = tagDetections.header;
if(!tagDetections.detections[i].pose.header.frame_id.empty())
{
p.header.frame_id = tagDetections.detections[i].pose.header.frame_id;
}
if(!tagDetections.detections[i].pose.header.stamp.isZero())
{
p.header.stamp = tagDetections.detections[i].pose.header.stamp;
}
uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p));
}
}