IMU 센서
직교하는 세 방향의 축에 발생하는 가속도, 각속도 그리고 지자기의 크기를 수집하는 센서이다.
std_msgs/Imu 메시지 타입
sin@Ubuntu1804:~$ rosmsg info sensor_msgs/Imu
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covarianceROS에서 제공하는 Imu 센서 메시지 타입은 위와 같이 header와 각 축의 각도 정보를 나타내는 quaternion 정보를 포함한다.
rviz에서는 각도를 쿼터니언 형태를 사용하기 때문에 오일러 각도를 쿼터니언으로 변환해서 메시지를 발행해야 한다.
오일러 -> 쿼터니언 변환
...
from tf.transformations import quaternion_from_euler
...quaternion_from_euler 함수는 오일러를 쿼터니언으로 변환하는 함수이다.
코드
오일러 각으로 수집된 IMU 센서의 데이터가 기록된 텍스트 파일을 불러와 imu 토픽을 발행하는 코드다. 그 다음 발행된 imu 토픽을 rviz를 통해 시각화 한다.
#!/usr/bin/env python
import rospy, math, rospkg
from sensor_msgs.msg import Imu
from tf.transformations import quaternion_from_euler
imu_msg = Imu()
imu_msg.header.frame_id = 'map'
degree2rad = float(math.pi)/float(180.0)
rad2degree = float(180.0)/float(math.pi)
rospy.init_node("imu_generator")
pub=rospy.Publisher('imu', Imu, queue_size=1)
path = rospkg.RosPack().get_path('rviz_imu')+"/src/imu_data.txt"
rate = rospy.Rate(10)
with open(path, 'r') as f:
lines = f.readlines()
for idx, line in enumerate(lines):
roll, pitch, yaw = map(float, line.replace(' :', '').replace(',', '').split(' ')[1::2])
x, y, z, w = quaternion_from_euler(roll, pitch, yaw)
imu_msg.orientation.x, \
imu_msg.orientation.y, \
imu_msg.orientation.z, \
imu_msg.orientation.w = x, y, z, w
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.seq = idx
pub.publish(imu_msg)
print(imu_msg)
rate.sleep()결과
lidar 센서
1채널 2D 라이다로 수집된 데이터를 시각화 하는것을 목표로 한다. 센서는 회전하면서 0.504도에 한 번씩 수집된 5000개의 데이터를 사용한다.
sin@Ubuntu1804:~/ros_ws/src$ rosmsg info sensor_msgs/LaserScan
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges (장애물 까지의 거리)
float32[] intensities라이다 센서의 데이터는 위의 메시지 타입을 사용해서 전달된다.
<launch>
<node name='rosbag_play' pkg='rosbag' type='play' output='screen' required='true'
args='$(find my_lidar)/src/lidar_topic.bag'/>
</launch> or
$ rosbag play --bags src/my_lidar/src/lidar_topic.bagrosbag을 사용하면 /scan 토픽의 메시지를 받을 수 있으며, rviz에서 Fixed frame을 laser로 바꾸면 다음과 같이 출력된다.
urdf modeling
<?xml version='1.0' ?>
<robot name='xycar' xmlns:xacro="https://ros.org/wiki/xacro">
<link name="base_link" />
<link name="baseplate">
<visual>
<material name="red" />
<origin rpy='0 0 0' xyz='0 0 0'/>
<geometry>
<box size='0.2 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'/>
<joint name="baseplate_to_front" type="fixed">
<parent link="baseplate"/>
<child link="front"/>
<origin rpy="0 0 0" xyz="0.1 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.1 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>
<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='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='orange'>
<color rgba='1.0 0.423529411765 0.0392146862745 1.0' />
</material>
</robot>modeling한 urdf를 rviz에서 정상적으로 출력하려면 robot_description, use_gui 그리고 robot_state_publisher에 대한 파라미터 설정이 필요하다.
<launch>
<!-- urdf -->
<param name="robot_description" textfile="$(find rviz_lidar)/urdf/rviz_lidar.urdf"/>
<param name="use_gui" value="true"/>
<!-- rviz -->
<node name="rviz_visualizer" pkg="rviz" type="rviz" required="true"
args="-d $(find rviz_lidar)/rviz/rviz_lidar.rviz"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!--node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /-->
<!-- lidar topic -->
<node name="rosbag_play" pkg="rosbag" type="play" output="screen" required="true"
args="$(find rviz_lidar)/src/lidar_topic.bag" />
<!-- lidar to Range -->
<node name="lidar" pkg="rviz_lidar" type="rviz_range.py" output="screen" />
<node name="lidar_fid" pkg="rviz_lidar" type="change_fid.py" output="screen" />
</launch>range.py
urdf에 modeling한 front, back, left 그리고 right에 range 형태로 표현하기 위한 토픽을 발행시키는 프로그램이다. scan 토픽을 발행하는 bag으로 부터 데이터를 받아 일정 각도의 데이터를 scan1~4라는 이름의 토픽으로 다시 발행한다.
#!/usr/bin/env python
import serial, rospy, time
from sensor_msgs.msg import Range
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Header
import tf
lidar_points = None
def lidar_callback(msg):
global lidar_points
print(len(msg.ranges))
lidar_points = msg.ranges
rospy.init_node('lidar')
rospy.Subscriber("/scan", LaserScan, lidar_callback, queue_size=1)
pub1 = rospy.Publisher('scan1', Range, queue_size = 10)
pub2 = rospy.Publisher('scan2', Range, queue_size = 10)
pub3 = rospy.Publisher('scan3', Range, queue_size = 10)
pub4 = rospy.Publisher('scan4', Range, queue_size = 10)
broadcaster = tf.TransformBroadcaster()
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
msg_range = Range()
msg_header = Header()
msg_range.radiation_type = Range().ULTRASOUND
msg_range.min_range = 0.02
msg_range.max_range = 2.0 #float("inf")
msg_range.field_of_view = (30.0/180.0)*3.14
while not rospy.is_shutdown():
if lidar_points == None:
continue
msg_header.frame_id = 'front'
msg_range.header = msg_header
msg_range.range = lidar_points[90]
pub1.publish(msg_range)
msg_header.frame_id = 'right'
msg_range.header = msg_header
msg_range.range = lidar_points[180]
pub2.publish(msg_range)
msg_header.frame_id = 'back'
msg_range.header = msg_header
msg_range.range = lidar_points[270]
pub3.publish(msg_range)
msg_header.frame_id = 'left'
msg_range.header = msg_header
msg_range.range = lidar_points[0]
pub4.publish(msg_range)
broadcaster.sendTransform(
(0, 0, 0.),
quat,
rospy.Time.now(),
"base_link",
"odom")
time.sleep(0.5)결과
모든 센서를 사용해서 시각화 하기
위에서 시각화했던 모든 센서들을 사용해서 시각화에 사용한다.
<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="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/> -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="driver" pkg="rviz_all" type="8_drive.py"/>
<node name="motor" pkg="rviz_all" type="converter.py"/>
<node name="odometry" pkg="rviz_all" type="odom_imu.py"/>
<node name="rosbag_play" pkg="rosbag" type="play" required="true"
args="$(find rviz_lidar)/src/lidar_topic.bag" />
<node name="lidar" pkg="rviz_lidar" type="rviz_range.py" output="screen"/>
<!-- <node name="lidar_fid" pkg="rviz_lidar" type="change_fid.py"/> -->
<node name="imu" pkg="rviz_imu" type="imu_generator.py"/>
</launch>차량 주행 시각화에서 사용했던 odom 파일을 그대로 가져와
Imudata = tf.transformations.quaternion_from_euler(0, 0, 0)
odom_broadcaster.sendTransform((x_, y_, 0.),
odom_quat,
current_time,
'base_link',
'odom')부분만 달라진다. 차량 주행 시각화에서는 odom에 yaw 변화만 사용해서 시각화에 반영했지만, 여기서는 imu에서 발생되는 데이터를 odom에 반영한다.
#!/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
current_speed = 0.4
wheel_base = 0.2
x_ = 0
y_ = 0
yaw_ = 0
Angle = 0
Imudata = tf.transformations.quaternion_from_euler(0, 0, 0)
rospy.init_node("odemetry_publisher")
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()
rate = rospy.Rate(30.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
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.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, yaw_))
odom_pub.publish(odom)
last_time = current_time
rate.sleep()결과
