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.

Sign up for GitHub

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

Landmark #530

Open
wants to merge 3 commits into
base: melodic-devel
Choose a base branch
from
Open

Landmark #530

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
459a8aa
add emcl
yasuohayashibara Jan 16, 2022
5ee330e
change parameters
yasuohayashibara Jan 16, 2022
dcfb3a6
landmark (temp)
yasuohayashibara Jan 16, 2022
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
9 changes: 9 additions & 0 deletions orne_bringup/launch/orne_alpha_with_vision_sim.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<launch>
<include file="$(find icart_mini_gazebo)/launch/icart_mini.launch">
<arg name="gui" value="true"/>
<arg name="model" value="$(find xacro)/xacro.py '$(find orne_description)/urdf/orne_alpha_with_vision_sim.xacro'"/>
<arg name="world" value="$(find icart_mini_gazebo)/worlds/Tsudanuma_2-3.world"/>
</include>

<include file="$(find orne_bringup)/launch/includes/base.launch.xml"/>
</launch>
64 changes: 64 additions & 0 deletions orne_description/urdf/orne_alpha_with_vision_sim.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
<?xml version="1.0"?>

<robot name="orne_alpha" xmlns:xacro="http://ros.org/wiki/xacro"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
>

<xacro:include filename="$(find orne_description)/urdf/orne_x.xacro"/>
<xacro:include filename="$(find icart_mini_description)/urdf/sensors/imu.urdf.xacro"/>
<!-- Setting for imu -->
<xacro:sensor_imu name="imu" parent="base_link" size="0.05 0.05 0.05">
<origin xyz="-0.145 0.0 0.21" rpy="0 0 0"/>
</xacro:sensor_imu>



<!-- Setting for levelly fixed 2D URG -->
<xacro:include filename="$(find icart_mini_description)/urdf/sensors/hokuyo.urdf.xacro"/>
<xacro:sensor_hokuyo name="hokuyo" parent="base_link">
<origin xyz="0.212 0 0.2485" rpy="0 0 0"/>
</xacro:sensor_hokuyo>


<!-- Setting for diagonally fixed 2D URG -->
<!--
<xacro:include filename="$(find icart_mini_description)/urdf/sensors/hokuyo.urdf.xacro"/>
<xacro:sensor_hokuyo name="diagonally_hokuyo" parent="base_link">
<origin xyz="0.098 0 0.422" rpy="0 0.296706 0"/>
</xacro:sensor_hokuyo>
-->

<!--
<xacro:include filename="$(find icart_mini_description)/urdf/sensors/hokuyo3d.urdf.xacro"/>
<xacro:sensor_hokuyo3d name="hokuyo3d" parent="base_link">
<origin xyz="0.2 0 0.7" rpy="0 0.174 0"/>
</xacro:sensor_hokuyo3d>
-->

<xacro:include filename="$(find orne_description)/urdf/sensors/vlp16.urdf.xacro"/>
<xacro:vlp16_2d name="velodyne" parent="base_link">
<origin xyz="0.098 0.0 0.422" rpy="0 0 0"/>
</xacro:vlp16_2d>

<!--
<xacro:vlp16_3d name="velodyne3d" parent="base_link">
<origin xyz="0.098 0.0 0.422" rpy="0 0 0"/>
</xacro:vlp16_3d>
-->

<!-- Setting for camera -->
<xacro:include filename="$(find orne_description)/urdf/sensors/camera.urdf.xacro"/>
<xacro:usb_camera prefix="camera"/>

<gazebo>
<plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
<frameName>world</frameName>
<bodyName>base_link</bodyName>
<topicName>/tracker</topicName>
<updateRate>10.0</updateRate>
</plugin>
</gazebo>

</robot>
76 changes: 76 additions & 0 deletions orne_description/urdf/sensors/camera.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<joint name="camera_joint" type="fixed">
<origin xyz="0.20 0.0 0.620" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<xacro:macro name="usb_camera" params="prefix:=camera">
<!-- Create laser reference frame -->
<link name="${prefix}_link">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>

<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
<material name="black"/>
</visual>

<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>

<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>

</xacro:macro>
</robot>
12 changes: 6 additions & 6 deletions orne_navigation_executor/initial_pose_cfg/initial_pose_alpha.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
# initial pose config
initial_pose_x: 115.153953552
initial_pose_y: -65.9101486206
initial_pose_a: -0.08152333681588794
initial_cov_xx: 0.065
initial_cov_yy: 0.065
initial_cov_aa: 0.065
initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_a: 0.0
initial_cov_xx: 0.0
initial_cov_yy: 0.0
initial_cov_aa: 0.0
23 changes: 18 additions & 5 deletions orne_navigation_executor/launch/localization.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,25 @@
<arg name="robot_name" default="alpha"/>
<arg name="map_file" default="$(find orne_navigation_executor)/maps/mymap"/>
<arg name="init_pos_file" default="$(find orne_navigation_executor)/initial_pose_cfg/initial_pose.yaml"/>
<arg name="emcl" default="false"/>

<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file).yaml"/>

<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find orne_navigation_executor)/param/localization_params.yaml" command="load"/>
<rosparam file="$(find orne_navigation_executor)/param/localization_params_$(arg robot_name).yaml" command="load"/>
<rosparam file="$(arg init_pos_file)" command="load"/>
</node>
<!-- AMCL -->
<group unless="$(arg emcl)">
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find orne_navigation_executor)/param/localization_params.yaml" command="load"/>
<rosparam file="$(find orne_navigation_executor)/param/localization_params_$(arg robot_name).yaml" command="load"/>
<rosparam file="$(arg init_pos_file)" command="load"/>
</node>
</group>

