Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP] jsk_hrp2jsknts_startup #1290

Closed
wants to merge 12 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Empty file added jsk_hrp2jsknts_robot/README.md
Empty file.
6 changes: 6 additions & 0 deletions jsk_hrp2jsknts_robot/jsk_hrp2jsknts_startup/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
cmake_minimum_required(VERSION 3.0.2)
project(jsk_hrp2jsknts_startup)

find_package(catkin REQUIRED)

catkin_package()
10 changes: 10 additions & 0 deletions jsk_hrp2jsknts_robot/jsk_hrp2jsknts_startup/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
```
roslaunch jsk_hrp2jsknts_startup hrp2jsknts_2dnav.launch
```
```
rosrun jsk_footstep_controller base-controller.l
```
```
rosnode kill pr2_teleop # if real robot. temporary
```
You can send 2d nav goal through rviz.
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
global_costmap:
## costmap_2d
footprint: [[ 0.3, 0.5], [-0.3, 0.5], [-0.3, -0.5], [ 0.3, -0.5]]
footprint_padding: 0.03
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
global_frame: map ## The global frame for the costmap to operate in.
robot_base_frame: base_footprint ## The name of the frame for the base link of the robot.
transform_tolerance: 3.0 ## Specifies the delay in transform (tf) data that is tolerable in seconds. # rtabmapの更新速度が遅いので3.0にする
update_frequency: 0.2 ## The frequency in Hz for the map to be updated. # rtabmapの更新速度が遅いのでglobal plannerの周期にする
publish_frequency: 0.2 ## The frequency in Hz for the map to be publish display information. # rtabmapの更新速度が遅いのでglobal plannerの周期にする
rolling_window: false ## Whether or not to use a rolling window version of the costmap. If the static_map parameter is set to true, this parameter must be set to false.
always_send_full_costmap: false ## If true the full costmap is published to "~<name>/costmap" every update. If false only the part of the costmap that has changed is published on the "~<name>/costmap_updates" topic.

static_layer:
map_topic: /rtabmap/grid_map ## The topic that the costmap subscribes to for the static map. This parameter is useful when you have multiple costmap instances within a single node that you want to use different static maps.
track_unknown_space: false ## If true, unknown values in the map messages are translated directly to the layer. Otherwise, unknown values in the map message are translated as FREE_SPACE in the layer. ## trueが望ましいが、オンラインでマップを生成する場合はunknownが多すぎる
first_map_only: false ## Only subscribe to the first message on the map topic, ignoring all subsequent messages
subscribe_to_updates: false ## In addition to map_topic, also subscribe to map_topic + "_updates"
use_maximum: false ## Only matters if the static layer is not the bottom layer. If true, only the maximum value will be written to the master costmap.
trinary_costmap: true ## If true, translates all map message values to NO_INFORMATION/FREE_SPACE/LETHAL_OBSTACLE (three values). If false, a full spectrum of intermediate values is possible.

inflation_layer:
inflation_radius: 0.75 ## The radius in meters to which the map inflates obstacle cost values.
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
<launch>
<!-- Construct a 3D Map under /map and Localize robot-->
<arg name="DELETE_DB" default="true"/>
<arg name="delete_db_on_start" value="--delete_db_on_start" if="$(arg DELETE_DB)"/>
<arg name="delete_db_on_start" value="" unless="$(arg DELETE_DB)"/>
<group ns="rtabmap">
<group ns="head_camera">
<node pkg="nodelet" type="nodelet" name="head_camera_nodelet" args="manager"/>

<node pkg="nodelet" type="nodelet" name="depth_image_creator" args="load jsk_pcl/DepthImageCreator head_camera_nodelet">
<remap from="~input" to="/camera_remote/depth_registered/points_self_filtered"/>
<remap from="~info" to="/camera_remote/rgb/camera_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync head_camera_nodelet">
<remap from="rgb/image" to="/camera_remote/rgb/image_rect_color"/>
<remap from="depth/image" to="depth_image_creator/output"/>
<remap from="rgb/camera_info" to="/camera_remote/rgb/camera_info"/>

<remap from="rgbd_image" to="rgbd_image"/>
</node>
</group>

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" args="$(arg delete_db_on_start)">
<remap from="rgbd_image" to="head_camera/rgbd_image"/>
<rosparam subst_value="true">
subscribe_rgb: false
subscribe_depth: false
subscribe_rgbd: true
rgbd_cameras: 1 ## If > 1, the rgbd_image topics should contain the camera index starting with 0. For example, if we have 2 cameras, you should set rgbd_image0 and rgbd_image1 topics.

frame_id: BODY
odom_frame_id: odom #最初のodomの原点がmapの原点になる。2次元地図を使用する場合は、mapのXY平面に対して射影が行われるが、このとき、Z成分が負の点やGrid/MaxObstacleHeightより大きい点はobstacleとしては無視される。そのため、最初のodomの原点のZ高さに注意を払う必要がある。

map_always_update: true ## 立ち止まった時にもアップデート
wait_for_transform_duration: 0.5 ## 通信遅延に対応するため

Grid/RayTracing: true ## octomap_occupied_space にray tracingが適用される
Grid/CellSize: 0.02
GridGlobal/OccupancyThr: 0.5 ## Occupancy threshold (value between 0 and 1).
GridGlobal/ProbMiss: 0.3 ## Probability of a miss (value between 0 and 0.5). ## 小さすぎるとobstacleを消してしまう
GridGlobal/ProbHit: 0.9 ## Probability of a hit (value between 0.5 and 1). ## 大きくないと特に2d mapに射影するときに存在を認識できない
Grid/PreVoxelFiltering: false ## Input cloud is downsampled by voxel filter (voxel size is "Grid/CellSize") before doing segmentation of obstacles and ground.

