자이카 - 뷰어 통합

목표

• 3D 모델링된 차량이 8자 주행을 하면서
  • 주변 장애물의 거리를 Range로 표시하고
  • IMU 센싱값에 따라 차체가 기울어진다.

구조

1. 자이카 모터 토픽 받아오기
2. Lidar 센서 토픽 rosbag으로 받아오기
3. imu_data_generator데이터 RVIZ에 표현하기

패키지 구성

• 패키지 이름은 rviz_all
  • 런치 파일 - rviz_all.launch
  • urdf 파일 - rviz_all.urdf, 기존에 제작한 xycar_3d.urdf + lidar_urdf.urdf 잘 합쳐보자
  • 소스 파일 - odom_imu.py, 기존에 제작한 rviz_odom.py를 수정해 보자

# 패키지 제작
catkin_create_pkg rviz_all rospy tf geometry_msgs urdf rviz xacro

# rviz_all.launch
<launch>
    <param name="robot_description" textfile="$(find rviz_all)/urdf/rviz_all.urdf"/>
    <param name="use_gui" value="true"/>

    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
                args="-d $(find rviz_all)/rviz/rviz_all.rviz"/>

    <node name="robot_state_publisher" pkg="robot_state_publisher" 
                type="state_publisher"/>

    <node name="driver" pkg="rviz_xycar" type="odom_8_drive.py" /> 
    <node name="odometry" pkg="rviz_all" type="odom_imu.py" />
    <node name="converter" pkg="rviz_xycar" type="converter.py" />

    <node name="rosbag_play" pkg="rosbag" type="play" output="screen" 
          required="true" args="$(find rviz_lidar)/src/lidar_topic.bag"/>
    <node name="lidar" pkg="rviz_lidar" type="lidar_urdf.py" output="screen"/>

    <node name="imu" pkg="rviz_imu" type="imu_generator.py"/>
</launch>

# rviz_all.urdf
<?xml version="1.0" ?>
<robot name="xycar" xmlns:xacro="http://www.ros.org/wiki/xacro">
 <link name="base_link"/>
  <link name="baseplate">
    <visual>
      <material name="acrylic"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.5 0.2 0.07"/>
      </geometry>
    </visual>
  </link>
  <joint name="base_link_to_baseplate" type="fixed">
    <parent link="base_link"/>
    <child link="baseplate"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="front_mount">
    <visual>
      <material name="blue"/>
      <origin rpy="0 0.0 0" xyz="-0.105 0 0"/>
      <geometry>
        <box size="0.50 0.12 0.01"/>
      </geometry>
    </visual>
  </link>
  <joint name="baseplate_to_front_mount" type="fixed">
    <parent link="baseplate"/>
    <child link="front_mount"/>
    <origin rpy="0 0 0" xyz="0.105 0 -0.059"/>
  </joint>

  <link name="front"/>
  <joint name="baseplate_to_front" type="fixed">
    <parent link="baseplate"/>
    <child link="front"/>
    <origin rpy="0 0 0" xyz="0.25 0 0"/>
  </joint>

  <link name="back"/>
  <joint name="baseplate_to_back" type="fixed">
    <parent link="baseplate"/>
    <child link="back"/>
    <origin rpy="0 0 3.14" xyz="-0.25 0 0"/>
  </joint>

  <link name="left"/>

  <joint name="baseplate_to_left" type="fixed">
    <parent link="baseplate"/>
    <child link="left"/>
    <origin rpy="0 0 1.57" xyz="0 0.1 0"/>
  </joint>

  <link name="right"/>

  <joint name="baseplate_to_right" type="fixed">
    <parent link="baseplate"/>
    <child link="right"/>
    <origin rpy="0 0 -1.57" xyz="0 -0.1 0"/>
  </joint>

  <link name="front_shaft">
    <visual>
      <material name="black"/>
      <origin rpy="1.57 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.285" radius="0.018"/>
      </geometry>
    </visual>
  </link>
  <joint name="front_mount_to_front_shaft" type="fixed">
    <parent link="front_mount"/>
    <child link="front_shaft"/>
    <origin rpy="0 0 0" xyz="0.105 0 -0.059"/>
  </joint>
  <link name="rear_shaft">
    <visual>
      <material name="black"/>
      <origin rpy="1.57 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.285" radius="0.018"/>
      </geometry>
    </visual>
  </link>
  <joint name="rear_mount_to_rear_shaft" type="fixed">
    <parent link="front_mount"/>
    <child link="rear_shaft"/>
    <origin rpy="0 0 0" xyz="-0.305 0 -0.059"/>
  </joint>
  <link name="front_right_hinge">
    <visual>
      <material name="white"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.015"/>
      </geometry>
    </visual>
  </link>
  <joint name="front_right_hinge_joint" type="revolute">
    <parent link="front_shaft"/>
    <child link="front_right_hinge"/>
    <origin rpy="0 0 0" xyz="0 -0.1425 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="10" lower="-0.34" 
                       upper="0.34" velocity="100"/>
  </joint>
  <link name="front_left_hinge">
    <visual>
      <material name="white"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.015"/>
      </geometry>
    </visual>
  </link>
  <joint name="front_left_hinge_joint" type="revolute">
    <parent link="front_shaft"/>
    <child link="front_left_hinge"/>
    <origin rpy="0 0 0" xyz="0 0.14 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="10" lower="-0.34" upper="0.34" velocity="100"/>
  </joint>
  <link name="front_right_wheel">
    <visual>
      <material name="black"/>
      <origin rpy="1.57 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.064" radius="0.07"/>
      </geometry>
    </visual>
  </link>
  <joint name="front_right_wheel_joint" type="continuous">
    <parent link="front_right_hinge"/>
    <child link="front_right_wheel"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="10" velocity="100"/>
  </joint>
  <link name="front_left_wheel">
    <visual>
      <material name="black"/>
      <origin rpy="1.57 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.064" radius="0.07"/>
      </geometry>
    </visual>
  </link>
  <joint name="front_left_wheel_joint" type="continuous">
    <parent link="front_left_hinge"/>
    <child link="front_left_wheel"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="10" velocity="100"/>
  </joint>
  <link name="rear_right_wheel">
    <visual>
      <material name="black"/>
      <origin rpy="1.57 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.064" radius="0.07"/>
      </geometry>
    </visual>
  </link>
  <joint name="rear_right_wheel_joint" type="continuous">
    <parent link="rear_shaft"/>
    <child link="rear_right_wheel"/>
    <origin rpy="0 0 0" xyz="0 -0.14 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="10" velocity="100"/>
  </joint>
  <link name="rear_left_wheel">
    <visual>
      <material name="black"/>
      <origin rpy="1.57 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.064" radius="0.07"/>
      </geometry>
    </visual>
  </link>
  <joint name="rear_left_wheel_joint" type="continuous">
    <parent link="rear_shaft"/>
    <child link="rear_left_wheel"/>
    <origin rpy="0 0 0" xyz="0 0.14 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="10" velocity="100"/>
  </joint>
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>
  <material name="orange">
    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
  </material>
  <material name="brown">
    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
  </material>
  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <material name="acrylic">
    <color rgba="1.0 1.0 1.0 0.4"/>
  </material>
