删除无用的launch文件，删除frame中的/，修改膨胀层系数

.gitignore

@@ -138,3 +138,5 @@ target/
 *.out
 *.app
 
+.vscode/
+

README.md

@@ -1,4 +1,4 @@
-# Copyright: www.corvin.cn ROS小课堂 2016-2019
+# Copyright: www.corvin.cn ROS小课堂 2016-2021
 # Author: corvin
 # Description:
      本代码仓库主要存储STDR仿真器相关测试代码，主要用于学习如何通过
@@ -39,7 +39,7 @@
 
    2.需要测试各SLAM算法，可以查看如下各操作：
      (1):需要测试gmapping建图，执行如下命令：
-         roslaunch stdr_gmapping stdr_gmapping.launch
+         roslaunch stdr_gmapping stdr_gmapping_keyboard.launch
 
          需要使用键盘来遥控机器人四处走动，执行以下命令：
          rosrun teleop_twist_keyboard teleop_twist_keyboard.py
@@ -50,10 +50,7 @@
          需要使用键盘来遥控机器人四处走动，执行以下命令：
          rosrun teleop_twist_keyboard teleop_twist_keyboard.py
 
-     (3):需要测试amcl定位自动导航，执行如下命令：
-         roslaunch stdr_navigation amcl_movebase_nav.launch
-
-     (4):想要实现定点巡航仿真的话，执行如下命令：
+     (3):想要实现定点巡航仿真的话，执行如下命令：
          roslaunch stdr_navigation patrol_nav.launch
 
    3.需要测试超声波避障策略：
@@ -67,3 +64,4 @@
     20180806: 新增可以定点巡航的测试代码patrol_nav.launch,patrol_nav.py.
     20180903: 新增install.sh,用于安装一些ros软件包,这样才能正常编译和使用该仿真.
     20181220: 新增了可以加载其他地图启动的launch文件,位于stdr_launch文件夹中.
+    20210326: 删除无用的launch文件，删除frame中的/，修改膨胀层系数.

scripts/setup_env.sh

@@ -1,12 +1,13 @@
 #!/bin/bash
 
-# Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+# Copyright: 2016-2021 ROS小课堂 www.corvin.cn
 # Author: corvin
 # Description:该脚本是为了安装一些必要软件包,这样才能正常运行软件包中的
 #   各个实例代码.该脚本只需要执行一次即可.
 # History:
 #   20181220:Initial this bash file.
 #   20190102:在安装upgrade时,增加一个判断,必须等命令执行成功才继续往下运行.
+#   20210326:stdr需要qt4-default支持.
 
 red="\e[31m"
 normal="\e[0m"
@@ -35,6 +36,7 @@ sudo apt install -y ros-$ROS_DISTRO-dwa-local-planner ros-$ROS_DISTRO-global-pla
 sudo apt install -y ros-$ROS_DISTRO-gmapping ros-$ROS_DISTRO-hector-mapping
 sudo apt install -y ros-$ROS_DISTRO-costmap-2d ros-$ROS_DISTRO-hector-nav-msgs
 sudo apt install -y ros-$ROS_DISTRO-map-server ros-$ROS_DISTRO-nav-core
+sudo apt install -y qt4-default
 sudo apt-get -y autoremove
 
 # second catkin_make worksapce

src/stdr_amcl/launch/home_map_amcl.launch

@@ -1,11 +1,12 @@
 <!--
-  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     启动amcl节点，这样就可以在全局地图上对机器人进行定位了。
   History:
     20180528: initial this file.
     20190102: 删除加载地图的launch文件,在自动导航时来启动加载地图的launch文件.
+    20210326: 删除frame中的/.
 -->
 <launch>
   <!-- startup rviz node to show amcl process -->
@@ -62,9 +63,9 @@
     <param name="odom_alpha3"     value="0.03"/>
     <param name="odom_alpha4"     value="0.03"/>
     <param name="odom_alpha5"     value="0.0"/> <!--only used if model is "omni"-->