<!-- EMCL -->
<group if="$(arg emcl)">
<node pkg="emcl" type="emcl_node" name="emcl_node" output="screen">
<rosparam file="$(find orne_navigation_executor)/param/emcl_params.yaml" command="load"/>
<rosparam file="$(arg init_pos_file)" command="load"/>
</node>
</group>

</launch>
2 changes: 2 additions & 0 deletions orne_navigation_executor/launch/nav_static_map.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
<arg name="map_file" default="$(find orne_navigation_executor)/maps/tsudanuma2-3/tsudanuma2-3"/>
<arg name="init_pos_file" default="$(find orne_navigation_executor)/initial_pose_cfg/initial_pose.yaml"/>
<arg name="odom_topic" default="combined_odom"/>
<arg name="emcl" default="false"/>

<include file="$(find orne_navigation_executor)/launch/move_base.launch">
<arg name="robot_name" value="$(arg robot_name)"/>
Expand All @@ -17,6 +18,7 @@
<arg name="robot_name" value="$(arg robot_name)"/>
<arg name="map_file" value="$(arg map_file)"/>
<arg name="init_pos_file" value="$(arg init_pos_file)"/>
<arg name="emcl" value="$(arg emcl)"/>
</include>

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find orne_navigation_executor)/rviz_cfg/nav.rviz"/>
Expand Down
2 changes: 2 additions & 0 deletions orne_navigation_executor/launch/play_waypoints_nav_alpha.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
<arg name="map_file" default="$(find orne_navigation_executor)/maps/mymap"/>
<arg name="init_pos_file" default="$(find orne_navigation_executor)/initial_pose_cfg/initial_pose_$(arg robot_name).yaml"/>
<arg name="waypoints_file" default="$(find orne_navigation_executor)/waypoints_cfg/waypoints_$(arg robot_name).yaml"/>
<arg name="emcl" default="true"/>

<arg name="suspend" default="False"/>
<param name="state" value="$(arg suspend)"/>
Expand All @@ -14,6 +15,7 @@
<arg name="map_file" value="$(arg map_file)"/>
<arg name="init_pos_file" value="$(arg init_pos_file)"/>
<arg name="waypoints_file" value="$(arg waypoints_file)"/>
<arg name="emcl" value="$(arg emcl)"/>
<arg name="suspend_file" value="$(arg suspend_file)"/>

</include>
Expand Down
4 changes: 3 additions & 1 deletion orne_navigation_executor/launch/play_waypoints_nav_common.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,14 @@
<arg name="init_pos_file" default="$(find orne_navigation_executor)/initial_pose_cfg/initial_pose_$(arg robot_name).yaml"/>
<arg name="waypoints_file" default="$(find orne_navigation_executor)/waypoints_cfg/waypoints_$(arg robot_name).yaml"/>
<arg name="dist_err" default="0.8"/>
<arg name="suspend_file" default="$(find orne_strategy)/suspend_cfg/suspend.yaml"/>
<arg name="suspend_file" default="$(find orne_strategy)/suspend_cfg/suspend.yaml"/>
<arg name="emcl" default="false"/>

<include file="$(find orne_navigation_executor)/launch/nav_static_map.launch">
<arg name="robot_name" value="$(arg robot_name)"/>
<arg name="map_file" value="$(arg map_file)"/>
<arg name="init_pos_file" value="$(arg init_pos_file)"/>
<arg name="emcl" value="$(arg emcl)"/>
</include>

<node name="waypoints_nav" pkg="fulanghua_waypoints_nav" type="waypoints_nav" output="screen">
Expand Down
4 changes: 2 additions & 2 deletions orne_navigation_executor/maps/mymap.pgm
View file
Open in desktop

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion orne_navigation_executor/maps/mymap_for_costmap.pgm
View file
Open in desktop

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion orne_navigation_executor/param/base_local_planner_params_alpha.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
TrajectoryPlannerROS:
holonomic_robot: false
max_vel_x: 0.9
min_vel_x: -0.4
min_vel_x: 0.1
max_vel_y: 0.0
min_vel_y: 0.0
acc_lim_y: 0.0
Expand Down
32 changes: 32 additions & 0 deletions orne_navigation_executor/param/emcl_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
odom_freq: 20
num_particles: 500

odom_frame_id: odom
footprint_frame_id: base_link
base_frame_id: base_link

initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_a: 0.0

odom_fw_dev_per_fw: 0.19
odom_fw_dev_per_rot: 0.0001
odom_rot_dev_per_fw: 0.13
odom_rot_dev_per_rot: 0.2

laser_likelihood_max_dist: 0.2

alpha_threshold: 0.4
open_space_threshold: 0.05

expansion_radius_position: 1.0
expansion_radius_orientation: 0.2

laser_min_range: 0.0
laser_max_range: 100000000.0

scan_increment: 1

#transform_tolerance: 0.5
#gui_publish_rate: 50.0
#laser_max_beams: 180
5 changes: 4 additions & 1 deletion orne_pkgs.install
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,7 @@
uri: https://github.com/open-rdc/adi_driver
local-name: adi_driver
version: master

- git:
uri: https://github.com/open-rdc/emcl.git
local-name: emcl
version: master