collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Ricky ChenTesting lower camera position6ac3dce

simulation
2.0 KiB86 linesraw
1#!/usr/bin/env python
2import rospy
3import tf
4import tf2_ros
5import geometry_msgs.msg
6import numpy as np
7from nav_msgs.msg import Odometry
8
9def broadcast_tf(msg):
10	br = tf2_ros.TransformBroadcaster()
11	t = geometry_msgs.msg.TransformStamped()
12	odom = msg
13
14	t.header.stamp = rospy.Time.now()
15	t.header.frame_id = "chassis"
16	t.child_frame_id = "lane_laser_frame"
17
18	# Update this later depending on position of camera picture IRL
19	t.transform.translation.x = 0.15
20	t.transform.translation.y = 0
21	t.transform.translation.z = 0.1
22
23	###
24	# EDIT: The following almost works. Have to read up on quaternions.
25	# Might use euler angles first then convert to quats
26	###
27
28	# Retrieve orientation data
29	orientation = odom.pose.pose.orientation
30
31	# Then convert to numpy array
32	quat = np.array([])
33	quat = np.append(quat, orientation.x)
34	quat = np.append(quat, orientation.y)
35	quat = np.append(quat, orientation.z)
36	quat = np.append(quat, orientation.w)
37
38	# Convert to Euler
39	euler = tf.transformations.euler_from_quaternion(quat)
40
41	# Cause tuples can't receive item assignment apparently
42	euler = list(euler)
43
44	# Rotates the exact opposite of RAScal's orientation in order to stay fixed
45	#euler[2] = euler[2] * -1
46	for i in range(3):
47		euler[i] *= -1
48
49	roll = euler[0]
50	pitch = euler[1]
51	yaw = euler[2]
52
53	# Convert back to quaternion
54	updated_quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
55
56	# Update transform publisher
57	t.transform.rotation.x = updated_quat[0]
58	t.transform.rotation.y = updated_quat[1]
59	t.transform.rotation.z = updated_quat[2]
60	t.transform.rotation.w = updated_quat[3]
61
62	# Send the 1s and 0s
63	br.sendTransform(t)
64
65	print('Roll: %.3g' % euler[0], 'Pitch: %.3g' % euler[1], 'Yaw: %.3g' % euler[2])
66
67	#print(updated_quat)
68	#print(odom.pose.pose.orientation.z)
69
70def odom_callback(msg):
71	# Set data to msg
72	data = msg
73
74	# Get scan data
75	broadcast_tf(data)
76
77rospy.init_node('lane_laser_broadcaster')
78
79rospy.Subscriber("/odom", Odometry, odom_callback)
80
81try:
82	rospy.spin()
83except KeyboardInterrupt:
84	print("Shutting down")
85
86