Author: admin

roslaunch halc0311_simulation turtlebot3_gazebo_halc.launch

rosrun halc0311_simulation turtlebot_move_spawn_write.py (this program is somehow so slow)

Thinking a better way to generate the waypoints…..

#1 run gazebo of simulation map
roslaunch halc0311_simulation turtlebot3_gazebo_halc.launch

#2 run amcl navigation (rviz) reading a scan map
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/catkin_ws/src/halc0311_simulation/map/halc0311_c.yaml

# for keyboard control
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

#3 save current amcl pose (waypoint) to a file and mark a waypoint on gazebo
rosrun halc0311_simulation recorddraw_goal2digit_onefile.py

#4 read the waypoint file and let the robot move along the waypoint
rosrun halc0311_simulation megarover_sequence_goals.py goal_sequence_20250316_161024.yaml

# cancel navigation goal
rostopic pub /move_base/cancel actionlib_msgs/GoalID -- {}

run keyboard or mouse

rosrun teleop_twist_keyboard teleop_twist_keyboard.py // not work
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch  //ok

roslaunch megarover3_ros mousectrl.launch

rosmsg show data_transmitter/WifiSignal

https://qiita.com/BEIKE/items/825e62bbd8d92b6d347e

git clone https://github.com/shilohc/map2gazebo.git cd ~/catkin_ws && catkin build source ~/catkin_ws/devel/setup.bash

roslaunch map2gazebo map2gazebo.launch params_file:=$(rospack find map2gazebo)/config/defaults.yaml

rosrun map_server map_server map.yaml

zotac-d05@d05:~/catkin_ws/src/halc_simulation/map$ rosrun map_server map_server halc0311_a.yaml

[ERROR] [1741507226.788502]: bad callback: <bound method MapConverter.map_callback of <__main__.MapConverter object at 0x7fd8238e8f40>>
Traceback (most recent call last):
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/zotac-d05/catkin_ws/src/map2gazebo/src/map2gazebo.py", line 30, in map_callback
    contours = self.get_occupied_regions(map_array)
  File "/home/zotac-d05/catkin_ws/src/map2gazebo/src/map2gazebo.py", line 72, in get_occupied_regions
    image, contours, hierarchy = cv2.findContours(
ValueError: not enough values to unpack (expected 3, got 2)

1. First we need to install these libraries
   pip install –user trimesh
   pip install –user numpy
   pip install –user pycollada
   pip install –user scipy
   pip install –user networkx
   

2. Then change open function variable from “w” to “wb”

    if mesh_type == "stl":
        with open(export_dir + "/map.stl", 'wb') as f:
            mesh.export(f, "stl")
        rospy.loginfo("Exported STL.  You can shut down this node now")
    elif mesh_type == "dae":
        with open(export_dir + "/map.dae", 'wb') as f:
            f.write(trimesh.exchange.dae.export_collada(mesh))

3. You can select output between stl or dae

4. The export directory was set to $(find map2gazebo)/models/map/meshes” if there is no this folder, it will say the error…

Then, File -> Save World As halc.world
hence you will get the current camera pose.
copy the models folder to the package where you copy .world to

<launch>

  <env name="GAZEBO_MODEL_PATH" value="$(find map2gazebo)/models:$(optenv GAZEBO_MODEL_PATH)" />

  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>

  <!-- Start Gazebo with a blank world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="headless" value="$(arg headless)"/>
    <arg name="world_name" value="$(find map2gazebo)/worlds/map.sdf"/>
  </include>

</launch>

<?xml version="1.0" ?>
<sdf version="1.4">
  <model name="map">
    <link name="link">
      <inertial>
        <mass>15</mass>
        <inertia>
          <ixx>0.0</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.0</iyy>
          <iyz>0.0</iyz>
          <izz>0.0</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <uri>model://map/meshes/map.stl</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name="visual">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <uri>model://map/meshes/map.stl</uri>
          </mesh>
        </geometry>
      </visual>
    </link>
    <static>1</static>
  </model>
</sdf>

zotac-d05@d05:~/catkin_ws/src$ catkin_create_pkg halc_simulation
Created file halc_simulation/package.xml
Created file halc_simulation/CMakeLists.txt
Successfully created files in /home/zotac-d05/catkin_ws/src/halc_simulation. Please adjust the values in package.xml.
zotac-d05@d05:~/catkin_ws/src$ cd halc_simulation
zotac-d05@d05:~/catkin_ws/src/halc_simulation$ mkdir -p worlds launch

zotac-d05@d05:~/catkin_ws$ catkin build

Creating a world with turtlebot (this method load map.sdf, unable to adjust the camera pose, just save the world via gazebo is fine!)

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>
  <arg name="yaw"   default="0.0"/>

  <env name="GAZEBO_MODEL_PATH" value="$(find halc_simulation)/models:$(optenv GAZEBO_MODEL_PATH)" />

  <!-- Start Gazebo with a blank world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find halc_simulation)/worlds/map.sdf"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