-    <param name="odom_frame_id"   value="/map_static"/>
-    <param name="base_frame_id"   value="/robot0"/>
-    <param name="global_frame_id" value="/map"/>
+    <param name="odom_frame_id"   value="map_static"/>
+    <param name="base_frame_id"   value="robot0"/>
+    <param name="global_frame_id" value="map"/>
     <param name="tf_broadcast"    value="true"/>
   </node>
 </launch>

src/stdr_amcl/launch/stdr_amcl.launch

@@ -1,11 +1,12 @@
 <!--
-  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     启动amcl节点，这样就可以在全局地图上对机器人进行定位了。
   History:
     20180528: initial this file.
     20190102: 删除加载地图的launch文件,在自动导航时来启动加载地图的launch文件.
+    20210326: 删除frame中的/.
 -->
 <launch>
   <!-- startup rviz node to show amcl process -->
@@ -62,9 +63,9 @@
     <param name="odom_alpha3"     value="0.03"/>
     <param name="odom_alpha4"     value="0.03"/>
     <param name="odom_alpha5"     value="0.0"/> <!--only used if model is "omni"-->
-    <param name="odom_frame_id"   value="/map_static"/>
-    <param name="base_frame_id"   value="/robot0"/>
-    <param name="global_frame_id" value="/map"/>
+    <param name="odom_frame_id"   value="map_static"/>
+    <param name="base_frame_id"   value="robot0"/>
+    <param name="global_frame_id" value="map"/>
     <param name="tf_broadcast"    value="true"/>
   </node>
 </launch>

src/stdr_gmapping/launch/gmapping_keyboard_home_map.launch

@@ -1,16 +1,17 @@
 <!--
-  Copyright(c): 2016-2018 ROS小课堂 www.corvin.cn
+  Copyright(c): 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     使用stdr的仿真robot下的laser来进行gmapping构图,在这里使用的是/robot0/laser_0,
     加载的地图是home.yaml。
   History:
     20181220: initial this file.
+    20210326: 删除frame中的/.
 -->
 <launch>
-  <arg name="base_frame" default="/robot0" />
-  <arg name="map_frame" default="/map" />
-  <arg name="odom_frame" default="/map_static" />
+  <arg name="base_frame" default="robot0" />
+  <arg name="map_frame" default="map" />
+  <arg name="odom_frame" default="map_static" />
 
   <!-- startup stdr simulator with robot0 -->
   <include file="$(find stdr_launchers)/launch/home_map_robot_gui.launch" />

src/stdr_gmapping/launch/stdr_gmapping_auto_explor.launch

