[2023-04-05] 1. RVIZ에서 모터와 센서 통합하기

RVIZ에서 모터와 센서 통합하기

• RVIZ 가상공간에서 8자 주행하는 자이카(3D 모델링)에 라이다센서와 IMU센서의 뷰어를 통합해보자
• 3D 모델링된 차량이 8자 주행을 하면서 주변 장애물까지의 거리값을 Range로 표시하고 IMU 센싱 값에 따라 차체가 기울어짐

과제에 필요한 것들

노드와 토픽의 설계도

rviz_all 패키지 구성

rviz_all.urdf

• URDF 파일
• xycar_3d.urdf + lidar_urdf.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"/> # x축으로 -10.5cm 이동
			<geometry>
				<box size="0.50 0.12 0.01"/> # 50cm, 12cm, 1cm 크기의 박스
			</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"/> # 라이다 센서 - 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"/> # 라이다 센서 - 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"/> # 라이다 센서 - 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"/> # 라이다 센서 - 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="gray">
		<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>

rviz_all.rviz

• RVIZ 파일

• rviz_odom.rviz 파일을 복사해서 사용

• By Topic 탭에서 라이다센서의 시각화에 쓰이는 Range 추가

• 이동궤적을 표시하기 위해 Odometry 추가

  • Odometry 속성 설정

    • Topic : /odom

    • Keep : 100

    • Sharft Length : 0.05

    • Head Length : 0.1

odom_imu.py

#!/usr/bin/env python

import math
from math import sin, cos, pi
import rospy
import tf
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  # JointState 토픽과 Imu 토픽 사용

global Angle
global Imudata

def callback(msg): # JointState 토픽 callback 함수
	global Angle
	Angle = msg.position[msg.name.index("front_left_hinge_joint")]

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

def callback_imu(msg): # Imu 토픽의 callback 함수
	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) # Imu 토픽 구독

rospy.init_node('odometry_publisher')

Imudata = tf.transformations.quaternion_from_euler(0, 0, 0)

odom_pub = rospy.Publisher("odom", Odometry, queue_size = 50) # Odometry 토픽 발행
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 	# IMU 값을 Odometry에 담아 전달한다.

	odom_broadcaster.sendTransform(
		(x_, y_, 0.),
		odom_quat,
		current_time,
		"base_link", 	# odometry 정보를 차체에 전달하는 base_link에 연결(IMU 값에 따라 차제가 움직이게 하기 위해)             "odom"
	)

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

	odom.pose.pose = Pose(Point(x_, y_, 0.), Quaternion(*odom_quat)) 	# set the pose(Position & Orientation)

	odom.child_frame_id = "base_link" 	# set the frame ID

	odom_pub.publish(odom) 	# publish the message
	
	last_time = current_time
  
	r.sleep()

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="robot_state_publisher"/>
  
	<node name="driver" pkg="rviz_xycar" type="odom_8_drive.py"/> 
	<node name="odometry" pkg="rviz_all" type="odom_imu.py"/> # Imu 데이터와 Odometry가 담긴 차량의 파이썬 파일
	<node name="motor" pkg="rviz_xycar" type="converter.py"/> # 8자 주행

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

실행결과

• $ roslaunch rviz_all rviz_all.launch