collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
6ac3dce
simulation
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 ): 10br = tf2_ros .TransformBroadcaster () 11t = geometry_msgs .msg .TransformStamped () 12odom = msg 13 14t .header .stamp = rospy .Time .now () 15t .header .frame_id = "chassis" 16t .child_frame_id = "lane_laser_frame" 17 18# Update this later depending on position of camera picture IRL 19t .transform .translation .x = 0.15 20t .transform .translation .y = 0 21t .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 29orientation = odom .pose .pose .orientation 30 31# Then convert to numpy array 32quat = np .array ([]) 33quat = np .append (quat ,orientation .x ) 34quat = np .append (quat ,orientation .y ) 35quat = np .append (quat ,orientation .z ) 36quat = np .append (quat ,orientation .w ) 37 38# Convert to Euler 39euler = tf .transformations .euler_from_quaternion (quat ) 40 41# Cause tuples can't receive item assignment apparently 42euler = list (euler ) 43 44# Rotates the exact opposite of RAScal's orientation in order to stay fixed 45#euler[2] = euler[2] * -1 46for i in range (3 ): 47euler [i ]*= - 1 48 49roll = euler [0 ] 50pitch = euler [1 ] 51yaw = euler [2 ] 52 53# Convert back to quaternion 54updated_quat = tf .transformations .quaternion_from_euler (roll ,pitch ,yaw ) 55 56# Update transform publisher 57t .transform .rotation .x = updated_quat [0 ] 58t .transform .rotation .y = updated_quat [1 ] 59t .transform .rotation .z = updated_quat [2 ] 60t .transform .rotation .w = updated_quat [3 ] 61 62# Send the 1s and 0s 63br .sendTransform (t ) 64 65'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 72data = msg 73 74# Get scan data 75broadcast_tf (data ) 76 77rospy .init_node ('lane_laser_broadcaster' ) 78 79rospy .Subscriber ("/odom" ,Odometry ,odom_callback ) 80 81try : 82rospy .spin () 83except KeyboardInterrupt : 84"Shutting down" ) 85 86