킴 테스트2 IMU 센서

 

scout_sim_launch > launch

 

1.simulator.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>

 

 

 

2. turtlesim.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>

 

 

 

 

---

 

 

scout_sim_launch > scripts

 

 

1. imu_sup.py

#!/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()

 

 

2. pulisher.py

#!/usr/bin/env python
#-*- coding:utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PolygonStamped
from geometry_msgs.msg import Point32
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray


NODE_NAME = "publisher_tester"
TOPIC_NAME = "cmd_vel1"
#MSG_TYPE = Twist
#MSG_TYPE = PolygonStamped
MSG_TYPE = Marker
#MSG_TYPE = MarkerArray
PUBLISH_HZ = 10.0

class Topic_publisher():
	def __init__(self):
		rospy.init_node(NODE_NAME)
		self.publisher1 = rospy.Publisher(TOPIC_NAME, MSG_TYPE, queue_size=1)
		rospy.Timer(rospy.Duration(1.0/PUBLISH_HZ), self.timer_CB3)
		rospy.spin()


	def timer_CB3(self, event):
		# visualization)msgs/Marker
		publishing_data_marker = Marker()
		publishing_data_marker.header.stamp = rospy.Time.now()
		publishing_data_marker.header.frame_id = "object"
		publishing_data_marker.type = 3
		publishing_data_marker.action = 0
		publishing_data_marker.pose.position.x = 1.0
		publishing_data_marker.pose.position.y = 1.0
		publishing_data_marker.pose.position.z = 3.0
		publishing_data_marker.pose.orientation.x = 1.0
		publishing_data_marker.pose.orientation.y = 1.0
		publishing_data_marker.pose.orientation.z = 0.0
		#publishing_data_marker.scale = Vector3(1.0, 1.0, 0.0)
		publishing_data_marker.scale.x = 1.0
		publishing_data_marker.scale.y = 1.0
		publishing_data_marker.scale.z = 1.0
		publishing_data_marker.color.r = 0
		publishing_data_marker.color.g = 141
		publishing_data_marker.color.b = 98
		publishing_data_marker.color.a = 0.8
		self.publisher1.publish(publishing_data_marker)

	def timer_CB2(self, event):
		publishing_data_Poly = PolygonStamped()
		#publising_data의 내부를 채우는 과정
		#publishing_data_Poly.header.seq = 0
		publishing_data_Poly.header.stamp = rospy.Time.now()
		# publishing_data_Poly.header.stamp = rospy.Time.from_sec(0)
		publishing_data_Poly.header.frame_id = "triangle"
		# publishing_data_Poly.polygon.points = Point32() (X)
		publishing_data_Poly.polygon.points = [Point32(x=0.0, y=0.0, z=0.0),Point32(x=3.0, y=3.0, z=0.0), Point32(x=6.0, y=0.0, z=0.0)]
		self.publisher1.publish(publishing_data_Poly)

	def timer_CB(self, event):
		publishing_data = Twist()
		#publising_data의 내부를 채우는 과정
		publishing_data.linear = Vector3(1.0, 2.0, 3.0)
		# publishing_data.linear.x = 1.0 식으로 넣어줘도 된다.
		publishing_data.angular = Vector3(0, 0, 1.0)
		self.publisher1.publish(publish_data)
		"""
		geometry_msgs/Vector3 linear
			  float64 x
			  float64 y
			  float64 z
			geometry_msgs/Vector3 angular
			  float64 x
			  float64 y
			  float64 z
		"""
		#publishing_data.linear.x
		#print(dir(Twist))
		#print(dir(publishing_data))
		#print(publishing_data.deserialize)
		#print(publishing_data.linear)
		


if __name__=="__main__":
	Topic_publisher()

 

 

3. twist_pub.py

#!/usr/bin/env python
#-*- coding:utf-8 -*-

import rospy
from geometry_msgs.msg import Twist

NODE_NAME = "turtle_mover"
TOPIC_NAME = "cmd_vel"
MSG_TYPE = Twist
PUBLISH_HZ = 10.0


class topic_publisher():
	def __init__(self):
		rospy.init_node(NODE_NAME)
		self.publisher1 = rospy.Publisher(TOPIC_NAME, MSG_TYPE, queue_size=1)
		rospy.Timer(rospy.Duration(1.0 / PUBLISH_HZ), self.timer_CB)
		rospy.spin()

	def timer_CB(self, event):
		publishing_data = Twist()

		publishing_data.linear.x = rospy.get_param("~linear_speed", 1.0) # nodename/linear_speed
		publishing_data.linear.y = 0.0
		publishing_data.linear.z = 0.0

		publishing_data.angular.x = 0
		publishing_data.angular.y = 0
		publishing_data.angular.z = rospy.get_param("~angular_speed", 1.0) # angular_speed

		self.publisher1.publish(publishing_data)

