Robotics StackExchange | Archived questions

how to get turtlebot's yaw in gazebo?

Hello, I'm trying to do line tracking through turtlebot3 (model burger) at gazebo. I think it need to know the value of Turtlebot's heading angle to control the steering. when running the launch file by adding turblebot urdf file, there are the following topics.

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="first_tb3"  default="tb3_0"/>
  <arg name="second_tb3" default="tb3_1"/>

  <arg name="first_tb3_x_pos" default="-3.0"/>
  <arg name="first_tb3_y_pos" default=" 0.0"/>
  <arg name="first_tb3_z_pos" default=" 0.0"/>

  <arg name="first_tb3_yaw"   default=" 0.0"/>




<arg name="second_tb3_x_pos" default=" 0.0"/>
  <arg name="second_tb3_y_pos" default="-3.0"/>
  <arg name="second_tb3_z_pos" default=" 0.0"/>
  <arg name="second_tb3_yaw"   default=" 1.57"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/empty.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>

  <group ns = "$(arg first_tb3)">

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

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg first_tb3)" />
    </node>
    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg first_tb3) -x $(arg first_tb3_x_pos) -y $(arg first_tb3_y_pos) -z $(arg first_tb3_z_pos) -Y $(arg first_tb3_yaw) -param robot_description" />
  </group>

  <group ns = "$(arg second_tb3)">

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

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
      <param name="publish_frequency" type="double" value="50.0" />
      <param name="tf_prefix" value="$(arg second_tb3)" />
    </node>
    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg second_tb3) -x $(arg second_tb3_x_pos) -y $(arg second_tb3_y_pos) -z $(arg second_tb3_z_pos) -Y $(arg second_tb3_yaw) -param robot_description" />
  </group>

and roslaunch 'rostopic list' are below

/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/rosout
/rosout_agg
/tb3_0/cmd_vel
/tb3_0/imu
/tb3_0/joint_states
/tb3_0/odom
/tb3_0/scan
/tb3_1/cmd_vel
/tb3_1/imu
/tb3_1/joint_states
/tb3_1/odom
/tb3_1/scan
/tf
/tf_static

Here, I take the imu topic and calculate the heading angle as a quaternion

example rostopic echo /tb3_1/imu

header: 
  seq: 1211
  stamp: 
    secs: 49
    nsecs: 470000000
  frame_id: "base_footprint"
orientation: 
  x: -0.00272906196186
  y: 0.00272004812062
  z: 0.706975273161
  w: 0.707227768613
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -3.8725728505e-05
  y: 0.00018048770886
  z: 8.43839290564e-06
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 6.26591426547e-11
  y: -3.62427788769e-10
  z: -1.67375709478e-11
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

here are quaternion code

rospy.Subscriber("/tb3_1/imu", Imu, Imu_callback) 

def Imu_callback(msg):
    global yaw
    quaternion = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)
    euler = tf.transformations.euler_from_quaternion(quaternion)
    roll= euler[0]
    pitch = euler[1]
    yaw = euler[2]

if I do this, yaw value changes every time it runs and is unstable. Even if it is calculated correctly for the first time, it does not change normally when the turtlebot is moved. In a robot with an actual IMU sensor installed, the yaw value was a code that worked properly. How do I solve these problems? Is there anything I set wrong?

Or is there any way to get the yaw value written on gazebo?

Asked by turinbot on 2022-09-12 06:47:34 UTC

Comments

Answers