def publish_msgs(self):
t = rospy.Time.now()
tf = TransformStamped()
tf.header.stamp = t
tf.header.frame_id = "world"
tf.child_frame_id = "base_footprint"
tf.transform.translation.x = self.pos[0][0]
tf.transform.translation.y = self.pos[1][0]
tf.transform.translation.z = 0.0
q = transformations.quaternion_from_euler(0, 0, self.dir)
tf.transform.rotation.x = q[0]
tf.transform.rotation.y = q[1]
tf.transform.rotation.z = q[2]
tf.transform.rotation.w = q[3]
self.br.sendTransform(tf)
tf.header.frame_id = "base_footprint"
tf.child_frame_id = "steer"
tf.transform.translation.x = self.wheel_base
tf.transform.translation.y = 0.0
tf.transform.translation.z = 0.0
q = transformations.quaternion_from_euler(0, 0, self.steer)
tf.transform.rotation.x = q[0]
tf.transform.rotation.y = q[1]
tf.transform.rotation.z = q[2]
tf.transform.rotation.w = q[3]
self.br.sendTransform(tf)
self.pub_twist.publish(self.twist)
def path_angle(node1, node2): """Returns the angle between two pathfinding nodes. e.g. node1 is (0,0) and node2 is (1,1) the angle is 45 degrees.""" dx = node2[0] - node1[0] dy = node2[1] - node1[1] angle = math.degrees(math.atan2(dy, dx)) return Quaternion(*transformations.quaternion_from_euler(0, 0, angle))
def calibrate_imu(self):
"""
This method will save the current orientation as the offset. All future publications will be adjusted in relation
to the saved orientation. Readings com in [yaw, pitch, roll]
:return:
"""
self.log("Calibrating IMU", 3)
r = rospy.Rate(20)
calibrated = False
reads = 0
while not calibrated and not rospy.is_shutdown():
try:
reads = reads + 1
imu_data = self.read_data()
if reads > 100:
q_data = transformations.quaternion_from_euler(
imu_data[2], imu_data[1], imu_data[0])
self.imu_offset.x = float(q_data[0])
self.imu_offset.y = float(q_data[1])
self.imu_offset.z = float(q_data[2])
self.imu_offset.w = -float(q_data[3])
calibrated = True
self.calibration = False
else:
if reads == 1:
self.log("Discarding 100 readings for calibration", 3)
r.sleep()
except KeyboardInterrupt:
raise KeyboardInterrupt()
def goto(self, p):
rospy.loginfo("[Navi] goto %s" % p)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2] / 180.0 * pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb,
self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(60))
if not result:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("reach goal %s succeeded!" % p)
return True
def __init__(self):
rospy.init_node("utm_odom")
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
datum = rospy.get_param("~datum", False)
mag_dec = rospy.get_param('~magnetic_declination_radians', 0.0)
yaw_offset = rospy.get_param('~yaw_offset', 0.0)
self.do_tf = rospy.get_param('~broadcast_odom_base', False)
pub_gps = rospy.get_param('~pub_gps', False)
rotation_offset = yaw_offset + mag_dec
rospy.loginfo(
"Magnetic Declination is {}\n IMU Reference Offset is:{}\nTotal Yaw Offset for UTM->Odom is {}"
.format(mag_dec, yaw_offset, rotation_offset))
self.odom_tf = tf2_ros.TransformBroadcaster()
# Transform odom into UTM frame
if datum:
rospy.loginfo(
"Setting datum to Latitude: {}, Longitude: {}".format(
datum[0], datum[1]))
datumPoint = utm.fromLatLong(datum[0], datum[1], 0.0) # get the
(zone, band) = datumPoint.gridZone()
rospy.loginfo("UTM Zone: {}{}".format(zone, band))
self.datum = datumPoint.toPoint()
rospy.loginfo("UTM Translation is: (X,Y,Z)=({},{},{})".format(
self.datum.x, self.datum.y, self.datum.z))
else:
self.datum = utm.UTMPoint(easting=0.0,
northing=0.0,
altitude=0.0,
zone=zone,
band=band).toPoint()
rospy.logwarn(
"Setting datum to current zone origin. odom frame will have large values!"
)
T = TransformStamped()
T.header.frame_id = "utm"
T.header.stamp = rospy.Time.now()
T.child_frame_id = "odom"
quat = tf.quaternion_from_euler(0, 0, rotation_offset, axes='sxyz')
T.transform.rotation.x = quat[0]
T.transform.rotation.y = quat[1]
T.transform.rotation.z = quat[2]
T.transform.rotation.w = quat[3]
T.transform.translation.x = self.datum.x # Northing
T.transform.translation.y = self.datum.y # Easting
T.transform.translation.z = self.datum.z # Altitude
br = tf2_ros.StaticTransformBroadcaster()
br.sendTransform(T)
self.pub = rospy.Publisher("odometry/gps", Odometry, queue_size=10)
imu_sub = message_filters.Subscriber('imu/data', Imu)
nav_sub = message_filters.Subscriber("gps/fix", NavSatFix)
ts = message_filters.ApproximateTimeSynchronizer([imu_sub, nav_sub],
10, 0.25)
ts.registerCallback(self.publish)
if pub_gps:
filt_sub = rospy.Subscriber("odometry/filtered", Odometry,
self.filt_cb)
rospy.spin()
def update_imu(self):
"""
Reads all characters in the buffer until finding \r\n
Messages should have the following format: "Q: w x y z | A: x y z | G: x y z"
:return:
"""
# Create new message
try:
imuMsg = Imu()
# Set the sensor covariances
imuMsg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
imuMsg.angular_velocity_covariance = [-1., 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.linear_acceleration_covariance = [
-1., 0, 0, 0, 0, 0, 0, 0, 0
]
imu_data = self.read_data()
if len(imu_data) == 0:
self.log("IMU is not answering", 2)
return
q_data = transformations.quaternion_from_euler(
imu_data[2], imu_data[1], imu_data[0])
q1 = Quaternion()
q1.x = float(q_data[0])
q1.y = float(q_data[1])
q1.z = float(q_data[2])
q1.w = float(q_data[3])
q_off = self.imu_offset
new_q = transformations.quaternion_multiply(
[q1.x, q1.y, q1.z, q1.w], [q_off.x, q_off.y, q_off.z, q_off.w])
imuMsg.orientation.x = new_q[0]
imuMsg.orientation.y = new_q[1]
imuMsg.orientation.z = new_q[2]
imuMsg.orientation.w = new_q[3]
# Handle message header
imuMsg.header.frame_id = "base_link_imu"
imuMsg.header.stamp = rospy.Time.now() + rospy.Duration(nsecs=5000)
self.imu_reading = imuMsg
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 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 goto(self, p):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
return True
def valueChanged(self):
self.msg.header.stamp = rospy.Time.now()
self.msg.transform.translation.x = self.tx.value()
self.msg.transform.translation.y = self.ty.value()
self.msg.transform.translation.z = self.tz.value()
q = transformations.quaternion_from_euler(self.rr.value(),
self.rp.value(),
self.ry.value())
self.msg.transform.rotation.x = q[0]
self.msg.transform.rotation.y = q[1]
self.msg.transform.rotation.z = q[2]
self.msg.transform.rotation.w = q[3]
self.tf_static_broadcast.sendTransform(self.msg)
def set_pose(self, p):
if self.move_base is None:
return False
x, y, th = p
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'map'
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
q = transformations.quaternion_from_euler(0.0, 0.0, th / 180.0 * pi)
pose.pose.pose.orientation.x = q[0]
pose.pose.pose.orientation.y = q[1]
pose.pose.pose.orientation.z = q[2]
pose.pose.pose.orientation.w = q[3]
self.set_pose_pub.publish(pose)
return True
def publish_rotation(self, header_frame_id, header_stamp, orientation,
confidence):
# type: (TODO, TODO, float, float) -> None
"""
Builds the ros message and publishes the result.
"""
msg = PoseWithCertaintyStamped()
# Create PoseWithCertaintyStamped-message where only the orentation is used
msg.header.frame_id = header_frame_id
msg.header.stamp = header_stamp
msg.pose.pose.pose.orientation = quaternion_from_euler(
0, 0, (orientation + self.orientation_offset) %
(2 *
math.pi)) # Orientation changes about PI in the second game half
msg.pose.confidence = confidence
# Publish VisualCompassMsg-message
self.pub_compass.publish(msg)
def goto(self, p):
while (self.center_flag_ == False):
self.rate.sleep()
msg1 = Twist()
msg1 = rospy.wait_for_message('/robot1/find_robot2',
Twist,
timeout=None)
if (abs(msg1.linear.x - 666.0) <= 0.01):
self.find_flag_ = True
if (self.find_flag_ == True):
return True
#Fill the ROS navigation points
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2] / 180.0 * pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
#Publish the navigation points and set the callback function
self.move_base.send_goal(goal, self._done_cb, self._active_cb,
self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(120))
if not result:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("reach goal %s succeeded!" % p)
return True
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 createSpawnMessage(
model_name='',
model_path='',
xyz=[0, 0, 0],
rpy=[0, 0, 0],
reference_frame='world'
):
"""
Create a gazebo_msgs.msg.SpawnModel request message from the parameters.
model_name -- Name of the model in the simulation
model_path -- Path to the sdf file of the model
xyz -- array of length 3 with the xyz coordinates
rpy -- array of length 3 with the rpy rotations. These are converted to a quaternion
reference_frame -- the reference frame of the coordinates
returns -- SpawnModelRequest instance
"""
request = SpawnModelRequest()
request.model_name = model_name
file = open(resource_retriever.get_filename(model_path, use_protocol=False))
request.model_xml = file.read()
file.close()
request.initial_pose.position.x = xyz[0]
request.initial_pose.position.y = xyz[1]
request.initial_pose.position.z = xyz[2]
quaternion = transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
request.initial_pose.orientation.x = quaternion[0]
request.initial_pose.orientation.y = quaternion[1]
request.initial_pose.orientation.z = quaternion[2]
request.initial_pose.orientation.w = quaternion[3]
request.reference_frame = reference_frame
return request
def fix_callback(msg):
if msg.longitude is None:
return
EPSG4612 = pyproj.Proj("+init=EPSG:4612")
EPSG2451 = pyproj.Proj("+init=EPSG:2451")
y,x = pyproj.transform(EPSG4612, EPSG2451, msg.longitude, msg.latitude)
tf = TransformStamped()
tf.header.stamp = msg.header.stamp
tf.header.frame_id = "world"
tf.child_frame_id = "gps"
tf.transform.translation.x = x
tf.transform.translation.y = y
tf.transform.translation.z = 0.0
q = transformations.quaternion_from_euler(0, 0, 0)
tf.transform.rotation.x = q[0]
tf.transform.rotation.y = q[1]
tf.transform.rotation.z = q[2]
tf.transform.rotation.w = q[3]
br.sendTransform(tf)
rospy.loginfo("{}, {}".format(x, y))
def set_position_in_gazebo(self, vehicle_msg):
gazebo_coordinates = ModelState()
gazebo_coordinates.model_name = self.ego_vehicle_id
gazebo_coordinates.pose.position.x = vehicle_msg.pos_x
gazebo_coordinates.pose.position.y = vehicle_msg.pos_y
gazebo_coordinates.pose.position.z = 0
vehicle_msg.heading = vehicle_msg.heading + PI / 2
heading_quaternion = transformations.quaternion_from_euler(
0, 0, vehicle_msg.heading)
gazebo_coordinates.pose.orientation.x = heading_quaternion[0]
gazebo_coordinates.pose.orientation.y = heading_quaternion[1]
gazebo_coordinates.pose.orientation.z = heading_quaternion[2]
gazebo_coordinates.pose.orientation.w = heading_quaternion[3]
# gazebo_coordinates.twist.linear.x = vehicle_msg.velocity
gazebo_coordinates.reference_frame = "world"
try:
self.set_model_state_srv(gazebo_coordinates)
except rospy.ServiceException as e:
rospy.logerr("Error receiving gazebo state: %s", e.message)
from geometry_msgs.msg import Quaternion from tf_conversions.transformations import quaternion_from_euler import math q1 = quaternion_from_euler(-2.735, 1.561, -2.744) q_rot = quaternion_from_euler(-math.pi / 2, 0, 0) q2 = quaternion_multiply(q_rot, q1) q2.normalize()
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 parseTask(self,task):
response = TriggerResponse()
# IF THIS IS A PLAN THAT REQUIRES SUPERVISION
if self._aware:
# if the task isn't the starting task.
if task.action!='START':
# if the energy cost mu or std = 0, then this task is buggy.
if task.cost_mu==0 or task.cost_std ==0:
rospy.logwarn("Buggy task, skipping. {}".format(task))
self.plan.skipTask()
return self.parseTask(self.plan.getTask())
# GET WAYPOINT COMPONENTS READY TO SEND TO THE GUIDANCE NODE
geo_msg = GeoPoseStamped()
geo_msg.header.frame_id="utm"
geo_msg.header.stamp = rospy.Time.now()
self.checkValidTask(task)
# Look at the action property and decide on what to do from there:
if task.action == "WP":
# waypoint task, configure for AP mode to a GPS waypoint.
if self.changeMode("/tau_com/AP"):
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
# turn off the timing lock
self._timing_lock = False
response.success = True
response.message = "Waypoint Task Selected, executing"
return response
else:
response.success = False
response.message = "Mode not configured to AP."
return response
elif task.action == "HP":
# hold position task, configure DP mode at a GPS waypoint and orientation for a set time.
if self.changeMode("/tau_com/DP"):
geo_msg.pose.position.altitude=0
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(*tf.quaternion_from_euler(0,0,task.data[2]))
self.hptime = task.data[3]
self.hptask = True
self.task_pub.publish(geo_msg)
# turn off the timing lock
self._timing_lock = False
response.success = True
response.message ="Hold Pose Task Selected, executing."
return response
else:
response.success = False
response.message = "Mode not configured to DP."
return response
elif task.action == "ROOT":
# root task reached, notify the operator that the mission is complete.
if self.changeMode("__none"):
self.mode="idle"
self._timing_lock = False
self.status_pub.publish("ASV currently in mode: {}".format(self.mode))
rospy.loginfo("Final task completed, now idling.")
response.success = True
response.message = "Final task completed, now idling."
return response
else:
rospy.logerr("Couldn't disable output topic, but mission complete.")
response.success = False
response.message = "Couldn't Idle for some reason?"
return response
elif task.action == "START":
# start task, is a waypoint task to move the vehicle to the starting position.
if self.changeMode("/tau_com/AP"):
self._start_flag = True
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
self._timing_lock = False
rospy.loginfo("Moving to starting waypoint of plan.")
response.success = True
response.message = "AP mode to starting point."
return response
else:
rospy.logerr("Couldn't configure to AP mode.")
response.success = False
response.message = "Couldn't configure to AP mode."
return response
elif task.action == "HOME":
# home task, is a waypoint task to move the vehicle to the starting position.
if self.changeMode("/tau_com/AP"):
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
self._timing_lock = False
rospy.loginfo("Moving to rendezvous point.")
response.success = True
response.message = "AP mode to home point."
return response
else:
rospy.logerr("Couldn't configure to AP mode.")
response.success = False
response.message = "Couldn't configure to AP mode."
return response
else:
rospy.logerr("Task of type {} is not supported, getting next task.".format(task.action))
self.plan.skipTask()
return self.parseTask(self.plan.getTask())
linearacceleration_z))
# Integrate between last acceleration and this one
vt_x = integrate.cumtrapz([lastaccel[0], accel[0]],[lasttime, timestamp] initial=0) * delta_time
vt_y = integrate.cumtrapz([lastaccel[1], accel[1]],[lasttime, timestamp] initial=0) * delta_time
vt_z = integrate.cumtrapz([lastaccel[2], accel[2]],[lasttime, timestamp] initial=0) * delta_time
pt_x = integrate.cumtrapz([vt_x[0], vt_x[1]],[lasttime, timestamp] initial=0) * delta_time
pt_y = integrate.cumtrapz([vt_y[0], vt_y[1]],[lasttime, timestamp] initial=0) * delta_time
pt_z = integrate.cumtrapz([vt_z[0], vt_z[1]],[lasttime, timestamp] initial=0) * delta_time
translation_x = pt_x[1]
translation_y = pt_y[1]
translation_z = pt_z[1]
# we want translation and rotation
tf2_msg.transform.translation = Vector3(translation_x,
translation_y,
translation_z)
angular_velocity = np.array((float(measurements[5]),
float(measurements[6]),
float(measurements[7]))
rotation = angular_velocity * delta_time
tf2_msg.transform.rotation = quaternion_from_euler(rotation[0],
rotation[1],
rotation[2])
pub.publish(tf2_msg)