</launch>

Use this kind of launch file is better!

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>
  <arg name="yaw"   default="0.0"/>

  <env name="GAZEBO_MODEL_PATH" value="$(find halc_simulation)/models:$(optenv GAZEBO_MODEL_PATH)" />

  <!-- Start Gazebo with a blank world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find halc_simulation)/worlds/halc.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

</launch>

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
roslaunch halc_simulation turtlebot3_gazebo_halc.launch
roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping

zotac-d05@d05:~$ rostopic echo /amcl_pose | sed -n -e '/pose:/,+10p'
pose: 
  pose: 
    position: 
      x: 2.6776955645384244
      y: 0.006516339994905264
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.04651613535574757
      w: 0.9989175387145657

roslaunch halc_simulation turtlebot3_gazebo_halc.launch
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/catkin_ws/src/halc_simulation/map/halc_map.yaml

rostopic echo /move_base/status | sed -n -e ‘/seq:/p’ -e ‘/status:/p’ -e ‘/text:/p’

zotac-d05@d05:~/catkin_ws/src/halc_simulation$
rosrun halc_simulation megarover_sequence_goals.py goal_halc_3.yaml

roslaunch turtlebot3_gazebo turtlebot3_square_world.launch

roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

Control Your TurtleBot3!
---------------------------
Moving around:
        w
   a    s    d
        x

the robot and lidar info go different direction…

try running the command from the tutorial…
ROS演習8-2019：地図作成・自己位置推定 (gmapping, amcl)

https://demura.net/education/16294.html

zotac-d05@d05:~$ roslaunch turtlebot3_gazebo turtlebot3_fmt_world.launch
... logging to /home/zotac-d05/.ros/log/c07d07c4-fca8-11ef-89bd-9985b327fdea/roslaunch-d05-17171.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

RLException: Invalid <param> tag: Cannot load command parameter [robot_description]: no such command [['/opt/ros/noetic/share/xacro/xacro.py', '/home/zotac-d05/catkin_ws/src/turtlebot3/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf.xacro']]. 

Param xml is <param name="robot_description" command="$(find xacro)/xacro.py $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro"/>
The traceback for the exception was written to the log file

go back to the tutorial
ROS演習7-2019：デッドレコニングを実装しよう！

https://demura.net/education/16291.html

• $ roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch
• $ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
• $ rosrun my_odom3  my_odom3

[ INFO] [1741500724.826418003]: Vel (Linear:0.000000, Angular:0.000000)
[ INFO] [1741500724.826487416]: Whell Pos (r:6.116184, l:6.116184)
[ INFO] [1741500724.866283956]: Whell Pos (r:6.116184, l:6.116184)
[ INFO] [1741500724.866399997]: Seq: 2666
[ INFO] [1741500724.866445659]: /odom Pos (x:0.201834, y:0.000000, z:0.000000)
[ INFO] [1741500724.866494649]: /odom Pose (roll:0.000000, pitch:-0.000000, yaw:0.000000) 

  ros::Subscriber sub  = nh.subscribe("/cmd_vel", 10, cbVel);
  ros::Subscriber sub2 = nh.subscribe("/odom", 100, cbOdom);
  ros::Subscriber sub3 = nh.subscribe("/gazebo/model_states", 100, cbModelStates);
  ros::Subscriber sub4 = nh.subscribe("/joint_states", 100, cbMyOdom);

as the below command run with out problem, so refer its launch file

zotac-d05@d05:~$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

change from

<param name="robot_description" command="$(find xacro)/xacro.py $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

to (xacro has no .py)

<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

note that there are turtlebot3_fmt_world.launch file, fmt.world file and fmt model file.

then try the program again
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
roslaunch turtlebot3_gazebo turtlebot3_fmt_world.launch
roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping

