터틀봇 시뮬레이션 가제보에서 yaw값을 얻고 싶습니다

Community 한국어 커뮤니티
1

안녕하세요, 가제보에서 터틀봇의 yaw 값을 얻고 싶은데 제대로 작동하지 않아 질문드립니다.

기존의 터틀봇 패키지를 수정하여

   <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>

위 launch 파일을 실행하면 imu topic이 발행되는데, 이 imu 값의 orientation을 쿼터니안 변환으로 업ㄷ어낸 roll,pitch,yaw에서 yaw값을 계산하려 하였습니다.
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]

두 가지 문제점이 있는데,

  1. imu 데이터가 cmd_vel을 0을 줘도 일정하지 않고 자꾸 증가합니다. 정확한 값을 얻어지는 것 같지 않습니다.

  2. 위 쿼터니언 코드를 통해 yaw을 반환하면 실행할 때마다 값이 일정하지 않고 달라집니다. 그리고 터틀봇을 움직이고 나면 yaw이 정상적으로 계산되지 않네요.

가제보 환경에서
image
yaw값은 정상적으로 표기되는데, 제대로 계산하거나 가제보에서 표기되는 yaw값을 받아올수는 없을까요?