@@ -1,78 +0,0 @@
-<!--
-  Copyright(c): 2016-2018 ROS小课堂 www.corvin.cn
-  Author: corvin
-  Description:
-    使用stdr的仿真robot下的laser来进行gmapping构图,在这里使用的是/robot0/laser_0,
-    然后使用hector_exploration_planner来自动探索未知区域来自动建图，不要键盘来遥控
-    机器人四处移动了。
-  History:
-    20180611: initial this file.
--->
-<launch>
-  <arg name="base_frame" default="/robot0" />
-  <arg name="map_frame"  default="/map" />
-  <arg name="odom_frame" default="/map_static" />
-
-  <!-- startup stdr simulator with robot0 -->
-  <include file="$(find stdr_launchers)/launch/server_with_map_and_gui_plus_robot.launch" />
-
-  <!-- startup rviz to show gmapping process -->
-  <node name="rviz" type="rviz" pkg="rviz" args="-d $(find stdr_gmapping)/rviz/gmapping_explor_config.rviz" />
-
-  <!-- startup slam_gmapping node -->
-  <node pkg="gmapping" type="slam_gmapping" name="stdr_gmapping_node" output="screen">
-      <remap from="scan" to="/robot0/laser_0" />
-      <remap from="map"  to="/gmapping/map" />
-
-      <param name="throttle_scans" value="1" />
-      <param name="base_frame" value="$(arg base_frame)"/>
-      <param name="map_frame"  value="$(arg map_frame)"/>
-      <param name="odom_frame" value="$(arg odom_frame)"/>
-      <param name="map_update_interval" value="3.0"/>
-      <!-- set maxUrange < maximum range of the real sensor <= maxRange -->
-      <param name="maxUrange" value="3.9"/>
-      <param name="maxRange"  value="4.0"/>
-      <param name="sigma" value="0.05"/>
-      <param name="kernelSize" value="1"/>
-      <param name="lstep" value="0.05"/>
-      <param name="astep" value="0.05"/>
-      <param name="iterations" value="5"/>
-      <param name="lsigma" value="0.075"/>
-      <param name="ogain"  value="3.0"/>
-      <param name="lskip"  value="0"/>
-      <param name="minimumScore" value="80"/>
-
-      <param name="srr" value="0.1"/>
-      <param name="srt" value="0.2"/>
-      <param name="str" value="0.1"/>
-      <param name="stt" value="0.2"/>
-
-      <param name="linearUpdate" value="1.0"/>
-      <param name="angularUpdate" value="0.5"/>
-      <param name="temporalUpdate" value="4.0"/>
-      <param name="resampleThreshold" value="0.5"/>
-      <param name="particles" value="30"/>
-
-      <param name="xmin"  value="-7.0" />
-      <param name="ymin"  value="-7.0" />
-      <param name="xmax"  value="7.0" />
-      <param name="ymax"  value="7.0" />
-      <param name="delta" value="0.05" />
-
-      <param name="llsamplerange" value="0.01" />
-      <param name="llsamplestep"  value="0.01" />
-      <param name="lasamplerange" value="0.005" />
-      <param name="lasamplestep"  value="0.005" />
-
-      <param name="transform_publish_period"  value="0.05" />
-      <param name="occ_thresh" value="0.25" />
-  </node>
-
-  <!-- startup hector_exploration_node -->
-  <include file="$(find hector_exploration_node)/launch/exploration_planner.launch" />
-
-  <!-- rosrun hector_exploration_control -->
-  <node pkg="hector_exploration_controller" type="simple_exploration_controller" name="gmapping_exploration_node" output="screen" />
-
-</launch>
-

src/stdr_gmapping/launch/stdr_gmapping_keyboard.launch

@@ -1,15 +1,16 @@
 <!--
-  Copyright(c): 2016-2018 ROS小课堂 www.corvin.cn
+  Copyright(c): 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     使用stdr的仿真robot下的laser来进行gmapping构图,在这里使用的是/robot0/laser_0。
   History:
     20180523: initial this file.
+    20210326: 删除frame中的/.
 -->
 <launch>
-  <arg name="base_frame" default="/robot0" />
-  <arg name="map_frame" default="/map" />
-  <arg name="odom_frame" default="/map_static" />
+  <arg name="base_frame" default="robot0" />
+  <arg name="map_frame" default="map" />
+  <arg name="odom_frame" default="map_static" />
 
   <!-- startup stdr simulator with robot0 -->
   <include file="$(find stdr_launchers)/launch/server_with_map_and_gui_plus_robot.launch" />

src/stdr_hector_mapping/launch/home_map_hector_mapping.launch

@@ -1,16 +1,17 @@
 <!--
-  Copyright: 2016-2018 www.corvin.cn
+  Copyright: 2016-2021 www.corvin.cn
   Author: corvin
   Description: In stdr simulator hector mapping,load home map to test.
   History:
     20181220: initial this file.
+    20210326: 删除frame中的/.
 -->
 <launch>
   <arg name="map_scanmatch_frame_name" default="scanmatcher_frame"/>
   <arg name="pub_map_odom_transform"   default="true"/>
   <arg name="scan_queue_size"          default="10"/>
-  <arg name="base_frame" default="/robot0" />
-  <arg name="odom_frame" default="/robot0" />
+  <arg name="base_frame" default="robot0" />
+  <arg name="odom_frame" default="robot0" />
   <arg name="scan_topic" default="/robot0/laser_0" />
   <arg name="map_topic"  default="/hector_map" />
   <arg name="map_size"   default="1200" />