no problem!!!

then fixed the robot and sensor orientation!

somehow just copy camera pose from map7f.world to 01_square_world.world fixed the orientation problem…

      <camera name='user_camera'>
        <pose>-5.44095 3.17636 35.0543 0 1.51164 -0.686989</pose>

note that I cannot add the world_name parameter through the launch file..

roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch world_name:=/home/zotac-d05/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worlds/turtlebot3_world.world fail

cd ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worlds/
roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch world_name:=turtlebot3_world.world fail

need to create a new launch file when has a new map
roslaunch turtlebot3_gazebo turtlebot3_world.launch ok

ps. wanna do this.. as the robot control code

• 演　習（レポート２）
  • 準備
    • 以下のROS C++スタイルガイドを参考にRobotクラスを作ろう。
      • ROS C++ スタイルガイド – ROS Wiki – ROS.org
  •  基本動作
    1. Turtlebot3を指定速度[m/s]で直進する以下のメンバ関数を作ろう。
       • void Robot::moveAtSpeed(double linear_vel)
    2. Turtlebot3を指定角速度[rad/s]で回転する以下のメンバ関数を作ろう。
       • void Robot::turnAtSpeed(double ang_vel)
    3. Turtlebot3を指定速度[m/s]で指定の距離[m]だけ直進して停止する以下のメンバ関数を作ろう。
       • void Robot::moveToDistance(double linear_vel, double dist)
    4. Turtlebot3を指定角速度[°/s]で指定の角度[°]だけ回転して停止する以下のメンバ関数を作ろう。
       • void Robot::turnToAngle(double ang_vel, double angle)
    5. Turtlebot3を指定位置（ロボット座標系）へ移動する以下のメンバ関数を作ろう。なお、ROSの座標系なのでロボットの進行方向がx、左方向がy軸の正方向です。
       • void Robot::moveToPoint(double x, double y)
  • ウェイポイントナビゲーション
    • スタート地点からゴールまで進むプログラムを作ろう。ロボットが通過するウェイポイントとその地点での姿勢を配列として実装しなさい。ロボットはウェイポイントで停止してもしなくても良いが、指定された姿勢を取ること。
  • デッドレコニング
    • デッドレコニングをcbMyOdom関数に実装しよう。rvizの場合は/odomトピックと値を比較し、gazeboが動く場合はシミュレータ上の真の位置Real Posと比較しよう。
• ヒント
  • Gazeboを起動するとTurtlebot3に速度指令を送らなくても滑って動く場合があります。その場合は次のパラメータを変更してみてください。
    • turtlebot3/turtlebot3_description/urdf/turtlebot3_burger.gazebo.xacro
      • 11,12, 21,22行目パラメータ摩擦係数mu1,mu2を0.1から1以下の大きな値。
        • <mu1>1.0</mu1>
          <mu2>1.0</mu2>
      • mu1, mu2を変化させてもスリップする場合はkp, kdのパラメータを変えましょう。gazeboは動力学計算にODEを使っており、ODEのdt(時間ステップ）、erp、cfmとは次の関係があります。
        • kp = erp/ (dt* cfm)
        • kd = (1.0- erp)/cfm
      • kp, kd, dtからerp, cfmを求める式
        • erp = dt * kp / (dt * kp + kd)
        • cfm = 1.0 / (dt * kp + kd)

zotac-d05@d05:~$ rospack find turtlebot3_gazebo
/home/zotac-d05/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo

zotac-d05@d05:~/catkin_ws/src/my_gazebo_world/worlds$ gazebo 02_map_world
Error [parser.cc:775] No <model> element in configFile[/home/zotac-d05/.gazebo/models/my_ground_plane/model.config]
Error Code 12 Msg: Unable to resolve uri[model://my_ground_plane] to model path [/home/zotac-d05/.gazebo/models/my_ground_plane] since it does not contain a model.config file.

Move 01_square_world to ~/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/worlds/01_square_world.world

Create turtlebot3_square_world.launch in turtlebot3_square_world.launch

Other world files/ launch files

empty.world
fmt.world
turtlebot3_autorace_2020.world
turtlebot3_autorace.world
turtlebot3_house.world
turtlebot3_stage_1.world
turtlebot3_stage_2.world
turtlebot3_stage_3.world
turtlebot3_stage_4.world
turtlebot3_world.world