</robot>

# odom_imu.py
#!/usr/bin/env python

import math
from math import sin, cos, pi
import rospy
import tf
import numpy as np
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from sensor_msgs.msg import JointState
from sensor_msgs.msg import Imu

global Angle
global Imudata

rospy.init_node('odometry_publisher')
Imudata = tf.transformations.quaternion_from_euler(0, 0, 0)

def callback(msg):
	global Angle
	Angle = msg.position[msg.name.index("front_left_hinge_joint")]

rospy.Subscriber('joint_states', JointState, callback)

def callback_imu(msg):
    global Imudata
    Imudata[0] = msg.orientation.x
    Imudata[1] = msg.orientation.y
    Imudata[2] = msg.orientation.z
    Imudata[3] = msg.orientation.w

rospy.Subscriber('imu', Imu, callback_imu)

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(30.0)

current_speed = 0.4
wheel_base = 0.2
x_ = 0
y_ = 0
yaw_ = 0
Angle = 0

while not rospy.is_shutdown():
	current_time = rospy.Time.now()

	dt = (current_time - last_time).to_sec()

	current_steering_angle = Angle
	current_angular_velocity = current_speed * math.tan(current_steering_angle) / wheel_base

	x_dot = current_speed * cos(yaw_)
	y_dot = current_speed * sin(yaw_)
	x_ += x_dot * dt
	y_ += y_dot * dt
	yaw_ += current_angular_velocity * dt

	odom_quat = Imudata / np.linalg.norm(Imudata)
	#odom_quat = tf.transformations.quaternion_from_euler(0, 0, yaw_)

	odom_broadcaster.sendTransform(
		(x_, y_, 0.),
		odom_quat,
		current_time,
		"base_link",
		"odom"
	)

	odom = Odometry()
	odom.header.stamp = current_time
	odom.header.frame_id = "odom"

	odom.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat))

	odom.child_frame_id = "base_link"

	odom_pub.publish(odom)

	last_time = current_time
	r.sleep()

결과

# 실행
roslaunch rviz_all rviz_all.launch

난리 났다. (원래 이게 맞음)

고생했던 오류

[ERROR] [1696931554.950944828]: Ignoring transform for child_frame_id "base_link" from authority "unknown_publisher" because of an invalid quaternion in the transform (-0.263765 0.191136 0.000000 -0.033200)

• odom_quat = Imudata에 집어넣는 과정에서 정규화가 되지 않았다는 오류가 발생했다.
• numpy를 import한 뒤,
  • odom_quat = Imudata에서
  • odom_quat = Imudata / np.linalg.norm(Imudata)로 수정해 해결했다.