From e7e26ec11da5aa84f199aa3aad1199641021b404 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Wed, 15 Jan 2025 17:54:07 +0100
Subject: [PATCH] update

---
 config/box_filter.yaml       |   2 +-
 launch/move_base.launch      |  65 +++++++
 launch/nav_rviz_view.launch  |   8 +
 launch/robot_pose_ekf.launch |  12 ++
 test.urdf                    | 326 -----------------------------------
 5 files changed, 86 insertions(+), 327 deletions(-)
 create mode 100644 launch/move_base.launch
 create mode 100644 launch/nav_rviz_view.launch
 create mode 100644 launch/robot_pose_ekf.launch
 delete mode 100644 test.urdf

diff --git a/config/box_filter.yaml b/config/box_filter.yaml
index d4c184d..7837058 100644
--- a/config/box_filter.yaml
+++ b/config/box_filter.yaml
@@ -4,7 +4,7 @@ scan_to_scan_filter_chain:
       name: box_filter
       type: laser_filters/LaserScanBoxFilter
       params:
-        box_frame: laser_frame
+        box_frame: laser #laser_frame
         max_x: 0.80 #was 0.16
         max_y: 0.18 #was 0.17
         max_z: 0.1
diff --git a/launch/move_base.launch b/launch/move_base.launch
new file mode 100644
index 0000000..dcaf5a5
--- /dev/null
+++ b/launch/move_base.launch
@@ -0,0 +1,65 @@
+<launch>
+  <param name="use_sim_time" value="false" />
+
+  <!--<include file="$(find segwayrmp)/launch/segwayrmp_configuration.launch" />-->
+
+  <!-- Run the map server -->
+  <!--<node name="map_server" pkg="map_server" type="map_server" args="$(find segwayrmp)/maps/mymap.pgm my_map_resolution"/>-->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find segwayrmp)/maps/mymap.yaml"/>
+
+  <!--move base-->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find segwayrmp)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find segwayrmp)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find segwayrmp)/param/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find segwayrmp)/param/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find segwayrmp)/param/base_local_planner_params.yaml" command="load" />
+  </node>
+
+   <!--启动 AMCL 节点 -->
+  <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
+    <param name="use_map_topic" value="false"/>
+    <!-- Publish scans from best pose at a max of 10 Hz -->
+    <param name="odom_model_type" value="diff"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="gui_publish_rate" value="10.0"/>
+    <param name="laser_max_beams" value="60"/>
+    <param name="laser_max_range" value="12.0"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="2000"/>
+    <param name="kld_err" value="0.05"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <!-- translation std dev, m -->
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="laser_z_hit" value="0.5"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.5"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <!-- <param name="laser_model_type" value="beam"/> -->
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.25"/>
+    <param name="update_min_a" value="0.2"/>
+
+    <param name="odom_frame_id" value="odom"/>
+
+    <param name="resample_interval" value="1"/>
+    <!-- Increase tolerance because the computer can get quite busy -->
+    <param name="transform_tolerance" value="1.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+    <!-- scan topic -->
+    <remap from="scan" to="scan"/>
+  </node>
+
+  <!-- TF -->
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.10 0.0 0.12 3.14 0.0 0.0 base_link laser 100"/>
+  <!-- static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  
+  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_link base_footprint 50" /> -->
+
+</launch>
\ No newline at end of file
diff --git a/launch/nav_rviz_view.launch b/launch/nav_rviz_view.launch
new file mode 100644
index 0000000..c8796dc
--- /dev/null
+++ b/launch/nav_rviz_view.launch
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <!--start rviz view -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find segwayrmp)/rviz/nav.rviz" />
+
+</launch>
\ No newline at end of file
diff --git a/launch/robot_pose_ekf.launch b/launch/robot_pose_ekf.launch
new file mode 100644
index 0000000..81ef665
--- /dev/null
+++ b/launch/robot_pose_ekf.launch
@@ -0,0 +1,12 @@
+<launch>
+ <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+    <param name="output_frame" value="odom"/>
+    <param name="freq" value="30.0"/>
+    <param name="sensor_timeout" value="1.0"/>
+    <param name="odom_used" value="true"/>
+    <param name="imu_used" value="true"/>
+    <param name="vo_used" value="false"/>
+    <param name="debug" value="false"/>
+    <param name="self_diagnose" value="false"/>
+  </node>
+</launch>
\ No newline at end of file
diff --git a/test.urdf b/test.urdf
deleted file mode 100644
index d5f4982..0000000
--- a/test.urdf
+++ /dev/null
@@ -1,326 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from description/robot.urdf.xacro   | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="robot">
-  <!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
-  <!-- These make use of xacro's mathematical functionality -->
-  <material name="white">
-    <color rgba="1 1 1 1"/>
-  </material>
-  <material name="orange">
-    <color rgba="1 0.3 0.1 1"/>
-  </material>
-  <material name="blue">
-    <color rgba="0.2 0.2 1 1"/>
-  </material>
-  <material name="black">
-    <color rgba="0 0 0 1"/>
-  </material>
-  <material name="red">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <material name="green">
-    <color rgba="0.0 0.8 0.0 1.0"/>
-  </material>
-  <material name="grey">
-    <color rgba="0.6 0.6 0.6 1.0"/>
-  </material>
-  <material name="grey2">
-    <color rgba="0.9 0.9 0.9 1.0"/>
-  </material>
-  <material name="lightGrey">
-    <color rgba="0.6 0.6 0.6 1.0"/>
-  </material>
-  <!-- BASE LINK -->
-  <link name="base_link">
-    </link>
-  <!-- BASE_FOOTPRINT LINK -->
-  <joint name="base_footprint_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="base_footprint"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint>
-  <link name="base_footprint">
-    </link>
-  <!-- CHASSIS LINK -->
-  <joint name="chassis_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="chassis"/>
-    <origin xyz="-0.22999999999999998 0 0.01"/>
-  </joint>
-  <link name="chassis">
-    <visual>
-      <origin xyz="0 0 0"/>
-      <geometry>
-        <box size="0.6 0.33 0.15"/>
-      </geometry>
-      <material name="orange"/>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0"/>
-      <geometry>
-        <box size="0.6 0.33 0.15"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.5"/>
-      <inertia ixx="0.005475000000000001" ixy="0.0" ixz="0.0" iyy="0.0159375" iyz="0.0" izz="0.0195375"/>
-    </inertial>
-  </link>
-  <gazebo reference="chassis">
-    <material>Gazebo/Orange</material>
-  </gazebo>
-  <!-- LEFT WHEEL LINK -->
-  <joint name="left_wheel_joint" type="continuous">
-    <parent link="chassis"/>
-    <child link="left_wheel"/>
-    <origin rpy="-1.5707963267948966 0 0" xyz="0.22999999999999998 0.218375 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="left_wheel">
-    <visual>
-      <geometry>
-        <cylinder length="0.08" radius="0.135"/>
-      </geometry>
-      <material name="blue"/>
-    </visual>
-    <collision>
-      <geometry>
-        <sphere radius="0.135"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="4.0"/>
-      <inertia ixx="0.020358333333333336" ixy="0.0" ixz="0.0" iyy="0.020358333333333336" iyz="0.0" izz="0.03645"/>
-    </inertial>
-  </link>
-  <gazebo reference="left_wheel">
-    <material>Gazebo/Blue</material>
-  </gazebo>
-  <!-- RIGHT WHEEL LINK -->
-  <joint name="right_wheel_joint" type="continuous">
-    <parent link="chassis"/>
-    <child link="right_wheel"/>
-    <origin rpy="1.5707963267948966 0 0" xyz="0.22999999999999998 -0.218375 0"/>
-    <axis xyz="0 0 -1"/>
-  </joint>
-  <link name="right_wheel">
-    <visual>
-      <geometry>
-        <cylinder length="0.08" radius="0.135"/>
-      </geometry>
-      <material name="blue"/>
-    </visual>
-    <collision>
-      <geometry>
-        <sphere radius="0.135"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="4.0"/>
-      <inertia ixx="0.020358333333333336" ixy="0.0" ixz="0.0" iyy="0.020358333333333336" iyz="0.0" izz="0.03645"/>
-    </inertial>
-  </link>
-  <gazebo reference="right_wheel">
-    <material>Gazebo/Blue</material>
-  </gazebo>
-  <!-- CASTER WHEEL LINK -->
-  <joint name="caster_wheel_joint" type="fixed">
-    <parent link="chassis"/>
-    <child link="caster_wheel"/>
-    <origin xyz="-0.15 0 -0.115"/>
-  </joint>
-  <link name="caster_wheel">
-    <visual>
-      <geometry>
-        <sphere radius="0.01"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <geometry>
-        <sphere radius="0.01"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.01"/>
-      <inertia ixx="4.0000000000000003e-07" ixy="0.0" ixz="0.0" iyy="4.0000000000000003e-07" iyz="0.0" izz="4.0000000000000003e-07"/>
-    </inertial>
-  </link>
-  <gazebo reference="caster_wheel">
-    <material>Gazebo/White</material>
-    <mu1 value="0.001"/>
-    <mu2 value="0.001"/>
-  </gazebo>
-  <!-- Lidar LINK -->
-  <!-- <joint name="lidar_joint" type="fixed">
-        <parent link="chassis"/>
-        <child link="laser_frame"/>
-        <origin xyz="${chassis_length/2 -0.150} 0 ${chassis_height + 0.030}" rpy="0 0 0"/>
-    </joint>
-
-
-    <link name="laser_frame">
-        <visual>
-            <geometry>
-                <sphere radius="${caster_wheel_radius}"/>
-            </geometry>
-            <material name="white"/>
-        </visual>
-        <collision>
-            <geometry>
-                <sphere radius="${caster_wheel_radius}"/>
-            </geometry>
-        </collision>
-        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
-            <origin xyz="0 0 0" rpy="0 0 0"/>
-        </xacro:inertial_sphere>
-    </link> -->
-  <joint name="lidar_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="laser_frame"/>
-    <origin rpy="0 0 3.141592653589793" xyz="-0.08499999999999999 0 0.18"/>
-  </joint>
-  <link name="laser_frame">
-    <visual>
-      <geometry>
-        <sphere radius="0.01"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <geometry>
-        <sphere radius="0.01"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.01"/>
-      <inertia ixx="4.0000000000000003e-07" ixy="0.0" ixz="0.0" iyy="4.0000000000000003e-07" iyz="0.0" izz="4.0000000000000003e-07"/>
-    </inertial>
-  </link>
-  <gazebo reference="laser_frame">
-    <material>Gazebo/White</material>
-  </gazebo>
-  <ros2_control name="RealRobot" type="system">
-    <hardware>
-      <plugin>diffdrive_arduino/DiffDriveArduino</plugin>
-      <param name="left_wheel_name">left_wheel_joint</param>
-      <param name="right_wheel_name">right_wheel_joint</param>
-      <param name="loop_rate">30</param>
-      <param name="device">/dev/ttyUSB0</param>
-      <param name="baud_rate">57600</param>
-      <param name="timeout">1000</param>
-      <param name="enc_counts_per_rev">3436</param>
-    </hardware>
-    <joint name="left_wheel_joint">
-      <command_interface name="velocity">
-        <param name="min">-10</param>
-        <param name="max">10</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="position"/>
-    </joint>
-    <joint name="right_wheel_joint">
-      <command_interface name="velocity">
-        <param name="min">-10</param>
-        <param name="max">10</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="position"/>
-    </joint>
-  </ros2_control>
-  <gazebo>
-    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-      <parameters>/home/bjorn/Documents/ros_projects/cps_rmp220_support/install/cps_rmp220_support/share/cps_rmp220_support/config/my_controllers.yaml</parameters>
-      <parameters>/home/bjorn/Documents/ros_projects/cps_rmp220_support/install/cps_rmp220_support/share/cps_rmp220_support/config/gaz_ros2_ctl_use_sim.yaml</parameters>
-    </plugin>
-  </gazebo>
-  <joint name="camera_joint" type="fixed">
-    <parent link="chassis"/>
-    <child link="camera_link"/>
-    <origin rpy="0 0 0" xyz="0.276 0 0.181"/>
-  </joint>
-  <link name="camera_link">
-    <visual>
-      <geometry>
-        <box size="0.010 0.03 0.03"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <visual>
-      <origin xyz="0 0 -0.05"/>
-      <geometry>
-        <cylinder length="0.1" radius="0.002"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-  </link>
-  <joint name="camera_optical_joint" type="fixed">
-    <parent link="camera_link"/>
-    <child link="camera_link_optical"/>
-    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
-  </joint>
-  <link name="camera_link_optical"/>
-  <gazebo reference="camera_link">
-    <material>Gazebo/Black</material>
-    <sensor name="camera" type="camera">
-      <pose> 0 0 0 0 0 0 </pose>
-      <visualize>true</visualize>
-      <update_rate>10</update_rate>
-      <camera>
-        <horizontal_fov>1.089</horizontal_fov>
-        <image>
-          <format>R8G8B8</format>
-          <width>640</width>
-          <height>480</height>
-        </image>
-        <clip>
-          <near>0.05</near>
-          <far>8.0</far>
-        </clip>
-      </camera>
-      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
-        <frame_name>camera_link_optical</frame_name>
-      </plugin>
-    </sensor>
-  </gazebo>
-  <!-- <xacro:include filename="depth_camera.xacro" /> -->
-  <joint name="face_joint" type="fixed">
-    <parent link="chassis"/>
-    <child link="face_link"/>
-    <origin rpy="0 0 0" xyz="0.3 0 0.075"/>
-  </joint>
-  <link name="face_link">
-    <visual>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0.05 0.01"/>
-      <geometry>
-        <cylinder length="0.002" radius="0.01"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <visual>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 -0.05 0.01"/>
-      <geometry>
-        <cylinder length="0.002" radius="0.01"/>
-      </geometry>
-      <material name="black"/>
-    </visual>
-    <visual>
-      <origin rpy="0 1.5 0" xyz="-0.011 0 -0.00"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.05"/>
-      </geometry>
-      <material name="red"/>
-    </visual>
-  </link>
-  <gazebo reference="face_link">
-    <material>Gazebo/Black</material>
-  </gazebo>
-</robot>