if __name__ == '__main__':
	topic_publisher()

 

 

4. lidar_cluster.py

#!/usr/bin/env python
#-*- coding:utf-8 -*-

import rospy
## Subscriber
## topic : lidar2D
## MSG_Type : sensor_msgs/LaserScan
## Callback : cluster_callback --> lidar clustering, publish to / cluster topic

## Publisher
## Topic : cluster
## MSG_Type : obstacle_detector/Obstacles ( obstacle_detector/SegmentObstacle)

from sensor_msgs.msg import LaserScan
from obstacle_detector.msg import Obstacles, SegmentObstacle
import math

class lidar_clustering():
	def __init__(self):
		rospy.init_node("lidar_clustering")
		sub = rospy.Subscriber("lidar2D", LaserScan, self.clustering_callback)
		self.obs_pub = rospy.Publisher("cluster", Obstacles, queue_size=1)
		self.DISTANCE_TH = 0.3 # 30cm 보다 작으면 같은 물체로 하자

		# 타이머를 써줄 필요 없다.왜냐하면 laser 값 하나만 있으면 되기 때문에 
		rospy.loginfo("Initialized")
		rospy.spin()

	def clustering_callback(self, data): ## len(data.ranges) 360개 0-359까지 인덱스 값 나온다.
		## Lidar scan data
		## Lidar 데이터를 입력으로 받아서(data)
		## 순서대로 확인 (data.ranges)
		## distance = ranges[i], ranges[i+1] 둘 사이의 거리
		## distance <= distance_threshold --> 같은 물체로 취급(기존 Segment에 점을 추가)
		## distance  > distance_threshold --> 다른 물체로 취급
		## (기존 Segment는 Obstacle을 append해주고, 새로운 Segment를 만들어서 점을 등록해준다.)
		Line_Segment = SegmentObstacle()
		Segment = [[0, 0]] ## [[0,3], [5,7], [8,20]]
		## [[]] --> [[0,1]] --> [[0,2]] --> [[0,3]] --> [[0,3],[]] --> [[0,3], [0,4]]
		Obstacle_data = Obstacles()
		Obstacle_data.header.stamp = rospy.Time.now()
		Obstacle_data.header.frame_id = data.header.frame_id

		for i, r in enumerate(data.ranges):
			if i > len(data.ranges) - 2: ## i가 358부터 가능하다. 358 이하이어야 진행 가능.
				continue
			else:
				if len(Segment[-1]) == 0:
					Segment[-1].append(i)
				D = self.calc_distance(data.ranges[i], data.ranges[i+1], data.angle_increment)
				if data.ranges[i+1] >= data.range_max:
					Segment.append([i+1, i+1]) ## 새로운 segment  생성 
				elif D < self.DISTANCE_TH: ## 같은 segment로 생성 []
					Segment[-1].pop(-1)
					Segment[-1].append(i+1) ## i, i+1을 비교하고 같은 물체이므로 뒤에 뺀 다음 i+1을 넣는다.
				else:
					Segment.append([i+1, i+1]) ## 새로운 segment  생성 
		print(Segment)

		for i in Segment:
			if len(i) != 2:
				pass
			else: ## 정상적으로 한 쌍으로 나온 애들만 따지자.
				tmp_data = SegmentObstacle()
				tmp_data.first_point.x = data.ranges[i[0]] * math.cos(data.angle_min + data.angle_increment * i[0])
				tmp_data.first_point.y = data.ranges[i[0]] * math.sin(data.angle_min + data.angle_increment * i[0])
				tmp_data.last_point.x = data.ranges[i[1]] * math.cos(data.angle_min + data.angle_increment * i[1])
				tmp_data.last_point.y = data.ranges[i[1]] * math.sin(data.angle_min + data.angle_increment * i[1])
				Obstacle_data.segments.append(tmp_data)

			self.obs_pub.publish(Obstacle_data)

	def calc_distance(self, r1, r2, theta_diff):
		D = r1 **2 + r2 ** 2 - 2 * r1 * r2 * math.cos(theta_diff)
		return (D ** 0.5)


if __name__ == '__main__':
	lidar_clustering()