...
POST No. 2590256
터틀봇3를 이용한 네이게이션(Navigation) 관련된 질문입니다.
2018-12-05 16:27:58 neoplanetz

안녕하세요. 현재 터틀봇3를 4대정도 구매해서 사용하고 있습니다.

 

다행히 이번 펌웨어 업데이트로 멀티 로봇 구동이 가능하도록 변경되서 정말 좋습니다.

 

 제 질문은 터틀봇3로 gmapping을 하면서 동시에 Navigation을 할수 있는 방법이 있을까요?

 

구글의 카토그래퍼 같은경우 실행해보았더니 SLAM을 하면서 동시에 Navigation이 가능하더군요.

 

그래서 저도 멀티 로봇을 이용해 gmapping을 하면서 동시에 Navigation 기능이 필요하더군요.

 

gmapping으로 map을 building 하면서 동시에 navigation 에서는 계속해서 조금씩 만들어지는 map을 받아 navigation을 수행하고 싶습니다.

 

혹시 가능한 방법이 있을까 질문 여쭤드립니다. 항상 Q&A 게시판에 친절하게 답변해주셔서 감사드립니다.

 

질문요약: 멀티 터틀봇3로 gmapping을 하면서 동시에 Rviz를 통해 Navigation을 할수 있는 방법(구글 cartographer와 유사한 방식)

2018-12-05 16:27:58
정요한
2018-12-06 16:32:38 유기웅

안녕하세요.

 

1. 대상 제품
- Turtlebot3

2. 고객 질문
- 멀티 turtlebot3으로 gmapping과 동시에 navigation 하는 방법

3. 참고 자료

4. 답변
- gmapping과 navigation을 동시에 하려면 저희가 제공하는 turtlebot3_slam 패키지의 turtlebot3_slam.launch 파일에 하기와 같이 move_base 노드를 추가 해주면 가능합니다.

<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
</include>

이 방법은 cartographer와 같은 방법이지만, gmapping 의 성능이  cartographer에 비해 좋지 않기 때문에 navigation 시 로봇이 제대로된 경로를 생성하지 못할 수도 있습니다.
때문에 실제 로봇으로 navigation을 실행하면 벽에 부딪힐 가능성이 높아서 추천 드리지 않습니다.
2018-12-06 16:32:38
유기웅
2018-12-06 23:53:07 정요한

답변 감사합니다.

 

말씀주신거와같이 move_base 노드를 추가하여 실행하였지만 다음과 같이 계속 경고가 뜹니다.

 

<경고 메세지>

[ WARN] [1544107140.495542062]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: source_frame base_footprint does not exist.. canTransform returned after 0.100358 timeout was 0.1.

 

제가 실행한 Launch 파일의 내용입니다.

 

<launch>
<!-- Arguments -->
<arg name="model" default="waffle" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="configuration_basename" default="turtlebot3_lds_2d.lua"/>
<arg name="slam_methods" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
<arg name="multi_robot_name" default=""/>
<arg name="set_base_frame" default="base_footprint"/>
<arg name="set_odom_frame" default="odom"/>
<arg name="set_map_frame" default="map"/>
<arg name="set_cmd_vel" default="/cmd_vel"/>

<!-- Turtlebot3 Remote -->
<include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
<arg name="model" value="$(arg model)" />
</include>

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" value="$(arg multi_robot_name)"/>
</node>

<!-- Turtlebot3 SLAM: Gmapping, Cartographer -->
<include file="$(find turtlebot3_slam)/launch/turtlebot3_$(arg slam_methods).launch">
<arg name="model" value="$(arg model)"/>
<arg name="configuration_basename" value="$(arg configuration_basename)"/>
<arg name="multi_robot_name" default="$(arg multi_robot_name)"/>
<arg name="set_base_frame" default="$(arg set_base_frame)"/>
<arg name="set_odom_frame" default="$(arg set_odom_frame)"/>
<arg name="set_map_frame" default="$(arg set_map_frame)"/>
</include>

<node pkg="tf" type="static_transform_publisher" name="world_to_$(arg multi_robot_name)_tf_broadcaster" args="0 0 0 0 0 0 /map /$(arg multi_robot_name)/map 100"/>

<!-- move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
<rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
<remap from="cmd_vel" to="$(arg set_cmd_vel)"/>
<remap from="odom" to="$(arg set_odom_frame)"/>
</node>

</launch>


그리고 실행한 명령은 다음과 같습니다.

$ ROS_NAMESPACE=tb3_0 roslaunch turtlebot3_bringup turtlebot3_remote_slam.launch slam_methods:=gmapping multi_robot_name:=tb3_0 set_base_frame:=tb3_0/base_footprint set_odom_frame:=tb3_0/odom set_map_frame:=tb3_0/map set_cmd_vel:=tb3_0/cmd_vel

제가 보기에  멀티로봇 네임스페이스 때문에 move_base 코드 안에 있는 base_footprint 가 제대로 네임스페이스 변경이 안된거 같은데 move_base 는 로보티즈의 패키지가 아닌거 같아 볼수가 없네요.

답변 부탁드리겠습니다 ㅠㅠ
2018-12-06 23:53:07
정요한
2018-12-07 00:17:26 정요한

