data = AccelWithCovariace()
data.accel.linear.x
data.accel.linear.y
...
data.accel.angular.x
...
data.covariancekw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src$ roslaunch rosbridge_server rosbridge_websocket.launch
2022-03-02 10:25:08+0900 [-] [INFO] [1646184308.352070]: Client connected. 7 clients total.
kw-cobot@kwcobot-HGDT-Series:~$ roslaunch scout_tf static_tf.launch
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src$ rosrun rqt_tf_tree rqt_tf_tree
초기 설정 자동으로 만들기
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src$ catkin_create_pkg scout_sim_launch rospy roscpp
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch$ mkdir launch
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch$ cd launch
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src/scout_sim_launch/launch$ subl simulater.launch
<launch>
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
<!-- roslaunch rosbridge_server rosbridge_websocket.launch-->
<include file="$(find scout_tf)/launch/static_tf.launch"/>
<!-- roslaunch scout_tf static_tf.launch -->
</launch>
kw-cobot@kwcobot-HGDT-Series:~/catkin_ws/src$ catkin build
터미널 창을 모두 닫고 재실행 후
kw-cobot@kwcobot-HGDT-Series:~$ roslaunch scout_sim_launch simulater.launch
을 입력하고 시뮬레이터에서 Edit -> Map and Vihicle 로 시뮬레이터를 다시 start 하면
setting /run_id to 00611320-99ca-11ec-955f-04d4c4919854
process[rosout-1]: started with pid [18471]
started core service [/rosout]
process[rosbridge_websocket-2]: started with pid [18479]
process[rosapi-3]: started with pid [18480]
process[base_link_to_laser-4]: started with pid [18481]
process[base_link_to_camera-5]: started with pid [18482]
process[base_link_to_imu-6]: started with pid [18488]
2022-03-02 10:42:22+0900 [-] Log opened.
[INFO] [1646185342.285493]: Rosapi started
2022-03-02 10:42:22+0900 [-] registered capabilities (classes):
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.call_service.CallService
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.advertise.Advertise
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.publish.Publish
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.subscribe.Subscribe
2022-03-02 10:42:22+0900 [-] - <class 'rosbridge_library.capabilities.defragmentation.Defragment'>
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.advertise_service.AdvertiseService
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.service_response.ServiceResponse
2022-03-02 10:42:22+0900 [-] - rosbridge_library.capabilities.unadvertise_service.UnadvertiseService
2022-03-02 10:42:22+0900 [-] WebSocketServerFactory starting on 9090
2022-03-02 10:42:22+0900 [-] Starting factory <autobahn.twisted.websocket.WebSocketServerFactory object at 0x7f6e96ea0450>
2022-03-02 10:42:22+0900 [-] [INFO] [1646185342.498679]: Rosbridge WebSocket server started at ws://0.0.0.0:9090
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.767584]: Client connected. 1 clients total.
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.771834]: Client connected. 2 clients total.
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.776681]: Client connected. 3 clients total.
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.784386]: Client connected. 4 clients total.
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.873218]: Client connected. 5 clients total.
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.875551]: Client connected. 6 clients total.
2022-03-02 10:42:36+0900 [-] [INFO] [1646185356.877696]: Client connected. 7 clients total.
2022-03-02 10:42:38+0900 [-] [INFO] [1646185358.756430]: [Client 2] Subscribed to /scout_light_control
2022-03-02 10:42:38+0900 [-] [INFO] [1646185358.759915]: [Client 3] Subscribed to /cmd_vel
위 처럼 7개의 client가 자동으로 셋팅된다.
IMU(imu)라는 토픽을 구독하는 코드를 짜주기
클래스 형태로 짜주기
이후 덧붙여서 다양한 데이터 값 받을 것이다.
이후 radian/sec를 시간에 대해 적분해주자 (구분구적법)
내가 푼 풀이
#!/usr/bin/env python
## imu subscriber
## print angular z
import rospy
from sensor_msgs.msg import Imu
import math
import time
class Imu_sub:
def __init__(self):
self.msg_sub = rospy.Subscriber("/imu", Imu ,self.imu_CB)
#self.end = time.time()
#print self.end, "end sec"
self.current_time = 0
def imu_CB(self, msg):
self.current_time = time.time()
self.angular_z = msg.angular_velocity.z
print "z(radian/sec) :", self.angular_z
def run():
rospy.init_node("get_imu_z_data", anonymous=False)
sub = Imu_sub()
old_time = sub.current_time
old_angular_z = sub.angular_z
interval_time = sub.current_time - old_time
print interval_time, "sec"
print "z(radian) :", old_angular_z * interval_time
rospy.spin()
if __name__ == "__main__":
run()
rostopic list / Imu
rosmsg show Imu
kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show Imu
[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_covariance
float64[9] orientation_covariance
은 오차를 나타낸다.
linear_acceleration을 쓰지 않는 이유 : 적분을 두 번하면 중간에 적분 상수가 끼게 되어 오차가 커지므로
angular_velocity를 한 번 적분하여 이동거리를 구한다.
rostopic echo /linear_velocity
#!/usr/bin/env python
## imu subscriber
## print angular z
import rospy
from sensor_msgs.msg import Imu
from scout_msgs.msg import ScoutStatus
## scout_msgs/ScoutStatus
import math
class imu_sub():
def __init__(self):
rospy.init_node("imu_sub")
rospy.Subscriber("imu", Imu, self.Imu_callback)
rospy.Subscriber("scout_status", ScoutStatus, self.Status_CB)
self.yaw = 0
self.x = 0
self.y = 0
self.old_v = 0
self.old_phi_dot = 0
self.old_time = 0
self.status_old_time = 0
self.old_x_dot = 0
self.old_y_dot = 0
rospy.Timer(rospy.Duration(0.1), self.timer_CB)
rospy.spin()
def timer_CB(self, event):
rospy.loginfo("(x_dot, y_dot, yaw_dot) = ({:.4f}, {:.4f}, {:.4f})".format(self.old_x_dot, self.old_y_dot, self.old_phi_dot))
rospy.loginfo("(x, y, yaw) = ({:.4f}, {:.4f}, {:.4f})".format(self.x, self.y, self.yaw))
def Status_CB(self, data):
self.v = data.linear_velocity
self.status_current_time = rospy.Time.now().to_sec()
if abs(self.v) < 0.001:
self.v = 0
self.x_dot = self.v * math.cos(self.yaw)
self.y_dot = self.v * math.sin(self.yaw)
# rospy.loginfo("{}, {}".format(self.v, self.status_current_time))
if self.status_old_time != 0:
time_diff = self.status_current_time - self.status_old_time
self.x += self.old_x_dot * time_diff
self.y += self.old_y_dot * time_diff
self.status_old_time = self.status_current_time
self.old_x_dot = self.x_dot
self.old_y_dot = self.y_dot
def Imu_callback(self, data):
phi_dot = data.angular_velocity.z
self.current_time = data.header.stamp.to_sec()
if abs(phi_dot) < 0.001:
phi_dot = 0
if self.old_time != 0:
time_diff = self.current_time - self.old_time
self.yaw += self.old_phi_dot * time_diff
# rospy.loginfo("angular_z = {}".format(phi_dot))
# rospy.loginfo("yaw = {}".format(self.yaw))
# rospy.loginfo("yaw_deg = {}".format(self.yaw * 180 / math.pi))
self.old_time = self.current_time
self.old_phi_dot = phi_dot
if __name__ == '__main__':
imu_sub()1. odometry
1. odometry : 주행 기록계
전원을 켰을 때부터의 이동 거리, 위치 측정한다.
바퀴의 회전수(Encoder센서 이용: 모터의 회전 각도가 나온다.)와 IMU(관성 측정 장치: 9dof, 6dof)를 기반으로 움직이는 물체의 위치를 측정한다.
dof: degree of frame
roll pitch yaw x y z의 가속도
자계센서 3개 까지 추가하면 9dof이다.
카라이크 모델 2가지
Ackermann Streering Model (에커만 스트링 모델) : 앞 바퀴 두 개로만 조향 가능. 나머지 두 개는 앞 뒤 회전만 가능함
Differencial Drive Model: 두 바퀴가 조향 가능, x,y 구하는 식은 같으나 파이를 구하는 값은 다르다.
v는 선속도를 의미한다.
바퀴의 각도는 델타
차량이 보고 있는 방향인 헤딩 앵글은 프사인이 나타낸다.
Dead Reckoning: 실외에서 GPS가 연결이 끊기거나 하는 경우
예를 들어 터널로 진입하였을 때 터널 내부에서 속도와 가속도로 이동할 때 쓴다.
Visual odometry: one take로 찍은 동영상을 참고로 하여 이동 거리 계산
T265 TRacking Camera: 단종됨, visual odomety 기능 탑재된 카메라
2. transformation
tf: frame(좌표축)사이의 변환 관계를 나타낸다.
총 6개의 프레임이 필요하다. x, y, z, roll, pitch, yaw
linear_velocity와 angular_velocity를 이용하면 x, y, z 식을 구할 수 있다.
3.
kw-cobot@kwcobot-HGDT-Series:~$ rostopic list
/Object_topic
/client_count
/cmd_vel
/connected_clients
/image_jpeg/compressed
/imu
/lidar2D
/rosout
/rosout_agg
/scout_light_control
/scout_status
/tf
kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show Odometry
[nav_msgs/Odometry]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
오도 메트리를 보내는 방법 메세지 타입에 담아서 보내는 방법, tf로 보내는 방법 두 가지로
imu 센서 값을 보내줄 수 있다.
오도메트리 자체는 위 메시지 타입으로 보낼 수 있으나
tf 타입은 하나밖에 보내지 못한다.
odom 밑에 base_link가 붙는다.
tf는 여러 명이 보낼 수 없다.
첫번재 방법: 메시지에 담아서 보내기
from nav_msgs.msg import Odometry
를 코드에 추가한다.
rostopic hz/ scoutstatus
작은 거에 맞추자. 60이 아닌 30hz로 한다.
rostopic echo / scout_odom
geometry_msgs/Twist
rosmsg show GeometryTwist
data = Twist()
각 들여쓰기 된 부분은
레벨을 나타낸다.
data라는 붕어빵
data.linear.x
data.linear.y
data.linear.z
data.angular.x
data.angular.y
data.angular.z
식으로 접근이 가능하다.
최종적으로 6개에게 접근이 가능하다.
두번째 예
kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show geometry_msgs/AccelWithCovariance
geometry_msgs/Accel accel
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
data.accel.linear.x
...
data.accel.angular.x
...
data.covariance식으로 접근이 가능하다.
kw-cobot@kwcobot-HGDT-Series:~$ rosmsg show Odometry
[nav_msgs/Odometry]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
msg로 토픽을 주고 받아서 나오도록 코드를 구현해본다.
결론적으로 나온 코드
#!/usr/bin/env python
## imu subscriber
## print angular z
import rospy
from sensor_msgs.msg import Imu
from scout_msgs.msg import ScoutStatus
## scout_msgs/ScoutStatus
from nav_msgs.msg import Odometry
## nav_msgs/Odometry --> Publish
import math
from tf.transformations import quaternion_from_euler
class imu_sub():
def __init__(self):
rospy.init_node("imu_sub")
rospy.Subscriber("imu", Imu, self.Imu_callback)
rospy.Subscriber("scout_status", ScoutStatus, self.Status_CB)
self.odom_pub = rospy.Publisher("scout_odom", Odometry, queue_size=1)
self.yaw = 0
self.x = 0
self.y = 0
self.old_v = 0
self.old_phi_dot = 0
self.old_time = 0
self.status_old_time = 0
self.old_x_dot = 0
self.old_y_dot = 0
rospy.Timer(rospy.Duration(1.0 / 30), self.timer_CB) ## 30hz
rospy.spin()
def timer_CB(self, event):
publish_data = Odometry()
#rospy.loginfo("(x_dot, y_dot, yaw_dot) = ({:.4f}, {:.4f}, {:.4f})".format(self.old_x_dot, self.old_y_dot, self.old_phi_dot))
#rospy.loginfo("(x, y, yaw) = ({:.4f}, {:.4f}, {:.4f})".format(self.x, self.y, self.yaw))
publish_data.header.stamp = rospy.Time.now()
publish_data.header.frame_id = "odom"
publish_data.child_frame_id = 'base_link'
publish_data.pose.pose.position.x = self.x
publish_data.pose.pose.position.y = self.y
publish_data.pose.pose.position.z = 0
(qx, qy, qz, qw) = quaternion_from_euler(0, 0, self.yaw)
publish_data.pose.pose.orientation.x = qx
publish_data.pose.pose.orientation.y = qy
publish_data.pose.pose.orientation.z = qz
publish_data.pose.pose.orientation.w = qw
publish_data.twist.twist.linear.x = self.old_v
publish_data.twist.twist.linear.y = 0
publish_data.twist.twist.linear.z = 0
publish_data.twist.twist.angular.x = 0
publish_data.twist.twist.angular.y = 0
publish_data.twist.twist.angular.z = self.old_phi_dot
"""
pose
pose
position
x --> self.x
y --> self.y
z --> 0
orientation
x --> qx
y --> qy
z --> qw
w --> tf code: (qx, qy, qz, qw)= quaternion_from_euler(roll,pitch,yaw)
(0, 0, self.yaw)
covaricance
twist
linear
x --> self.old_v
y --> 0
z --> 0
angular
x --> 0
y --> 0
z --> self.old_phi_dot
"""
self.odom_pub.publish(publish_data)
def Status_CB(self, data):
self.v = data.linear_velocity
self.status_current_time = rospy.Time.now().to_sec()
if abs(self.v) < 0.001:
self.v = 0
self.x_dot = self.v * math.cos(self.yaw)
self.y_dot = self.v * math.sin(self.yaw)
# rospy.loginfo("{}, {}".format(self.v, self.status_current_time))
if self.status_old_time != 0:
time_diff = self.status_current_time - self.status_old_time
self.x += self.old_x_dot * time_diff
self.y += self.old_y_dot * time_diff
self.status_old_time = self.status_current_time
self.old_x_dot = self.x_dot
self.old_y_dot = self.y_dot
def Imu_callback(self, data):
phi_dot = data.angular_velocity.z
self.current_time = data.header.stamp.to_sec()
if abs(phi_dot) < 0.001:
phi_dot = 0
if self.old_time != 0:
time_diff = self.current_time - self.old_time
self.yaw += self.old_phi_dot * time_diff
# rospy.loginfo("angular_z = {}".format(phi_dot))
# rospy.loginfo("yaw = {}".format(self.yaw))
# rospy.loginfo("yaw_deg = {}".format(self.yaw * 180 / math.pi))
self.old_time = self.current_time
self.old_phi_dot = phi_dot
if __name__ == '__main__':
imu_sub()
kw-cobot@kwcobot-HGDT-Series:~$ rosr scout_sim_launch imu_sub.py
kw-cobot@kwcobot-HGDT-Series:~$ rostopic list
/Object_topic
/client_count
/cmd_vel
/connected_clients
/image_jpeg/compressed
/imu
/lidar2D
/rosout
/rosout_agg
/scout_light_control
/scout_odom
/scout_status
/statistics
/tf
kw-cobot@kwcobot-HGDT-Series:~$ rostopic echo /scout_odom를 입력하면
원점을 기준으로 x, y 의 position이 나오게 되고 틀어진 각을 z(radian/sec)와 w(radian)으로 나타낸다.
이 때 orientation은 오일러 값으로 계산하여 구한 값 4개 이다.
오일러 값이 필요한 이유는 오일러 짐벌락이라고 하는 각 축 3개의 축이 겹치게 될 때 나타나게 된다.
아래 블로그를 참고하자.
또한 twist값은 roll pitch yaw를 나타낸다.
짐벌락과 오일러 각 (gimbal lock, euler angles)
개요 오일러 각이란? 짐벌락이란? 짐벌락은 왜 생기나? 1. Gimbal 이란? 출처 - [http://ko.wikipedia.org/wiki/%EC%A7%90%EB%B2%8C] 이것이 바로 짐벌이다. 위키에는 단일 축으로 물체가 회전하도록 중심축을 가..
handhp1.tistory.com
전체적인 rqt_graph
rviz를 열어서 아래와 같이 설정한다.
안되면 rosrun 다시 실행할 것.
odometer를 통해 slam을 구현할 수 있다.
센서와 결합해서 지도를 파악하여 SLAM 을 구현할 수 있다. 민형기쌤한테 배워야겠다.
'Robot > ROS' 카테고리의 다른 글
| [simulator] publisher (0) | 2022.03.03 |
|---|---|
| [simulator] 범용적인 publisher 코드 짜기 (0) | 2022.03.02 |
| [urdf] 팁 (0) | 2022.02.28 |
| 두번째 (0) | 2022.02.25 |
| 참고 자료 (0) | 2022.02.25 |