@@ -25,7 +26,7 @@
     <remap from="map"  to="$(arg map_topic)"  />
 
     <!-- Frame names -->
-    <param name="map_frame"  value="/map_static" />
+    <param name="map_frame"  value="map_static" />
     <param name="base_frame" value="$(arg base_frame)" />
     <param name="odom_frame" value="$(arg odom_frame)" />
 

src/stdr_hector_mapping/launch/stdr_hector_mapping.launch

@@ -1,16 +1,17 @@
 <!--
-  Copyright: 2016-2018 www.corvin.cn
+  Copyright: 2016-2021 www.corvin.cn
   Author: corvin
   Description: In stdr simulator hector mapping.
   History:
     20180601: initial this file.
+    20210326: 删除frame中的/.
 -->
 <launch>
   <arg name="map_scanmatch_frame_name" default="scanmatcher_frame"/>
   <arg name="pub_map_odom_transform"   default="true"/>
   <arg name="scan_queue_size"          default="10"/>
-  <arg name="base_frame" default="/robot0" />
-  <arg name="odom_frame" default="/robot0" />
+  <arg name="base_frame" default="robot0" />
+  <arg name="odom_frame" default="robot0" />
   <arg name="scan_topic" default="/robot0/laser_0" />
   <arg name="map_topic"  default="/hector_map" />
   <arg name="map_size"   default="512" />
@@ -25,7 +26,7 @@
     <remap from="map"  to="$(arg map_topic)"  />
 
     <!-- Frame names -->
-    <param name="map_frame"  value="/map_static" />
+    <param name="map_frame"  value="map_static" />
     <param name="base_frame" value="$(arg base_frame)" />
     <param name="odom_frame" value="$(arg odom_frame)" />
 

src/stdr_move_base/config/costmap_common_params.yaml

@@ -1,5 +1,5 @@
 #FileName: costmap_common_params.yaml
-#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Copyright: 2016-2021 ROS小课堂 www.corvin.cn
 #Author: corvin
 #Description:
 # 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
@@ -8,6 +8,7 @@
 #
 #History:
 # 20180613: initial this file.
+# 20210326: 更改膨胀层参数.
 
 robot_radius: 0.2
 