그 후에 터틀봇 네비게이션쪽 파라미터쪽을 고쳐볼까 해서 한번 파라미터를 수정해보았습니다. 총 3개의 파타미터 파일을 수정했습니다.

 

1. costmap_common_params_waffle.yaml

obstacle_range: 3.0
raytrace_range: 3.5

footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
#robot_radius: 0.17

inflation_radius: 1.0
cost_scaling_factor: 3.0

map_type: costmap
observation_sources: tb3_0/scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

 

2. global_costmap_params.yaml

global_costmap:
global_frame: tb3_0/map
robot_base_frame: tb3_0/base_footprint

update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5

static_map: true

 

3. local_costmap_params.yaml

local_costmap:
global_frame: tb3_0/odom
robot_base_frame: tb3_0/base_footprint

update_frequency: 10.0
publish_frequency: 10.0
transform_tolerance: 0.5

static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.05

 

이렇게 수정후 다시 돌려보니 다음과 같은 경고가 계속 뜹니다.

[ WARN] [1544109345.625193729]: MessageFilter [target=tb3_0/odom base_scan ]: Dropped 100.00% of messages so far. Please turn the [ros.costmap_2d.message_notifier] rosconsole logger to DEBUG for more information.

[ WARN] [1544109345.634551584]: MessageFilter [target=tb3_0/map base_scan ]: Dropped 100.00% of messages so far. Please turn the [ros.costmap_2d.message_notifier] rosconsole logger to DEBUG for more information.



그리고 현재 저의 rostopic 은 다음과 같습니다.
/diagnostics
/rosout
/rosout_agg
/tb3_0/battery_state
/tb3_0/camera/color/camera_info
/tb3_0/camera/color/image_raw
/tb3_0/camera/color/image_raw/compressed
/tb3_0/camera/color/image_raw/compressed/parameter_descriptions
/tb3_0/camera/color/image_raw/compressed/parameter_updates
/tb3_0/camera/color/image_raw/compressedDepth
/tb3_0/camera/color/image_raw/compressedDepth/parameter_descriptions
/tb3_0/camera/color/image_raw/compressedDepth/parameter_updates
/tb3_0/camera/color/image_raw/theora
/tb3_0/camera/color/image_raw/theora/parameter_descriptions
/tb3_0/camera/color/image_raw/theora/parameter_updates
/tb3_0/camera/depth/camera_info
/tb3_0/camera/depth/image_raw
/tb3_0/camera/depth/image_raw/compressed
/tb3_0/camera/depth/image_raw/compressed/parameter_descriptions
/tb3_0/camera/depth/image_raw/compressed/parameter_updates
/tb3_0/camera/depth/image_raw/compressedDepth
/tb3_0/camera/depth/image_raw/compressedDepth/parameter_descriptions
/tb3_0/camera/depth/image_raw/compressedDepth/parameter_updates
/tb3_0/camera/depth/image_raw/theora
/tb3_0/camera/depth/image_raw/theora/parameter_descriptions
/tb3_0/camera/depth/image_raw/theora/parameter_updates
/tb3_0/camera/depth/points
/tb3_0/camera/driver/parameter_descriptions
/tb3_0/camera/driver/parameter_updates
/tb3_0/camera/ir/camera_info
/tb3_0/camera/ir/image_raw
/tb3_0/camera/ir/image_raw/compressed
/tb3_0/camera/ir/image_raw/compressed/parameter_descriptions
/tb3_0/camera/ir/image_raw/compressed/parameter_updates
/tb3_0/camera/ir/image_raw/compressedDepth
/tb3_0/camera/ir/image_raw/compressedDepth/parameter_descriptions
/tb3_0/camera/ir/image_raw/compressedDepth/parameter_updates
/tb3_0/camera/ir/image_raw/theora
/tb3_0/camera/ir/image_raw/theora/parameter_descriptions
/tb3_0/camera/ir/image_raw/theora/parameter_updates
/tb3_0/camera/ir2/camera_info
/tb3_0/camera/ir2/image_raw
/tb3_0/camera/ir2/image_raw/compressed
/tb3_0/camera/ir2/image_raw/compressed/parameter_descriptions
/tb3_0/camera/ir2/image_raw/compressed/parameter_updates
/tb3_0/camera/ir2/image_raw/compressedDepth
/tb3_0/camera/ir2/image_raw/compressedDepth/parameter_descriptions
/tb3_0/camera/ir2/image_raw/compressedDepth/parameter_updates
/tb3_0/camera/ir2/image_raw/theora
/tb3_0/camera/ir2/image_raw/theora/parameter_descriptions
/tb3_0/camera/ir2/image_raw/theora/parameter_updates
/tb3_0/camera/nodelet_manager/bond
/tb3_0/cmd_vel
/tb3_0/cmd_vel_rc100
/tb3_0/diagnostics
/tb3_0/firmware_version
/tb3_0/imu
/tb3_0/joint_states
/tb3_0/magnetic_field
/tb3_0/motor_power
/tb3_0/odom
/tb3_0/reset
/tb3_0/scan
/tb3_0/sensor_state
/tb3_0/sound
/tb3_0/version_info
/tf
/tf_static

그리고 현재의 TF Tree는 다음과 같습니다.




답변 부탁드리겠습니다 ㅠㅠ

 

 

 

 

 

2018-12-07 00:17:26
정요한
답변달기
웹에디터 시작 웹 에디터 끝