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

Pose goals can be sent directly to rtabmap node, Added SetGoal service (set target node id)

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 13:56:20 -05:00
parent 19f667b3f4
commit cd4975849a
12 changed files with 181 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/az3_mapping_client_nav.launch
+3 -16
View File
@@ -16,22 +16,9 @@
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
<!-- use a relay on this machine, same for images -->
<node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
<node if="$(arg rviz)" name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
<node if="$(arg rviz)" name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
<node if="$(arg rviz)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
<node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
<param name="lazy" type="bool" value="true"/>
</node>
</launch>
launch/azimut3/az3_mapping_robot_nav.launch
+37 -65
View File
@@ -7,73 +7,9 @@
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
<remap from="joy" to="/joy"/>
<node name="teleop" pkg="nodelet" type="nodelet"
args="standalone azimut_tools/Teleop"/>
<param name="cmd_eta/abtr_priority" value="50"/>
</group>
<group ns="go_back">
<remap from="joy" to="/joy"/>
<remap from="goal" to="/planner_goal"/>
<node name="go_back" pkg="azimut_tools" type="go_back"/>
<rosparam>
btn_save: 1
btn_goto: 3
</rosparam>
</group>
<group ns="planner">
<remap from="openni_points" to="/planner_cloud"/>
<remap from="base_scan" to="/base_scan"/>
<remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<include file="$(find az3_navigation)/launch/az3_move_base_slam.launch"/>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
</node>
<node name="register_cmd_eta" pkg="abtr_priority" type="register"
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="rgb/image_out" to="data_throttled_image"/>
<remap from="depth/image_out" to="data_throttled_image_depth"/>
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
</node>
<!-- for the planner -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb_planner" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<remap from="cloud" to="/planner_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="decimation" type="int" value="4"/>
</node>
</group>
<!-- SLAM (robot side) -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
@@ -94,7 +30,6 @@
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
@@ -136,4 +71,41 @@
</node>
</group>
<!-- teleop -->
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
<remap from="joy" to="/joy"/>
<node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
<param name="cmd_eta/abtr_priority" value="50"/>
</group>
<!-- ROS navigation stack move_base -->
<group ns="planner">
<remap from="openni_points" to="/planner_cloud"/>
<remap from="base_scan" to="/base_scan"/>
<remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser_only.yaml" command="load" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
</node>
<!-- Arbitration between teleop and planner -->
<node name="register_cmd_eta" pkg="abtr_priority" type="register"
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
</launch>
launch/azimut3/config/azimut3_nav.rviz
+27 -27
View File
@@ -6,23 +6,23 @@ Panels:
Expanded:
- /Global Options1
- /TF1/Frames1
- /Info1/Status1
- /MapGraph1
- /MapCloud1
- /Path1
- /Path2
Splitter Ratio: 0.601881
Tree Height: 337
Tree Height: 437
- Class: rviz/Selection
Name: Selection
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Image
SyncSource: ""
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
@@ -127,8 +127,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 5.19727
Min Value: -0.547517
Max Value: 3.46596
Min Value: -5.27159e-08
Value: true
Axis: X
Channel Name: intensity
@@ -136,7 +136,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
@@ -152,7 +152,7 @@ Visualization Manager:
Topic: /base_scan
Use Fixed Frame: false
Use rainbow: true
Value: true
Value: false
- Alpha: 0.7
Class: rviz/Map
Color Scheme: map
@@ -173,10 +173,10 @@ Visualization Manager:
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Enabled: false
Name: Map
Topic: /planner/move_base/global_costmap/costmap
Value: true
Value: false
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
@@ -249,7 +249,7 @@ Visualization Manager:
Value: true
Visual Enabled: true
- Class: rviz/Image
Enabled: true
Enabled: false
Image Topic: /camera/data_throttled_image_relay
Max Value: 1
Median window: 5
@@ -258,7 +258,7 @@ Visualization Manager:
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Value: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@@ -384,32 +384,32 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 14.3083
Distance: 3.44552
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.91382
Y: -0.564774
Z: 0.661231
X: 0.0430748
Y: -0.00858197
Z: -0.156054
Name: Current View
Near Clip Distance: 0.01
Pitch: 1.4054
Target Frame: <Fixed Frame>
Pitch: 0.9304
Target Frame: base_footprint
Value: Orbit (rviz)
Yaw: 3.1704
Yaw: 2.26538
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 826
Hide Left Dock: false
collapsed: true
Height: 504
Hide Left Dock: true
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000151000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000192000000dd00fffffffb0000000a0049006d00610067006501000001c00000015c0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002f4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003ad000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001510000028ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000028f000000dd00fffffffb0000000a0049006d006100670065000000018a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002ca000001b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -418,6 +418,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1561
X: 42
Y: 53
Width: 714
X: 935
Y: 32
launch/azimut3/config/local_costmap_params_laser_only.yaml
+33
View File
@@ -0,0 +1,33 @@
local_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 2.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.025
origin_x: -2.0
origin_y: -2.0
observation_sources: laser_scan_sensor
#observation_sources: laser_scan_sensor point_cloud_sensor
laser_scan_sensor: {
data_type: LaserScan,
topic: base_scan,
expected_update_rate: 0.2,
marking: true,
clearing: true}
# assuming receiving a cloud from rtabmap/obstacles_detection node
#point_cloud_sensor: {
# sensor_frame: base_footprint,
# data_type: PointCloud2,
# topic: openni_points,
# expected_update_rate: 0.5,
# marking: true,
# clearing: true,
# min_obstacle_height: -99999.0,
# max_obstacle_height: 99999.0}