@@ -19,7 +20,7 @@ obstacle_layer:
   raytrace_range: 3.0
   observation_sources: laser_scan_sensor
   laser_scan_sensor: {
-    sensor_frame: /robot0_laser_0,
+    sensor_frame: robot0_laser_0,
     data_type: LaserScan,
     topic: /robot0/laser_0,
     marking: true,
@@ -29,7 +30,7 @@ obstacle_layer:
 inflation_layer:
   enabled: true
   cost_scaling_factor: 5.0
-  inflation_radius: 0.36
+  inflation_radius: 0.2
 
 static_layer:
   enabled: true

src/stdr_move_base/config/dwa_local_planner_params.yaml

@@ -1,5 +1,5 @@
 #FileName: dwa_local_planner_params.yaml
-#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Copyright: 2016-2021 ROS小课堂 www.corvin.cn
 #Author: corvin
 #Description:
 #  dwa_local_planner提供一个能够驱动底座的控制器,该控制器连接了路径规划器和机器人.
@@ -87,7 +87,7 @@ DWAPlannerROS:
   latch_xy_goal_tolerance: false
 
 # Forward Simulation Parameters
-  sim_time: 2.0    # 1.7
+  sim_time: 3.0    # 1.7
   sim_granularity: 0.025
   vx_samples: 6    # default 3
   vy_samples: 1    # diff drive robot, there is only one sample
@@ -95,8 +95,8 @@ DWAPlannerROS:
   controller_frequency: 5.0
 
 # Trajectory Scoring Parameters
-  path_distance_bias: 52.0  # 32.0  -weighting for how much it should stick to the global path plan
-  goal_distance_bias: 12.0  # 24.0  -wighting for how much it should attempt to reach its goal
+  path_distance_bias: 32.0  # 32.0  -weighting for how much it should stick to the global path plan
+  goal_distance_bias: 30.0  # 24.0  -wighting for how much it should attempt to reach its goal
   occdist_scale: 0.3        # 0.01  -weighting for how much the controller should avoid obstacles
   forward_point_distance: 0.325 # 0.325 -how far along to place an additional scoring point
   stop_time_buffer: 0.2     # 0.2   -amount of time a robot must stop in before colliding for a valid traj.

src/stdr_move_base/config/global_costmap_params.yaml

@@ -1,5 +1,5 @@
 #FileName: global_costmap_params.yaml
-#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Copyright: 2016-2021 ROS小课堂 www.corvin.cn
 #Author: corvin
 #Description:
 #  全局代价地图参数配置文件,各参数的意义如下：
@@ -9,8 +9,8 @@
 #History:
 #  20180613: initial this file.
 global_costmap:
-  global_frame: /map
-  robot_base_frame: /robot0
+  global_frame: map
+  robot_base_frame: robot0
   update_frequency: 0.5
   static_map: true
   rolling_window: false

src/stdr_move_base/config/global_planner_params.yaml

@@ -1,5 +1,5 @@
 #FileName: global_planner_params.yaml
-#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Copyright: 2016-2021 ROS小课堂 www.corvin.cn
 #Author: corvin
 #Description:
 #  这个包为导航实现了一种快速,内插值的全局路径规划器,

src/stdr_move_base/config/local_costmap_params.yaml

@@ -1,5 +1,5 @@
 #FileName: local_costmap_params.yaml
-#Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#Copyright: 2016-2021 ROS小课堂 www.corvin.cn
 #Author: corvin
 #Description:
 #  本地代价地图需要配置的参数，各参数意义如下:
@@ -10,8 +10,8 @@
 #  20180613: initial this file.
 
 local_costmap:
-  global_frame: /map_static
-  robot_base_frame: /robot0
+  global_frame: map_static
+  robot_base_frame: robot0
   update_frequency: 5.0
   publish_frequency: 3.0
   static_map: false

src/stdr_move_base/config/move_base_params.yaml

@@ -1,5 +1,5 @@
 #FileName: move_base_params.yaml
-#Copyright: 2016-2018 ROS小课堂www.corvin.cn
+#Copyright: 2016-2021 ROS小课堂www.corvin.cn
 #Author: corvin
 #Description:
 # move_base软件包的通用配置参数,现在依次解释每个参数意义:

src/stdr_move_base/launch/stdr_move_base.launch

@@ -1,17 +1,18 @@
 <!--
   FileName: stdr_move_base.launch
-  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     启动move_base节点，加载各个配置文件。
   History:
     20180528: initial this file.
     20180530: add arg param to rename all kinds of frames,topics,add load params file.
+    20210326: 删除frame中的/.
 -->
 <launch>
-  <arg name="odom_frame_id"   default="/map_static"/>
-  <arg name="base_frame_id"   default="/robot0"/>
-  <arg name="global_frame_id" default="/map"/>
+  <arg name="odom_frame_id"   default="map_static"/>
+  <arg name="base_frame_id"   default="robot0"/>
+  <arg name="global_frame_id" default="map"/>
 
   <arg name="odom_topic"      default="/robot0/odom"/>
   <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>

src/stdr_navigation/launch/gmapping_map_nav.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2018: www.corvin.cn
+  Copyright: 2016-2021: www.corvin.cn
   Author: corvin
   Description:
     STDR simulator robot auto navigation with amcl and move base packages.

src/stdr_navigation/launch/hector_map_nav.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     STDR simulator robot auto navigation with amcl and move base packages.

src/stdr_navigation/launch/home_map_nav.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description:
     STDR simulator robot auto navigation with amcl and move base packages.

src/stdr_navigation/launch/load_gmapping_map.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description: 该启动文件用于加载gmapping建立的地图,这样就可以进行自动导航了.
   History:

src/stdr_navigation/launch/load_hector_map.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description: 该启动文件用于加载hector_mapping建立的地图,这样就可以进行自动导航了.
   History:

src/stdr_navigation/launch/load_home_map.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Copyright: 2016-2021 ROS小课堂 www.corvin.cn
   Author: corvin
   Description: 该启动文件用于加载自定义的home地图,这样就可以进行自动导航了.
   History:

src/stdr_navigation/launch/patrol_nav.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2018: www.corvin.cn
+  Copyright: 2016-2021: www.corvin.cn
   Author: corvin
   Description:
         启动move_base和amcl来进行自动导航,然后启动patrol_nav_node,该节点可以

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -66,70 +66,70 @@ speedBindings={
 	      }
 
 def getKey():
-	tty.setraw(sys.stdin.fileno())
-	select.select([sys.stdin], [], [], 0)
-	key = sys.stdin.read(1)
-	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
-	return key
+    tty.setraw(sys.stdin.fileno())
+    select.select([sys.stdin], [], [], 0)
+    key = sys.stdin.read(1)
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    return key
 
 
 def vels(speed,turn):
-	return "currently:\tspeed %s\tturn %s " % (speed,turn)
+    return "currently:\tspeed %s\tturn %s " % (speed,turn)
 
 if __name__=="__main__":
-    	settings = termios.tcgetattr(sys.stdin)
-
-	pub = rospy.Publisher('/robot0/cmd_vel', Twist, queue_size = 2)
-	rospy.init_node('teleop_twist_keyboard')
-
-	speed = rospy.get_param("~speed", 0.25)
-	turn = rospy.get_param("~turn", 0.4)
-	x = 0
-	y = 0
-	z = 0
-	th = 0
-	status = 0
-
-	try:
-		print(msg)
-		print(vels(speed,turn))
-		while(1):
-			key = getKey()
-			if key in moveBindings.keys():
-				x = moveBindings[key][0]
-				y = moveBindings[key][1]
-				z = moveBindings[key][2]
-				th = moveBindings[key][3]
-			elif key in speedBindings.keys():
-				speed = speed * speedBindings[key][0]
-				turn = turn * speedBindings[key][1]
-
-				print(vels(speed,turn))
-				if (status == 14):
-					print(msg)
-				status = (status + 1) % 15
-			else:
-				x = 0
-				y = 0
-				z = 0
-				th = 0
-				if (key == '\x03'):
-					break
-
-			twist = Twist()
-			twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
-			twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
-			pub.publish(twist)
-
-	except Exception as e:
-		print(e)
-
-	finally:
-		twist = Twist()
-		twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
-		twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
-		pub.publish(twist)
-
-    		termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    settings = termios.tcgetattr(sys.stdin)
+
+    pub = rospy.Publisher('/robot0/cmd_vel', Twist, queue_size = 2)
+    rospy.init_node('teleop_twist_keyboard')
+
+    speed = rospy.get_param("~speed", 0.25)
+    turn = rospy.get_param("~turn", 0.4)
+    x = 0
+    y = 0
+    z = 0
+    th = 0
+    status = 0
+
+    try:
+        print(msg)
+        print(vels(speed,turn))
+        while(1):
+            key = getKey()
+            if key in moveBindings.keys():
+                x = moveBindings[key][0]
+                y = moveBindings[key][1]
+                z = moveBindings[key][2]
+                th = moveBindings[key][3]
+            elif key in speedBindings.keys():
+                speed = speed * speedBindings[key][0]
+                turn = turn * speedBindings[key][1]
+
+                print(vels(speed,turn))
+                if (status == 14):
+                    print(msg)
+                status = (status + 1) % 15
+            else:
+                x = 0
+                y = 0
+                z = 0
+                th = 0
+            if (key == '\x03'):
+                break
+
+            twist = Twist()
+            twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
+            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
+            pub.publish(twist)
+
+    except Exception as e:
+        print(e)
+
+    finally:
+        twist = Twist()
+        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
+        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
+        pub.publish(twist)
+
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
 
 

src/ultrasonic_obstacle_avoidance/launch/ultrasonic_avoidance.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2018 www.corvin.cn
+  Copyright: 2016-2021 www.corvin.cn
   Author: corvin
   Description:
     stdr simulator with robot0 ultrasonic .