def process(args):
## DON'T ALTER ------------------------------------------------
inbagobj = rosbag.Bag(args.inbag, 'r')
imuDF = pd.DataFrame(columns=('timestamp', 'phi', 'theta', 'psi', 'p', 'q',
'r', 'ax', 'ay', 'az'))
with rosbag.Bag(args.inbag, 'r') as inbagobj:
for topic, msg, t in inbagobj.read_messages():
if topic == "/imu/data":
quat = (msg.orientation.x, msg.orientation.y,
msg.orientation.z, msg.orientation.w)
eul = T.euler_from_quaternion(quat, axes='sxyz')
t = msg.header.stamp
tmp = pd.DataFrame(
{
'timestamp': pd.Timestamp(t.to_sec(), unit="s"),
'phi': eul[0],
'theta': eul[1],
'psi': eul[2],
'p': msg.angular_velocity.x,
'q': msg.angular_velocity.y,
'r': msg.angular_velocity.z,
'ax': msg.linear_acceleration.x,
'ay': msg.linear_acceleration.y,
'az': msg.linear_acceleration.z
},
index=[0])
imuDF = imuDF.append(tmp, sort=False, ignore_index=True)
imuDF.to_csv(args.outcsv)
def callback(self, data):
# Simulation: Read the position of the landmarks from gazebo in
# the absolute frame and transform them relative to the robot's frame
# Check for the landmarks within sensor's the field of view.
# Simulation labels: Purple: 0, Orange: 1, Yellow: 2, Blue: 3, Green: 4, Red: 5, Black: 6, Turquoise: 7
msg_landmarks = Landmarks_Msg()
#robot pose -> Ground truth pose from gazebo
robot_pose_gt = data.pose[10]
yaw = transformations.euler_from_quaternion([robot_pose_gt.orientation.x,
robot_pose_gt.orientation.y,
robot_pose_gt.orientation.z,
robot_pose_gt.orientation.w])[2]
robot_pose_abs_gt = [[robot_pose_gt.position.x], [robot_pose_gt.position.y], [pi2pi(yaw)]]
marker_array_msg = MarkerArray()
# In the gazebo/model_states, the cylinders have an index in the range [2,9]
for i in range(0, 8):
landmark_position_abs_gt = [data.pose[i+2].position.x,
data.pose[i+2].position.y]
# convert from absolute to relative coordinate frame using the ground
# truth pose of the robot
[landmark_position_rel_gt, _, _] = Absolute2RelativeXY(robot_pose_abs_gt,
landmark_position_abs_gt)
# print("landmark {} at relative location: {}, {}".format(i-2, landmark_position_rel_gt[0][0], landmark_position_rel_gt[1][0]))
# check for infront and within field of view
if landmark_position_rel_gt[0][0]>0.3:
tan_val = abs(landmark_position_rel_gt[1][0]/landmark_position_rel_gt[0][0])
angle = math.atan(min(tan_val,4))
# if angle < 1.047:
if angle < 0.47878508936:
# standard deviation proportional to the distance to the landmark
s_landmark_x = std_dev_landmark_x * landmark_position_rel_gt[0][0]
s_landmark_y = std_dev_landmark_y * landmark_position_rel_gt[1][0]
# add noise to the ground truth position of the landmark
landmark_position_rel_x = landmark_position_rel_gt[0][0] + s_landmark_x * np.random.standard_normal()
landmark_position_rel_y = landmark_position_rel_gt[1][0] + s_landmark_y * np.random.standard_normal()
# populate the position and covariance of the landmarks
msg_landmark = Landmark_Msg()
msg_landmark.label = i
msg_landmark.x = landmark_position_rel_x # x
msg_landmark.y = landmark_position_rel_y # y
msg_landmark.s_x = s_landmark_x**2 # s_x^2
msg_landmark.s_y = s_landmark_y**2 # s_y^2
# add to the landmarks message
msg_landmarks.landmarks.append(msg_landmark)
marker = self.create_landmark_marker(i, landmark_position_rel_gt[0][0], landmark_position_rel_gt[1][0])
marker_array_msg.markers.append(marker)
if len(msg_landmarks.landmarks) > 0:
self.pub_landmarks.publish(msg_landmarks)
self.pub_landmark_markers.publish(marker_array_msg)
def calibrate_imu(self):
"""
This method will wait for the IMU to output values and wait for the values to get steady
:return:
"""
self.log("Calibrating IMU", 5)
activated = False
calibrated = False
reads = 0
# Wait for the IMU to start outputting values
while not activated and not rospy.is_shutdown():
try:
data = self.ser.readline()
if len(data.split(",")) == 16:
self.log("IMU is outputting values", 7)
activated = True
else:
reads = reads + 1
if reads > 50:
self.log("IMU is not outputting values. Restarting.", 2, alert="warn")
self.connection()
reads = 0
except KeyboardInterrupt:
raise KeyboardInterrupt()
finally:
self.rate.sleep()
# Wait for yaw values to stabilize
# rtcDate,rtcTime,Q6_1,Q6_2,Q6_3,RawAX,RawAY,RawAZ,RawGX,RawGY,RawGZ,RawMX,RawMY,RawMZ,output_Hz,\r\n
reads = 0
th_hist = [1e5]*3
while not calibrated and not rospy.is_shutdown():
try:
data = self.ser.readline().split(",")
q = [float(data[2]), float(data[3]), float(data[4]), 0]
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) <= 1.0:
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
else:
continue
th_hist.pop(0)
th_hist.append(transformations.euler_from_quaternion(q)[2])
# Compute second derivative
deriv = (th_hist[2]-th_hist[1]) - (th_hist[1]-th_hist[0])
if reads > 50 and abs(deriv) < 1e-6:
self.log("IMU is calibrated.", 5)
acc_x = float(data[5])
acc_y = float(data[6])
acc_z = float(data[7])
self.acc_ratio = 9.82/np.linalg.norm([acc_x, acc_y, acc_z])
calibrated = True
else:
self.log("IMU is calibrating.", 7)
reads = reads + 1
except KeyboardInterrupt:
raise KeyboardInterrupt()
finally:
self.rate.sleep()
self.calibration = False
return True
def __cb_odom(self, msg):
self._pose_stamped.header = msg.header
self._pose_stamped.pose = msg.pose.pose
self._pose_2d.x = msg.pose.pose.position.x
self._pose_2d.y = msg.pose.pose.position.y
self._pose_2d.theta = transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])[2]
def pose_handler(self, pose):
# Get x, y and yaw
self.x = pose.position.x
self.y = pose.position.y
orientation_q = pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(_, _, self.theta) = euler_from_quaternion(orientation_list)
def stateCallback(self, odometry_msg):
#Odometry.pose message comes in as an odom frame.
#Need to convert to UTM for true North heading error.
pose_in = PoseStamped()
pose_in.header = odometry_msg.header
pose_in.pose = odometry_msg.pose.pose
if self.tfBuffer.can_transform(pose_in.header.frame_id,"utm",rospy.Time.now(),rospy.Duration.from_sec(0.5)):
self.pose_state = self.tfListener.transformPose("utm",pose_in)
else:
rospy.logwarn("{}: No tf available at time requested.".format(rospy.get_name()))
return
#heading_error
q_state = (self.pose_state.pose.orientation.x,
self.pose_state.pose.orientation.y,
self.pose_state.pose.orientation.z,
self.pose_state.pose.orientation.w)
eul_state = transform.euler_from_quaternion(q_state,'sxyz')
q_set = (self.pose_set.pose.orientation.x,
self.pose_set.pose.orientation.y,
self.pose_set.pose.orientation.z,
self.pose_set.pose.orientation.w)
eul_set = transform.euler_from_quaternion(q_set,'sxyz')
dY = self.pose_set.pose.position.y-self.pose_state.pose.position.y
dX = self.pose_set.pose.position.x-self.pose_state.pose.position.x
psi_set = atan2(dY,dX)
psi_err = -(psi_set-eul_state[2])
# if the heading error is greater than pi/5 and the speed setpoint is U, set to 0.0.
if abs(psi_err) > pi/5 and abs(self.pids[0].getSetpoint())>0:
self.pids[0].setSetpoint(0.0)
elif abs(psi_err) < pi/5 and abs(self.pids[0].getSetpoint())==0:
self.pids[0].setSetpoint(self.U)
if abs(psi_err)>pi:
if psi_err<0:
psi_err+=2*pi
else:
psi_err-=2*pi
self.pids[1].__integral=0.0
self.pids[1].__derivative=0.0
self.pids[1].__previouserror=0.0
feedback = np.array([odometry_msg.twist.twist.linear.x,psi_err])
self.last_feedback = feedback
self.last_feedback_time = odometry_msg.header.stamp
def data_gen(self):
imu = self.imu_hist[-1]
q = [
imu.orientation.x, imu.orientation.y, imu.orientation.z,
imu.orientation.w
]
w = [
imu.angular_velocity.x, imu.angular_velocity.y,
imu.angular_velocity.z
]
[roll, pitch, yaw] = trf.euler_from_quaternion(q)
yield [q[2], q[3], w[2], yaw]
def read_position_from_gazebo(self):
try:
gazebo_coordinates = self.get_model_state_srv(
self.ego_vehicle_id, "")
except rospy.ServiceException as e:
rospy.logerr("Error receiving gazebo state: %s", e.message)
gazebo_coordinates = None
if gazebo_coordinates is not None:
roll, pitch, yaw = transformations.euler_from_quaternion([
gazebo_coordinates.pose.orientation.x,
gazebo_coordinates.pose.orientation.y,
gazebo_coordinates.pose.orientation.z,
gazebo_coordinates.pose.orientation.w
])
self.pos_x = gazebo_coordinates.pose.position.x + 2 * cos(yaw)
self.pos_y = gazebo_coordinates.pose.position.y + 2 * sin(yaw)
rotate = transformations.quaternion_from_euler(0, 0, -PI / 2)
rotate_2 = transformations.quaternion_from_euler(
0, 0, 2 * PI - yaw)
angle_rotated = transformations.quaternion_multiply(
rotate_2, rotate)
roll_r, pitch_r, yaw_r = transformations.euler_from_quaternion(
angle_rotated)
self.angle = degrees(yaw_r) + 180
# moveToXY(self, vehID, edgeID, lane, x, y, angle=-1001.0, keepRoute=1)
traci.vehicle.moveToXY(
self.ego_vehicle_id,
traci.vehicle.getRoadID(self.ego_vehicle_id),
traci.vehicle.getLaneIndex(self.ego_vehicle_id), self.pos_x,
self.pos_y, self.angle, 0)
traci.vehicle.setSpeed(self.ego_vehicle_id,
fabs(gazebo_coordinates.twist.linear.x))
def get_pos(self):
try:
(trans, rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("tf Error")
return None
euler = transformations.euler_from_quaternion(rot)
#print euler[2] / pi * 180
x = trans[0]
y = trans[1]
th = euler[2] / pi * 180
return (x, y, th)
def obtain_mobileplatform_states(self):
try:
(trans,rot) = self.tf_listener.lookupTransform( '/map','/base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("tf Error")
return None
euler = transformations.euler_from_quaternion(rot)
self.mobile_platform_joints_value[0] = trans[0]
self.mobile_platform_joints_value[1] = trans[1]
self.mobile_platform_joints_value[2] = euler[2]
# print("mobile platform x is:", self.mobile_platform_joints_value[0])
# print("mobile platform y is:", self.mobile_platform_joints_value[1])
print("the angle is:", self.mobile_platform_joints_value[2])
def publish_euler_imu(self, imu_reading):
euler = transformations.euler_from_quaternion([
imu_reading.orientation.x, imu_reading.orientation.y,
imu_reading.orientation.z, imu_reading.orientation.w
])
imu_quaternion = "quaternion = ({}, {}, {}, {})".format(
imu_reading.orientation.x, imu_reading.orientation.y,
imu_reading.orientation.z, imu_reading.orientation.w)
imu_euler = "euler = ({}, {}, {})".format(euler[0], euler[1], euler[2])
imu_euler_deg = "euler_deg = ({}, {}, {})".format(
degrees(euler[0]), degrees(euler[1]), degrees(euler[2]))
euler_imu_string = imu_quaternion + "\n" + imu_euler + "\n" + imu_euler_deg
self.imu_euler_pub.publish(euler_imu_string)
def send(self, waypoints_data):
waypoints_payload = []
for row in waypoints_data:
waypoint_dict = {}
waypoint_dict["name"] = "Ponto " + str(row[0]) # "Ponto ID"
waypoint_dict["lat"] = float(row[10])
waypoint_dict["lng"] = float(row[11])
waypoint_dict["map_id"] = 1
waypoint_dict["x"] = float(row[1])
waypoint_dict["y"] = float(row[2])
waypoint_dict["teta"] = transformations.euler_from_quaternion(
[row[4], row[5], row[6],
row[7]])[2] # Converts quaternion to RPY (teta = yaw)
waypoints_payload.append(waypoint_dict)
requests.post(
"http://localhost:5002/waypoints",
json=waypoints_payload) # TODO: Update with the real endpoit
rospy.loginfo("Waypoints sent")
def trace(self, state, serial):
base_to_sensor = self.base_to_sensor_tfs[serial]
self.pose_stamped.pose.position.x = state[0]
self.pose_stamped.pose.position.y = state[1]
self.pose_stamped.pose.orientation = quaternion_from_euler(0.0, 0.0, state[2])
pose_sensor_frame = tf2_geometry_msgs.do_transform_pose(self.pose_stamped, base_to_sensor)
# ray trace
trace_angle = euler_from_quaternion([
pose_sensor_frame.pose.orientation.w,
pose_sensor_frame.pose.orientation.x,
pose_sensor_frame.pose.orientation.y,
pose_sensor_frame.pose.orientation.z,
])[2]
distance = self.range_method.calc_range(
pose_sensor_frame.pose.position.x,
pose_sensor_frame.pose.position.y,
trace_angle
)
return distance
def compass_callback(self, msg):
"""
Processes the date from the visual compass and draws the graph
"""
canvas = np.zeros((300,300), dtype=np.uint8)
angle = ((2 * math.pi - euler_from_quaternion(msg.pose.pose.pose.orientation)[2]) - 0.5 * math.pi)
length = msg.confidence * self.scale
vektor = (int(math.cos(angle)*length), int(math.sin(angle)*length))
center = (canvas.shape[0]/2, canvas.shape[1]/2)
point = (center[0] + vektor[0], center[1] + vektor[1])
img = cv2.arrowedLine(canvas, tuple(center), tuple(point), (255,255,255), thickness = 3, tipLength = 0.3)
output_str = "{} Deg | {}%".format(int(math.degrees(msg.orientation)), int(msg.confidence * 100))
text_margin = 10
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(img, output_str, (text_margin,canvas.shape[1] - text_margin), font, 1,(255,255,255),1,cv2.LINE_AA)
cv2.imshow("Visual Compass", img)
cv2.waitKey(1)
def updateOutput(self, event):
if self.setpoint_valid and self.enabled:
if self.set_pose.header.frame_id != 'base_link':
self.set_pose.header.stamp = rospy.Time.now()
try:
self.tfListener.waitForTransform(
self.set_pose.header.frame_id, "base_link",
rospy.Time.now(), rospy.Duration(1.0))
error_vector = self.tfListener.transformPose(
"base_link", self.set_pose)
except:
rospy.logwarn("No TF, setting feedback to all zeros")
error_vector = self.set_pose
error_vector.pose.position.x = 0
error_vector.pose.position.y = 0
error_vector.pose.orientation = Quaternion(0, 0, 0, 1)
else:
error_vector = self.set_pose
quat = (error_vector.pose.orientation.x,
error_vector.pose.orientation.y,
error_vector.pose.orientation.z,
error_vector.pose.orientation.w)
eul = transform.euler_from_quaternion(quat)
rospy.logdebug(error_vector)
self.last_feedback = [
-error_vector.pose.position.x, -error_vector.pose.position.y,
-eul[2]
]
wrench_output = WrenchStamped()
dt = (event.current_real - event.last_real).to_sec()
wrench_output.wrench.force.x = self.pids[0].update(
self.last_feedback[0], dt)
wrench_output.wrench.force.y = self.pids[1].update(
self.last_feedback[1], dt)
wrench_output.wrench.torque.z = self.pids[2].update(
self.last_feedback[2], dt)
wrench_output.header.stamp = rospy.Time.now()
wrench_output.header.frame_id = 'DP'
self.pub.publish(wrench_output)
error_msg = Readings()
error_msg.header.stamp = rospy.Time.now()
error_msg.header.frame_id = '[x,y,Psi]'
error_msg.data = [
self.pids[0].error, self.pids[1].error, self.pids[2].error
]
self.error_pub.publish(error_msg)
wrench_output.wrench.force.x = self.pids[0].P
wrench_output.wrench.force.y = self.pids[1].P
wrench_output.wrench.torque.z = self.pids[2].P
self.p_pub.publish(wrench_output)
wrench_output.wrench.force.x = self.pids[0].I
wrench_output.wrench.force.y = self.pids[1].I
wrench_output.wrench.torque.z = self.pids[2].I
self.i_pub.publish(wrench_output)
wrench_output.wrench.force.x = self.pids[0].D
wrench_output.wrench.force.y = self.pids[1].D
wrench_output.wrench.torque.z = self.pids[2].D
self.d_pub.publish(wrench_output)
def odom_orientation(self, q):
''' function for calculating value in degrees from quaternion'''
y, p, r = transformations.euler_from_quaternion([q.w, q.x, q.y, q.z])
return y * 180 / pi
def compute_imu_msg(self):
# Create new message
try:
# Get data
self.get_imu_data()
[
q1, q2, q3, accuracy, roll, pitch, yaw, w_x, w_y, w_z, acc_x,
acc_y, acc_z
] = self.parse_msg()
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix + "imu"
imu_msg.header.stamp = rospy.Time.now() # + rospy.Duration(0.5)
if ((q1 * q1) + (q2 * q2) + (q3 * q3)) < 1:
q4 = [
q1, q2, q3,
np.sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)))
]
else:
self.log("Inconsistent readings from IMU", 2, alert="warn")
return True
# Compute the Orientation Quaternion
new_q = Quaternion()
new_q.x = q1
new_q.y = q2
new_q.z = q3
new_q.w = q4
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001
]
# Angular Velocity
imu_msg.angular_velocity.x = round(w_x, 4)
imu_msg.angular_velocity.y = round(w_y, 4)
imu_msg.angular_velocity.z = round(w_z, 4)
# Datasheet says:
# - Noise Spectral Density: 0.015dps/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(0.015/np.sqrt(20), 2)
# factor = 0.02
# imu_msg.angular_velocity_covariance = [
# diag, w_x*factor, w_x*factor,
# w_y*factor, diag, w_y*factor,
# w_z*factor, w_z*factor, diag
# ]
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = 0.0001
imu_msg.angular_velocity_covariance[4] = 0.0001
imu_msg.angular_velocity_covariance[8] = 0.0001
# imu_msg.angular_velocity_covariance = [-1] * 9
# Linear Acceleration
imu_msg.linear_acceleration.x = round(acc_x, 4)
imu_msg.linear_acceleration.y = round(acc_y, 4)
imu_msg.linear_acceleration.z = round(acc_z, 4)
# imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = 9.82
# imu_msg.linear_acceleration_covariance = [-1] * 9
# Datasheet says:
# - Noise Spectral Density: 230microg/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(230e-6/np.sqrt(20), 2)/256.
# factor = 0.02/256.
# imu_msg.linear_acceleration_covariance = [
# diag, acc_x*factor, acc_x*factor,
# acc_y*factor, diag, acc_y*factor,
# acc_z*factor, acc_z*factor, diag
# ]
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.001
imu_msg.linear_acceleration_covariance[4] = 0.001
imu_msg.linear_acceleration_covariance[8] = 0.001
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion(
[new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(
r, p, y))
except SerialException as serial_exc:
self.log(
"SerialException while reading from IMU: {}".format(
serial_exc), 3)
self.calibration = True
except ValueError as val_err:
self.log("Value error from IMU data - {}".format(val_err), 5)
self.val_exc = self.val_exc + 1
except Exception as imu_exc:
self.log(imu_exc, 3)
raise imu_exc
def compute_imu_msg(self):
"""
This method reads data from the Openlog Artemis output.
We receive 3 coord Quaternion, which causes discontinuities in rotation.
RPY coords are stored and reused to compute a new quaternion
"""
# Get data
# rtcDate,rtcTime,Q6_1,Q6_2,Q6_3,RawAX,RawAY,RawAZ,RawGX,RawGY,RawGZ,RawMX,RawMY,RawMZ,output_Hz,\r\n
try:
data = self.ser.readline().split(",")
except SerialException:
self.log("Error reading data from IMU", 2, alert="warn")
self.fails = self.fails + 1
if self.fails > 10:
self.connection()
return
else:
self.fails = 0
if len(data) < 16:
self.log("IMU Communication failed", 4, alert="warn")
return
# Compute Absolute Quaternion
try:
q = [float(data[2]), float(data[3]), float(data[4]), 0]
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) > 1.0:
self.log("Inconsistent IMU readings", 4, alert="warn")
return
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
except ValueError:
self.log("Error converting IMU message - {}".format(data), 5, alert="warn")
return
# Compute Linear Acceleration
acc = [round(float(data[5])*self.acc_ratio, 3), round(float(data[6])*self.acc_ratio, 3), round(float(data[7])*self.acc_ratio, 3)]
self.acc_hist.pop(0)
self.acc_hist.append(acc)
# Compute Angular Velocity
# w_x = float(data[8])*3.14/180
# w_y = float(data[9])*3.14/180
# w_z = float(data[10])*3.14/180
# Compute Angular Velocity from Quat6
lq = self.last_imu[-1].orientation
euler1 = transformations.euler_from_quaternion([lq.x, lq.y, lq.z, lq.w])
euler2 = transformations.euler_from_quaternion(q)
curr_time = rospy.Time.now()
dt = (curr_time - self.last_imu[-1].header.stamp).to_sec()
w = []
for i in range(0, 3):
dth = euler2[i] - euler1[i]
# The IMU Quaternion jumps need to be handled
while (3.14 < dth) or (dth < -3.14):
dth = dth - np.sign(dth)*2*np.pi
# Keep euler angles in [-2p and 2pi]
self.euler[i] += dth
while (2*np.pi < self.euler[i]) or (self.euler[i] < -2*np.pi):
self.euler[i] = self.euler[i] - np.sign(self.euler[i])*2*np.pi
w.append(round(dth/dt, 4))
q_est = transformations.quaternion_from_euler(self.euler[0], self.euler[1], self.euler[2])
new_q = Quaternion()
new_q.x = q_est[0]
new_q.y = q_est[1]
new_q.z = q_est[2]
new_q.w = q_est[3]
# Compute IMU Msg
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix+"imu"
imu_msg.header.stamp = curr_time
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.001, 0, 0,
0, 0.001, 0,
0, 0, 0.001
]
# Angular Velocity
imu_msg.angular_velocity.x = w[0]
imu_msg.angular_velocity.y = w[1]
imu_msg.angular_velocity.z = w[2]
# Datasheet says:
# - Noise Spectral Density: 0.015dps/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(0.015/np.sqrt(20), 2)
# factor = 0.02
# imu_msg.angular_velocity_covariance = [
# diag, w_x*factor, w_x*factor,
# w_y*factor, diag, w_y*factor,
# w_z*factor, w_z*factor, diag
# ]
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = -1
imu_msg.angular_velocity_covariance[0] = 0.01
imu_msg.angular_velocity_covariance[4] = 0.01
imu_msg.angular_velocity_covariance[8] = 0.05
# imu_msg.angular_velocity_covariance = [-1] * 9
# Linear Acceleration
acc = [0, 0, 0]
for idx in range(0, 3):
data = [a[idx] for a in self.acc_hist]
res = butter_lowpass_filter(data, cutoff=0.5, fs=10.0, order=1)
acc[idx] = res[-1]
imu_msg.linear_acceleration.x = acc[0]
imu_msg.linear_acceleration.y = acc[1]
imu_msg.linear_acceleration.z = acc[2]
# imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = 9.82
# imu_msg.linear_acceleration_covariance = [-1] * 9
# Datasheet says:
# - Noise Spectral Density: 230microg/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(230e-6/np.sqrt(20), 2)/256.
# factor = 0.02/256.
# imu_msg.linear_acceleration_covariance = [
# diag, acc_x*factor, acc_x*factor,
# acc_y*factor, diag, acc_y*factor,
# acc_z*factor, acc_z*factor, diag
# ]
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.01
imu_msg.linear_acceleration_covariance[4] = 0.01
imu_msg.linear_acceleration_covariance[8] = 0.05
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion([new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(r, p, y))
self.last_imu.pop(0)
self.last_imu.append(imu_msg)
def odom_orientation(self, q):
y, p, r = transformations.euler_from_quaternion([q.w, q.x, q.y, q.z])
return y * 180 / pi
def handle_imu(self, data, timestamp):
"""
Accelerometer aX,aY,aZ milli g
Gyro gX,gY,gZ Degrees per Second
Magnetometer mX,mY,mZ micro Tesla
Temperature imu_degC Degrees Centigrade
"""
# Compute Quaternion, if data is consistent
q = [float(data[self.data_headers["IMU"]["QX"]]),
float(data[self.data_headers["IMU"]["QY"]]),
float(data[self.data_headers["IMU"]["QZ"]]),
0]
# Data may be inconsistent, try to fix it
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) > 1.0:
q_norm = (q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])
q[0] = q[0]/q_norm
q[1] = q[1]/q_norm
q[2] = q[2]/q_norm
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
# Save IMU history for calibration
self.imu_history.pop(0)
self.imu_history.append(transformations.euler_from_quaternion(q)[2])
if not self.imu_calibrated:
# Wait for heading readings to stabilize
if self.reads > 50 and abs((self.imu_history[2]-self.imu_history[1]) - (self.imu_history[1]-self.imu_history[0])) < 1e-6:
self.log("IMU is calibrated.", 5)
self.imu_calibrated = True
else:
new_q = Quaternion()
new_q.x = q[0]
new_q.y = q[1]
new_q.z = q[2]
new_q.w = q[3]
# Compute Linear Acceleration - converting to ms-2
acc_x = float(data[self.data_headers["IMU"]["AX"]])/1000
acc_y = float(data[self.data_headers["IMU"]["AY"]])/1000
acc_z = float(data[self.data_headers["IMU"]["AZ"]])/1000
# Compute Angular Velocity
w_x = float(data[self.data_headers["IMU"]["GX"]])*np.pi/180
w_y = float(data[self.data_headers["IMU"]["GY"]])*np.pi/180
w_z = float(data[self.data_headers["IMU"]["GY"]])*np.pi/180
# Compute IMU Msg
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix+"imu"
imu_msg.header.stamp = timestamp
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.01, 0, 0,
0, 0.01, 0,
0, 0, 0.01
]
# Angular Velocity
imu_msg.angular_velocity.x = w_x
imu_msg.angular_velocity.y = w_y
imu_msg.angular_velocity.z = w_z
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = -1
# Linear Acceleration
imu_msg.linear_acceleration.x = acc_x
imu_msg.linear_acceleration.y = acc_y
imu_msg.linear_acceleration.z = acc_z
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.01
imu_msg.linear_acceleration_covariance[4] = 0.01
imu_msg.linear_acceleration_covariance[8] = 0.01
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion([new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(r, p, y))
return True
def follow(self, path):
waypoints = []
for pose in path.poses:
waypoints.append([pose.pose.position.x, pose.pose.position.y])
# Listener loop
i = 0
# Errors
last_lin_error, accu_lin_error = [0, 0]
last_ang_error, accu_ang_error = [0, 0]
# rospy.sleep(3.0)
r = rospy.Rate(30) # 30hz
while not rospy.is_shutdown() and i < len(waypoints):
# Listener block
try:
trans = self.tfBuffer.lookup_transform(path.header.frame_id,
'base_footprint',
rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
current_theta = tf.euler_from_quaternion([
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
])[2]
x, y = waypoints[i]
x_error = x - trans.transform.translation.x
y_error = y - trans.transform.translation.y
linear_error = np.sqrt(x_error**2 + y_error**2)
angular_error = self.normalizeAngle(
np.arctan2(y_error, x_error) - current_theta)
kobuki_vel = Twist()
if linear_error <= 0.05:
self.pub.publish(Twist())
rospy.loginfo('Waypoint achieved: ' + str(waypoints[i]))
i = i + 1
continue
else:
param_names = rospy.get_param_names()
# Linear
kpv = rospy.get_param(
"/PID/Kpv") if "/PID/Kpv" in param_names else 6.6 #2.6
kiv = rospy.get_param(
"/PID/Kiv") if "/PID/Kiv" in param_names else 0.1 #0.004
kdv = rospy.get_param(
"/PID/Kdv") if "/PID/Kdv" in param_names else 1.100
accu_lin_error += linear_error
rate_error = linear_error - last_lin_error
last_lin_error = linear_error
v = kpv * linear_error + kiv * accu_lin_error + kdv * rate_error
# Angular
kpw = rospy.get_param(
"/PID/Kpw") if "/PID/Kpw" in param_names else 6.0 #14.000
kiw = rospy.get_param(
"/PID/Kiw") if "/PID/Kiw" in param_names else 0.0056
kdw = rospy.get_param(
"/PID/Kdw") if "/PID/Kdw" in param_names else 10.0 #4.5900
accu_ang_error += angular_error
rate_error = angular_error - last_ang_error
last_ang_error = angular_error
w = kpw * angular_error + kiw * accu_ang_error + kdw * rate_error
kobuki_vel.linear.x = v
if abs(kobuki_vel.linear.x) > self.v_cruising:
kobuki_vel.linear.x = self.v_cruising
kobuki_vel.angular.z = w
if abs(kobuki_vel.angular.z) > self.w_cruising:
kobuki_vel.angular.z = self.w_cruising * \
np.sign(kobuki_vel.angular.z)
# print self.waypoints[i]
# print [trans.transform.translation.x, trans.transform.translation.y]
# print angular_error
# print kobuki_vel
# print
self.pub.publish(kobuki_vel)
r.sleep()