Grid/MaxGroundHeight: 0.0 ## Maximum ground height (0=disabled).
Grid/MaxObstacleHeight: 0.0 ## Maximum obstacles height (0=disabled).
Grid/NormalsSegmentation: false ## Segment ground from obstacles using point normals, otherwise a fast passthrough is used
Grid/MapFrameProjection: false ## Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead. ## MaxGroundHeightがmap_frame_id系(true)かframe_id系(false)かに影響。falseの方が精度が良い? falseの場合, 2d map生成時にBODY相対でZ高さに応じて障害物を射影するので、BODYが傾くと対応不可
</rosparam>
</node>

<!-- add height information to project 2d -->
<node pkg="jsk_robot_startup" type="height_limit_rtabmap.py" name="height_limit_rtabmap">
<rosparam>
rate: 0.01
body_frame: BODY
foot_frame: base_footprint
lower_limit: 0.15
upper_limit: 1.6
rtabmap_ns: /rtabmap
enable_start: true
</rosparam>
</node>
</group>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<launch>
<include file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/self_filter.launch"/>
<include file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/global_map.launch"/>
<include file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/local_map.launch"/>
<include file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/move_base.launch"/>
<node pkg="rviz" type="rviz" name="move_base_rviz" args="-d $(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/move_base.rviz"/>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
local_costmap:
## costmap_2d
footprint: [[ 0.1, 0.25], [-0.1, 0.25], [-0.1, -0.25], [ 0.1, -0.25]] ## local plan後のfeasibility checkに用いられる. 小さいほうが安定する
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
robot_base_frame: base_footprint ## The name of the frame for the base link of the robot.
global_frame: odom ## The global frame for the costmap to operate in.
transform_tolerance: 3.0 ## Specifies the delay in transform (tf) data that is tolerable in seconds. # 通信遅延に対応
update_frequency: 2.0 ## The frequency in Hz for the map to be updated.
publish_frequency: 2.0 ## The frequency in Hz for the map to be publish display information.
rolling_window: false ## Whether or not to use a rolling window version of the costmap. If the static_map parameter is set to true, this parameter must be set to false.
always_send_full_costmap: false ## If true the full costmap is published to "~<name>/costmap" every update. If false only the part of the costmap that has changed is published on the "~<name>/costmap_updates" topic.

## staticmap
static_layer:
map_topic: /octomap/projected_map ## The topic that the costmap subscribes to for the static map. This parameter is useful when you have multiple costmap instances within a single node that you want to use different static maps.
track_unknown_space: false ## If true, unknown values in the map messages are translated directly to the layer. Otherwise, unknown values in the map message are translated as FREE_SPACE in the layer. ## trueが望ましいが、オンラインでマップを生成する場合はunknownが多すぎる
first_map_only: false ## Only subscribe to the first message on the map topic, ignoring all subsequent messages
subscribe_to_updates: false ## In addition to map_topic, also subscribe to map_topic + "_updates"
use_maximum: false ## Only matters if the static layer is not the bottom layer. If true, only the maximum value will be written to the master costmap.
trinary_costmap: true ## If true, translates all map message values to NO_INFORMATION/FREE_SPACE/LETHAL_OBSTACLE (three values). If false, a full spectrum of intermediate values is possible.
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
<launch>
<group ns="octomap">
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<remap from="cloud_in" to="/camera_remote/depth_registered/points_self_filtered" />

<rosparam>
frame_id: odom # Static global frame in which the map will be published
resolution: 0.02 # Resolution in meter for the map
base_frame_id: base_footprint ## The robot's base frame in which ground plane detection is performed (if enabled)
height_map: true # Whether visualization should encode height with different colors
sensor_model:
max_range: 5.0 #Maximum range in meter for inserting point cloud data when dynamically building a map.
filter_ground: false ## Whether the ground plane should be detected and ignored from scan data when dynamically building a map, using pcl::SACMODEL_PERPENDICULAR_PLANE. This clears everything up to the ground, but will not insert the ground as obstacle in the map. If this is enabled, it can be further configured with the ~ground_filter/... parameters. ## 視界に床が入らない場合も多いためfalse
miss: 0.3 ## Probabilities for hits and misses in the sensor model when dynamically building a map
occupancy_min_z: -infinity ## odom座標系であることに注意
occupancy_max_z: +infinity ## Minimum and maximum height of occupied cells to be consider in the final map. This ignores all occupied voxels outside of the interval when sending out visualizations and collision maps, but will not affect the actual octomap representation.
</rosparam>
</node>

<!-- add height information to project 2d -->
<node pkg="jsk_robot_startup" type="height_limit_octomap.py" name="height_limit_octomap">
<rosparam>
rate: 0.01
odom_frame: odom
foot_frame: base_footprint
lower_limit: 0.15
upper_limit: 1.6
enable_start: true
</rosparam>
</node>
</group>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/move_base_params.yaml" command="load" />
<rosparam file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/local_costmap_params.yaml" command="load" />
<rosparam file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/global_costmap_params.yaml" command="load" />
<rosparam file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/navfn_ros_params.yaml" command="load" />
<rosparam file="$(find jsk_hrp2jsknts_startup)/jsk_hrp2jsknts_move_base/teb_local_planner_params.yaml" command="load" />
</node>
</launch>
Loading