def update_kalman(ar_tags):
print "started update"
rospy.wait_for_service('innovation')
update = rospy.ServiceProxy('innovation', NuSrv)
listener = tf.TransformListener()
while True:
try:
try:
(trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
except:
print "Couldn't look up transform"
continue
lin = Vector3()
quat = Quaternion()
lin.x = trans[0]
lin.y = trans[1]
lin.z = trans[2]
quat.x = rot[0]
quat.y = rot[1]
quat.z = rot[2]
quat.w = rot[3]
transform = Transform()
transform.translation = lin
transform.rotation = quat
test = update(transform, ar_tags['ar1'])
print "Service call succeeded"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def matrix_to_transform(H):
''' Convert a 4x4 homogeneous transform matrix to a ros Transform message '''
scale, shear, angles, trans, persp = transformations.decompose_matrix(H)
q = transformations.quaternion_from_euler(*angles)
transform_msg = Transform()
transform_msg.translation = Vector3(*trans)
transform_msg.rotation = Quaternion(*q)
return transform_msg
def multi_dof_joint_state(cls, msg, obj):
obj.header = decode.header(msg, obj.header, "")
obj.joint_names = msg["joint_names"]
for i in msg['_length_trans']:
trans = Transform()
trans.translation = decode.translation(msg, trans.translation, i)
trans.rotation = decode.rotation(msg, trans.rotation, i)
obj.transforms.append(trans)
for i in msg['_length_twist']:
tw = Twist()
tw.linear = decode.linear(msg, tw.linear, i)
tw.angular = decode.angular(msg, tw.angular, i)
obj.twist.append(twist)
for i in msg['_length_twist']:
wr = Wrench()
wr.force = decode.force(msg, wr.force, i)
wr.torque = decode.torque(msg, wr.torque, i)
obj.wrench.append(wr)
return(obj)
def update(self, meas_mbes, odom):
# Compute AUV MBES ping ranges
particle_tf = Transform()
particle_tf.translation = odom.pose.pose.position
particle_tf.rotation = odom.pose.pose.orientation
tf_mat = matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat, np.matmul(tf_mat, self.base2mbes_mat))
mbes_meas_ranges = pcloud2ranges(meas_mbes, m2auv)
# Measurement update of each particle
weights = []
for i in range(self.pc):
self.particles[i].meas_update(mbes_meas_ranges)
weights.append(self.particles[i].w)
weights_array = np.asarray(weights)
# Add small non-zero value to avoid hitting zero
weights_array += 1.e-30
return weights_array
def transform_frame_to_map(self, cloud, tf):
rospy.loginfo("creating transformer")
if (tf is None):
self.listener = tf.TransformListener()
else:
self.listener = TransformationStore().msg_to_transformer(tf)
rospy.sleep(5)
self.child_camera_frame = cloud.header.frame_id
rospy.loginfo("doing conversion")
t = self.listener.getLatestCommonTime("map", self.child_camera_frame)
tr_r = self.listener.lookupTransform("map", self.child_camera_frame, t)
tr = Transform()
tr.translation = Vector3(tr_r[0][0], tr_r[0][1], tr_r[0][2])
tr.rotation = Quaternion(tr_r[1][0], tr_r[1][1], tr_r[1][2],
tr_r[1][3])
#self.transform_frame_to_map = tr
tr_s = TransformStamped()
tr_s.header = std_msgs.msg.Header()
tr_s.header.stamp = rospy.Time.now()
tr_s.header.frame_id = 'map'
tr_s.child_frame_id = self.child_camera_frame
tr_s.transform = tr
t_kdl = self.transform_to_kdl(tr_s)
points_out = []
for p_in in pc2.read_points(cloud, field_names=["x", "y", "z", "rgb"]):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append([p_out[0], p_out[1], p_out[2], p_in[3]])
fil_fields = []
for x in cloud.fields:
if (x.name in "x" or x.name in "y" or x.name in "z"
or x.name in "rgb"):
fil_fields.append(x)
res = pc2.create_cloud(cloud.header, fil_fields, points_out)
return res
def send_waypoint(self, x, y, z, yaw, pitch, roll):
trajectory_msg = MultiDOFJointTrajectory()
trajectory_msg.header.stamp = rospy.Time.now()
desired_position = Vector3()
desired_position.x = x
desired_position.y = y
desired_position.z = z
desired_rotation = Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
roll, pitch, yaw))
transform = Transform()
transform.translation = desired_position
transform.rotation = desired_rotation
point = MultiDOFJointTrajectoryPoint()
point.transforms.append(transform)
trajectory_msg.points.append(point)
self.trajectory_pub.publish(trajectory_msg)
rospy.loginfo("Published waypoint")
def pingCB(self, auv_ping, exp_ping, auv_pose, pf_pose):
try:
particle_tf = Transform()
particle_tf.translation = auv_pose.pose.pose.position
particle_tf.rotation = auv_pose.pose.pose.orientation
tf_mat = self.matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat,
np.matmul(tf_mat, self.base2mbes_mat))
auv_ping_ranges = self.ping2ranges(auv_ping)
exp_ping_ranges = self.pcloud2ranges(exp_ping, m2auv)
# print "------"
# print len(auv_ping_ranges)
# print len(exp_ping_ranges)
# TODO: do the trimming of pings better than this
idx1 = np.round(
np.linspace(0,
len(exp_ping_ranges) - 1,
self.max_height)).astype(int)
idx2 = np.round(
np.linspace(0,
len(auv_ping_ranges) - 40,
self.max_height)).astype(int)
self.waterfall.append(
abs(auv_ping_ranges[idx2] - exp_ping_ranges[idx1]))
self.active_auv_poses.append(auv_pose)
beams_vec = self.ping2vecs(exp_ping, m2auv)
self.active_pf_pings.append(beams_vec[idx1])
if len(self.waterfall) > self.max_height:
self.waterfall.pop(0)
self.active_auv_poses.pop(0)
self.active_pf_pings.pop(0)
self.new_msg = True
except rospy.ROSInternalException:
pass
def get_mbes_goal(self):
# Find particle's mbes pose without broadcasting/listening to tf transforms
particle_tf = Transform()
particle_tf.translation = self.p_pose.position
particle_tf.rotation = self.p_pose.orientation
mat_part = matrix_from_tf(particle_tf)
self.trans_mat = self.m2o_tf_mat.dot(mat_part.dot(self.mbes_tf_mat))
trans = TransformStamped()
trans.transform.translation.x = translation_from_matrix(self.trans_mat)[0]
trans.transform.translation.y = translation_from_matrix(self.trans_mat)[1]
trans.transform.translation.z = translation_from_matrix(self.trans_mat)[2]
trans.transform.rotation = Quaternion(*quaternion_from_matrix(self.trans_mat))
# Build MbesSimGoal to send to action server
mbes_goal = MbesSimGoal()
mbes_goal.mbes_pose.header.frame_id = self.map_frame
mbes_goal.mbes_pose.header.seq = self.index
mbes_goal.mbes_pose.header.stamp = rospy.Time.now()
mbes_goal.mbes_pose.transform = trans.transform
mbes_goal.beams_num.data = self.beams_num
return mbes_goal
def publish_trajectory(self, trajectory):
trajectory_msg = MultiDOFJointTrajectory()
trajectory_msg.header.frame_id = 'world'
trajectory_msg.joint_names = ['base']
for idx in range(len(trajectory)):
point = trajectory[idx]
point['time'] = trajectory[idx]['time']
transform = Transform()
transform.translation = Vector3(*(point['point'].tolist()))
transform.rotation = Quaternion(
*(vec_to_quat(point['velocity']).tolist()))
velocity = Twist()
velocity.linear = Vector3(*(point['velocity'].tolist()))
acceleration = Twist()
acceleration.linear = Vector3(*(point['acceleration'].tolist()))
trajectory_msg.points.append(
MultiDOFJointTrajectoryPoint([transform], [velocity],
[acceleration],
rospy.Duration(point['time'])))
trajectory_msg.header.stamp = rospy.Time.now()
self.traj_pub.publish(trajectory_msg)
def predict_meas(self, pose_t, beams_num):
# Find particle's mbes pose without broadcasting/listening to tf transforms
particle_tf = Transform()
particle_tf.translation = pose_t.position
particle_tf.rotation = pose_t.orientation
mat_part = matrix_from_tf(particle_tf)
self.trans_mat = self.m2o_tf_mat.dot(mat_part.dot(self.mbes_tf_mat))
trans = TransformStamped()
trans.transform.translation.x = translation_from_matrix(self.trans_mat)[0]
trans.transform.translation.y = translation_from_matrix(self.trans_mat)[1]
trans.transform.translation.z = translation_from_matrix(self.trans_mat)[2]
trans.transform.rotation = Quaternion(*quaternion_from_matrix(self.trans_mat))
# Build MbesSimGoal to send to action server
mbes_goal = MbesSimGoal()
mbes_goal.mbes_pose.header.frame_id = self.map_frame
mbes_goal.mbes_pose.header.stamp = rospy.Time.now()
mbes_goal.mbes_pose.transform = trans.transform
mbes_goal.beams_num.data = beams_num
# Get result from action server
self.ac_mbes.send_goal(mbes_goal)
mbes_pcloud = PointCloud2()
if self.ac_mbes.wait_for_result(rospy.Duration(0.03)):
mbes_res = self.ac_mbes.get_result()
# Pack result into PointCloud2
mbes_pcloud = mbes_res.sim_mbes
mbes_pcloud.header.frame_id = self.map_frame
got_result = True
else:
got_result = False
return (got_result, mbes_pcloud)
def update(self):
self.get_logger().info("Start update")
buf = self.sock.recvfrom(1000)[0]
data = buf.decode().rstrip('\x00')
self.get_logger().info("Received : {}".format(data))
if data == "***shutdown***":
self.get_logger().info("Client Shutdown")
self.shutdownClient = True
elif data == "***restart***":
self.get_logger().info("Client Restart")
self.restart.data = True
self.restart_pub.publish(self.restart)
self.restart.data = False
self.currentStep = self.currentStep + 1
if self.currentStep != self.config.max_steps:
udp_str = data
self.debug_string.data = udp_str
self.debug_pub.publish(self.debug_string)
self.sensorsMsgFromString(data)
self.ctrl_pub.publish(self.torcs_ctrl)
self.torcs_sensors_pub.publish(self.torcs_sensors)
self.globalSpeed_pub.publish(self.globalSpeed)
self.globalPose_pub.publish(self.globalPose)
self.globalRPY_pub.publish(self.globalRPY)
self.track_pub.publish(self.track)
self.opponents_pub.publish(self.opponents)
self.focus_pub.publish(self.focus)
self.speed_pub.publish(self.speed)
if self.torcs_ctrl.meta == 1:
self.restart.data = True
self.restart_pub.publish(self.restart)
self.restart.data = False
broadcast = tf2_ros.TransformBroadcaster(self)
transform_stamped = TransformStamped()
transform_stamped.header = Header()
transform_stamped.header.frame_id = "world"
transform_stamped.header.stamp = self.get_clock().now().to_msg()
transform_stamped.child_frame_id = "baselink"
transform = Transform()
translation = Vector3()
translation.x = self.globalPose.pose.position.x
translation.y = self.globalPose.pose.position.y
translation.z = self.globalPose.pose.position.z
transform.translation = translation
quat_base = Quaternion()
quat_base.x = self.globalRPY.vector.x
quat_base.y = self.globalRPY.vector.y
quat_base.z = self.globalRPY.vector.z
transform.rotation = quat_base
transform_stamped.transform = transform
broadcast.sendTransform(transform_stamped)
action = self.ctrlMsgToString()
data = action # haya
else:
data = "(meta 1)"
if self.sock.sendto(
data.encode(),
(self.config.host_name, self.config.server_port)) < 0:
self.get_logger().info("cannot send data")
self.sock.close()
sys.exit()
else:
self.get_logger().info("Sending: {}".format(data))
self.get_logger().info("End update")
def pose2trans(pose):
trans = Transform()
trans.rotation = pose.orientation
trans.translation = pose.position
return trans
def toTransformMsg(tq):
t = TransformMSG()
t.rotation = toRotMsg(tq)
t.translation = toTranslationMsg(tq)
return t
def follow_ar_tag(zumy, ar_tags):
listener = tf.TransformListener()
zumy_vel = rospy.Publisher('%s/cmd_vel' % zumy, Twist, queue_size=2)
rate = rospy.Rate(10)
print ar_tags
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform(ar_tags['ar1'],
ar_tags['arZ'],
rospy.Time(0))
rospy.wait_for_service('innovation')
KF = rospy.ServiceProxy('innovation', NuSrv)
transformMessage = Transform()
transformMessage.translation = Vector3(trans[0], trans[1],
trans[2])
transformMessage.rotation = Quaternion(rot[0], rot[1], rot[2],
rot[3])
kfMessage = NuSrvRequest()
kfMessage.transform = transformMessage
kfMessage.origin_tag = ar_tags['ar1']
KF(kfMessage)
(trans, rot) = listener.lookupTransform(ar_tags['arZ'],
ar_tags['ar1'],
rospy.Time(0))
except Exception as e:
try:
(trans,
rot) = listener.lookupTransform('zumy1', ar_tags['ar1'],
rospy.Time(0))
except Exception as f:
print f
print e
continue
# YOUR CODE HERE
# The code should compute the twist given
# the translation and rotation between arZ and ar1
# Then send it publish it to the zumy
if np.linalg.norm(trans) > 0.02:
rbt = return_rbt(trans=trans, rot=rot)
velocity, omega = compute_twist(rbt=rbt)
theta = math.atan(trans[1] / trans[0])
if trans[0] < 0:
theta = math.pi + theta
print(theta)
if abs(theta) < .05:
twistMessage = Twist()
l = Vector3()
l.x = .2
l.y = 0
l.z = 0
twistMessage.linear = l
v = Vector3()
v.x = 0
v.y = 0
v.z = 0
twistMessage.angular = v
zumy_vel.publish(twistMessage)
else:
print 'hi'
twistMessage = Twist()
l = Vector3()
l.x = 0
l.y = 0
l.z = 0
twistMessage.linear = l
v = Vector3()
v.x = 0
v.y = 0
v.z = theta * 0.5
twistMessage.angular = v
zumy_vel.publish(twistMessage)
else:
twistMessage = Twist()
l = Vector3()
l.x = 0
l.y = 0
l.z = 0
twistMessage.linear = l
v = Vector3()
v.x = 0
v.y = 0
v.z = 0
twistMessage.angular = v
zumy_vel.publish(twistMessage)
rate.sleep()
def _tf_callback(self, msg):
""" called whenever a new tfMessage is received. Creates a list of detected objects
and iterates through these to return their type and location w.r.t. map and odometry frames."""
detected_objects = []
for item in self.item_list:
if self.tf_listener.frameExists(item):
detected_objects.append(item)
rospy.loginfo(detected_objects)
if detected_objects:
for item in detected_objects:
object_h = item # type: str
obj_type = self.get_type(object_h)
t = self.tf_listener.getLatestCommonTime(
"/camera_link", object_h)
#rospy.loginfo("Object detected")
#(pos, rot) = self.tf_listener.lookupTransform("/camera_rgb_optical_frame", , t)
(pos, rot) = self.tf_listener.lookupTransform(
"/camera_link", object_h, t)
# create the object pose
obj_pos = geometry_msgs.msg.PoseStamped()
obj_pos.header.frame_id = "/camera_link"
obj_pos.pose.position.x = pos[0]
obj_pos.pose.position.y = pos[1]
obj_pos.pose.position.z = pos[2]
obj_pos.pose.orientation.x = rot[0]
obj_pos.pose.orientation.y = rot[1]
obj_pos.pose.orientation.z = rot[2]
obj_pos.pose.orientation.w = rot[3]
obj_pose = self.tf_listener.transformPose(
"/base_link", obj_pos)
obj_pose.header.frame_id = obj_type
transform = Transform()
transform.translation = obj_pose.pose.position
transform.rotation = obj_pose.pose.orientation
# Insert new Transform into a TransformStamped object and add to the tf tree
new_tfstamped = TransformStamped()
new_tfstamped.child_frame_id = obj_type
new_tfstamped.header.frame_id = "/base_link"
new_tfstamped.transform = transform
new_tfstamped.header.stamp = t
# add to tf list
self.tf_message = tfMessage(transforms=[new_tfstamped])
self.tf_publisher.publish(self.tf_message)
self.or_pub.publish(obj_pose)
if obj_type in self.in_map_log:
t = self.tf_listener.getLatestCommonTime("/map", object_h)
#(pos, rot) = self.tf_listener.lookupTransform("/camera_rgb_optical_frame", object_h, t)
(pos, rot) = self.tf_listener.lookupTransform(
"/camera_link", object_h, t)
# get the object pose
obj_pos = geometry_msgs.msg.PoseStamped()
obj_pos.header.frame_id = "/camera_link"
obj_pos.pose.position.x = pos[0]
obj_pos.pose.position.y = pos[1]
obj_pos.pose.position.z = pos[2]
obj_pos.pose.orientation.x = rot[0]
obj_pos.pose.orientation.y = rot[1]
obj_pos.pose.orientation.z = rot[2]
obj_pos.pose.orientation.w = rot[3]
self.object_to_map(obj_pos, obj_type, t)
self.in_map_log.remove(obj_type)
def transform_from_pose(cls, pose):
transform = Transform()
transform.translation = pose.position
transform.rotation = pose.orientation
return transform
import math
if __name__ == '__main__':
if len(sys.argv) < 4:
print("Usage: rosrun cascaded_pid_control set_point.py target_x target_y target_z [target_yaw]")
exit(-1)
rospy.init_node("SetPoint")
x, y, z = float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])
yaw = 0
if len(sys.argv) > 4:
yaw = float(sys.argv[4])
print("Will set location of the drone to {}, {}, {}. Yaw: {} degree".format(x, y, z, yaw))
traj_pub = rospy.Publisher("/CascadedPidControl/trajectory", MultiDOFJointTrajectory, queue_size=1, latch=True)
trajectory = MultiDOFJointTrajectory()
trajectory.header.frame_id = 'world'
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ['base']
transform = Transform()
transform.translation = Vector3(x, y, z)
transform.rotation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, yaw * math.pi / 180.0, 'rxyz').tolist())
trajectory.points.append(MultiDOFJointTrajectoryPoint([transform], [Twist()], [Twist()], rospy.Duration(0)))
traj_pub.publish(trajectory)
while not rospy.is_shutdown():
rospy.sleep(rospy.Duration(3))
rospy.signal_shutdown("Job done.")
rospy.spin()
def visual_odometry_core(self):
"""
Runs pose estimation using a visual odometry pipeline assuming that images are obtained from a monocular camera
set on a duckiebot wondering around duckietown
:return: The estimated transformation between the current image and the one from last call.
:rtype: geometry_msgs.TransformStamped
"""
parameters = self.parameters
train_image = self.images[1]
query_image = self.images[0]
# Initialize transformation between camera frames
t = Transform()
############################################
# MAIN BODY #
############################################
processed_data_plotter = DataPlotter(train_image, query_image, parameters)
# Instantiate histogram logic filter
histogram_filter = HistogramLogicFilter(train_image.width, train_image.height)
start = time.time()
if parameters.matcher == 'KNN':
# K-Nearest Neighbors matcher
bf = cv2.BFMatcher()
matches = bf.knnMatch(train_image.descriptors, query_image.descriptors, k=parameters.knn_neighbors)
else:
# Brute force matcher
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(train_image.descriptors, query_image.descriptors)
end = time.time()
print("TIME: Matching done. Elapsed time: %s", end - start)
# Initialize fitness trackers
fitness = float('-inf')
max_fit = fitness
# Explore all the weight values
for weight in parameters.knn_weight:
# Filter knn matches by best to second best match ratio
if parameters.matcher == 'KNN':
start = time.time()
matches = knn_match_filter(matches, weight)
end = time.time()
print("TIME: Distance filtering of matches done. Elapsed time: %s", end - start)
# Filter histograms by gaussian function fitting
if parameters.filter_by_histogram:
start = time.time()
histogram_filter.filter_matches_by_histogram_fitting(
matches, train_image.keypoints, query_image.keypoints, parameters.threshold_angle,
parameters.threshold_length)
end = time.time()
print("TIME: Histogram filtering done. Elapsed time: %s", end - start)
fitness = histogram_filter.angle_fitness + histogram_filter.length_fitness
# Store current configuration as best configuration if fitness is new maximum
if fitness > max_fit:
histogram_filter.save_configuration()
max_fit = fitness
# Recover best configuration from histogram filtering (for best weight)
if parameters.filter_by_histogram and histogram_filter.saved_configuration is not None:
unfiltered_matches = matches
matches = histogram_filter.saved_configuration.filter_data_by_histogram()
# Publish the results of histogram filtering
if parameters.plot_histogram_filtering:
self.histogram_img = processed_data_plotter.plot_histogram_filtering(unfiltered_matches, matches, histogram_filter)
n_final_matches = len(matches)
# Lists of final filtered matches
matched_train_points = [None] * n_final_matches
matched_query_points = [None] * n_final_matches
for match_index, match_object in enumerate(matches):
matched_train_points[match_index] = train_image.keypoints[match_object.queryIdx].pt
matched_query_points[match_index] = query_image.keypoints[match_object.trainIdx].pt
try:
[h, w] = [query_image.height, query_image.width]
start = time.time()
# Split between far-region and close region matches
match_distance_filter = \
DistanceFilter(matched_query_points, matched_train_points, self.camera_K,
self.camera_D, (h, w), (parameters.shrink_x_ratio, parameters.shrink_y_ratio))
match_distance_filter.split_by_distance_mask(self.stingray_mask)
end = time.time()
print("TIME: Mask filtering done. Elapsed time: %s", end - start)
if parameters.plot_masking:
self.mask_img = processed_data_plotter.plot_displacements_from_distance_mask(match_distance_filter)
start = time.time()
n_distant_matches = len(match_distance_filter.rectified_distant_query_points)
if n_distant_matches > 0:
rot_hypothesis = []
# Average sign of rotation
rot_sign = np.sign(np.average(np.array(matched_query_points) - np.array(matched_train_points), axis=0)[0])
# Calculate two rotation hypothesis for all far matches assuming that they lay distant to camera
for distant_q_p, distant_t_p in \
zip(match_distance_filter.rectified_distant_query_points,
match_distance_filter.rectified_distant_train_points):
a = (distant_t_p[0] - distant_q_p[0]) \
/ np.sqrt((distant_t_p[0]*distant_q_p[0])**2 + distant_t_p[0]**2 + distant_q_p[0]**2 + 1)
rot = np.arcsin(a)
# Use hypothesis whose sign is consistent with the average sign
for rot_h in np.unique(rot):
if np.sign(rot_h) == rot_sign:
rot_hypothesis.append(-rot_h)
else:
rot_hypothesis.append(rot_h)
rot_hypothesis = np.unique(rot_hypothesis)
rot_hypothesis_rmse = np.zeros((len(rot_hypothesis), 1))
# Select the best hypothesis using 1 point RANSAC
for hypothesis_index in range(0, len(rot_hypothesis)):
hypothesis = rot_hypothesis[hypothesis_index]
rot_mat = np.array([[np.cos(hypothesis), 0, np.sin(hypothesis)],
[0, 1, 0],
[-np.sin(hypothesis), 0, np.cos(hypothesis)]])
rotated_train_points = np.matmul(
np.append(np.reshape(match_distance_filter.rectified_distant_train_points, (n_distant_matches, 2)),
np.ones((n_distant_matches, 1)), axis=1),
rot_mat)
# Calculate rmse of hypothesis with all peripheral points
error = rotated_train_points[:, 0:2] - np.reshape(
match_distance_filter.rectified_distant_query_points, (n_distant_matches, 2))
rot_hypothesis_rmse[hypothesis_index] = np.sum(np.sqrt(np.sum(np.power(error, 2), axis=1)))
theta = rot_hypothesis[np.argmin(rot_hypothesis_rmse)]
self.last_theta = theta
else:
# If there were not enough matches to estimate a new theta, use the one from previous iteration
theta = self.last_theta
z_rot_mat = np.array([[np.cos(theta), np.sin(theta), 0], [-np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
t_vec = [self.joy_command.v / 30.0, 0, 0]
# Calculate quaternion of z-mapped rotation
[roll, pitch, yaw] = rotation_matrix_to_euler_angles(z_rot_mat)
z_quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
end = time.time()
print("TIME: Pose estimation done. Elapsed time: %s", end - start)
t.translation = Vector3(t_vec[0], t_vec[1], t_vec[2])
t.rotation = Quaternion(z_quaternion[0], z_quaternion[1], z_quaternion[2], z_quaternion[3])
except Exception:
raise
return t, self.histogram_img, self.mask_img
def toTransformMsg(tq):
t = TransformMSG()
t.rotation = toRotMsg(tq)
t.translation = toTranslationMsg(tq)
return t
if __name__ == '__main__':
rospy.init_node('baselink_tf')
listener = tf.TransformListener()
liste = rospy.Publisher('transfor_baselink',Transform,queue_size=1)
rate = rospy.Rate(10.0)
msg = Transform()
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/ar_marker_6', '/camera', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
msg.translation = trans
msg.rotation = rot
print(trans)
rospy.loginfo('I heard translation %s',msg.translation)
rospy.loginfo('I heard rotation %s',msg.rotation)
liste.publish(msg)
rate.sleep()
def create_path(self, odometry, gates,
last_visited_gate): # types are Odometry, [Pose], Pose
start_position = odometry.pose.pose.position
start_velocity = geo.rotate_vector_wrt_quaternion(
odometry.pose.pose.orientation, odometry.twist.twist.linear)
if last_visited_gate is not None:
visited_index = None
for i in range(len(gates)):
if geo.distance(last_visited_gate.position,
gates[i].position) < 1.0:
visited_index = i
if visited_index is not None:
gates = gates[visited_index + 1:] + gates[:visited_index + 1]
# in case of re-planning, determine the current nearest waypoint. look forward about some specific
# time into the current path (currently 1s) and let that future waypoint be the starting waypoint of
# re-planning
waypoints = []
start = geo.vector_to_list(start_position)
look_ahead_time = 1
nearest_waypoint_idx = None
planning_waypoint_idx = None
current_gate_location = geo.vector_to_list(
self.gates[self.target_gate_idx].position)
if self.current_path is not None:
nearest_waypoint_idx = self.search_for_nearest_waypoint(
geo.vector_to_list(start_position))
if nearest_waypoint_idx >= 0:
current_path = self.current_path["path"]
waypoints.append(
current_path[nearest_waypoint_idx]["point_as_list"])
waypoint_time = current_path[nearest_waypoint_idx]["time"]
planning_waypoint_idx = nearest_waypoint_idx
current_path_length = len(current_path)
while planning_waypoint_idx < current_path_length:
planning_waypoint = current_path[planning_waypoint_idx]
if planning_waypoint[
"time"] - waypoint_time < look_ahead_time:
planning_waypoint_idx += 1
else:
break
if planning_waypoint_idx >= current_path_length:
planning_waypoint_idx = current_path_length - 1
start_position = current_path[planning_waypoint_idx][
"point_as_list"]
waypoints.append(start_position)
if len(waypoints) == 0:
print(
"The first planning or a re-planning from scratch is needed.")
orientation = np.array(current_gate_location) - np.array(start)
orientation_normalized = orientation / np.linalg.norm(orientation)
# append a point 5 meters behind along with the current position as the starting position
waypoints.append(
(np.array(start) - 5 * orientation_normalized).tolist())
waypoints.append(start)
else:
print("Re-planning")
# we already have two position around current position,
# we still need 2 positionp around the gate
#
# make sure we won't going back if we're still heading to the current gate
# and if we're too close to the target gate, head for the next too.
target_gate_location = geo.vector_to_list(
self.gates[self.target_gate_idx].position)
if self.is_cross_gate(self.target_gate_idx, waypoints[0],
waypoints[1]):
if self.target_gate_idx + 1 < len(self.gates):
rospy.loginfo(
"About to cross gate{}. Heading for next gate at index {}".
format(self.target_gate_idx, self.target_gate_idx + 1))
gate = self.gates[self.target_gate_idx + 1]
else:
rospy.loginfo(
"Gates are about to be all passed. Stop planning.")
gate = self.gates[self.target_gate_idx]
self.stop_planning = True
return
elif self.distance_to_gate(waypoints[0], self.target_gate_idx) <= 0.5 or \
self.distance_to_gate(waypoints[1], self.target_gate_idx) <= 0.5:
if self.target_gate_idx + 1 < len(self.gates):
rospy.loginfo(
"Close to gate {}. Heading for gate at index {}".format(
self.target_gate_idx, self.target_gate_idx + 1))
gate = self.gates[self.target_gate_idx + 1]
else:
rospy.loginfo(
"Gates are about to be all passed. Stop planning.")
gate = self.gates[self.target_gate_idx]
self.stop_planning = True
return
else:
gate = self.gates[self.target_gate_idx]
# append waypoints +- 0.5 meters around the next gate
offsets = [-0.5, 0.5]
for offset in offsets:
waypoints.append(
geo.vector_to_list(
geo.point_plus_vector(
gate.position,
geo.quaternion_to_vector(gate.orientation, offset))))
# wp = np.array(waypoints)
# w0 = wp[:-1]
# w1 = wp[1:]
# diff = w0 - w1
# norm = np.linalg.norm(diff, axis=1)
# new_path_chord_length = np.sum(norm)
# scale derivative w.r.t. total chord length
# if deriv is not None:
# old_path_chord_length = self.current_path["chord_length"]
# ratio = min(new_path_chord_length / old_path_chord_length, 1.0)
# deriv = list(ratio * v for v in deriv)
raw_waypoints_marker = Marker()
raw_waypoints_marker.header.stamp = rospy.Time.now()
raw_waypoints_marker.header.frame_id = "world"
raw_waypoints_marker.color = ColorRGBA(1.0, 1.0, 0.0, 1.0)
raw_waypoints_marker.scale = Vector3(0.5, 0.5, 0.5)
raw_waypoints_marker.type = Marker.SPHERE_LIST
raw_waypoints_marker.action = Marker.ADD
raw_waypoints_marker.id = 1
for wp in waypoints:
raw_waypoints_marker.points.append(Point(wp[0], wp[1], wp[2]))
self.raw_waypoints_viz_pub.publish(raw_waypoints_marker)
path = SmoothedPath()
path.fit(waypoints)
trajectory_points = []
def visit_cb(pt, deriv1, deriv2, s, t):
# curvature is calcuated as norm(deriv1 x deriv2) / norm(deriv1)**3
# see: https://en.wikipedia.org/wiki/Curvature#Local_expressions_2
d1xd2 = np.cross(deriv1, deriv2)
norm_d1 = np.linalg.norm(deriv1)
norm_d2 = np.linalg.norm(deriv2)
if norm_d1 > 1e-5:
c = np.linalg.norm(d1xd2) / norm_d1**3
else:
c = 0
# the first order derivative is given as ds/dt, where s is the arc length and t is the internal parameter of the spline,
# not time.
# because of this, the magnitude of first order derivative is not the same with viable speed,
# but nevertheless, the direction of the derivative of them are the same.
# also note that unit normal vector at point is just the normalized second order derivative,
# it is in the opposite direction of the radius vector
# the cross product of unit tangent vector and unit normal vector
# is also mentioned as unit binormal vector
if norm_d1 > 1e-5:
unit_d1 = deriv1 / norm_d1
else:
unit_d1 = np.array([0.0, 0.0, 0.0])
if norm_d2 > 1e-5:
unit_d2 = deriv2 / norm_d2
else:
unit_d2 = np.array([0.0, 0.0, 0.0])
unit_binormal = np.cross(unit_d1, unit_d2)
ds = 0
if len(trajectory_points) > 0:
last_point = trajectory_points[-1]
ds = s - last_point['s']
trajectory_points.append({
't':
t,
's':
s,
'point_as_list':
pt,
'derivative':
deriv1,
'derivative2':
deriv2,
'ds':
ds,
'point':
Vector3(*(pt.tolist())),
'speed':
self.max_speed,
'speed_direction':
Vector3(*(unit_d1.tolist())),
'unit_normal':
Vector3(*(unit_d2.tolist())),
'unit_binormal':
Vector3(*(unit_binormal.tolist())),
'curvature':
c
})
path.visit_at_interval(self.ds, visit_cb, self.trajectory_length)
self.current_path = {"chord_length": 0, "path": trajectory_points}
# trajectory_points = [{'s': i * ds, 'speed': self.max_speed, 'curvature': geo.spline_distance_to_curvature(waypoint_spline, i*ds)} for i in range(30)]
vmin = self.min_speed
vmax = self.max_speed
amax = self.max_acceleration
def safe_speed(curvature, speed_neighbor, ds, accel=None):
if accel is not None:
return math.sqrt(speed_neighbor**2 + 2 * ds * accel)
centripetal = curvature * speed_neighbor**2
if centripetal >= amax:
return max(vmin, min(vmax, math.sqrt(abs(amax / curvature))))
remaining_acceleration = math.sqrt(amax**2 - centripetal**2)
# see /Planning Motion Trajectories for Mobile Robots Using Splines/
# (refered as Sprunk[2008] later) for more details (eq 3.21)
v_this = math.sqrt(speed_neighbor**2 +
2 * ds * remaining_acceleration)
return max(vmin, min(vmax, v_this))
#print(geo.magnitude_vector(start_velocity))
# trajectory_points[0]['speed'] = min(vmax, max(vmin, geo.magnitude_vector(start_velocity)))
trajectory_points[0]['speed'] = geo.magnitude_vector(start_velocity)
#print(trajectory_points[0]['speed'])
num_traj = len(trajectory_points)
for i in range(
num_traj - 1
): # iterate forwards, skipping first point which is fixed to current speed
trajectory_points[i + 1]['speed'] = safe_speed(
trajectory_points[i + 1]['curvature'],
trajectory_points[i]['speed'], trajectory_points[i + 1]['ds'])
# skip the backward phase for 2 reason:
# 1. we don't need a smooth stop. just complete the course
# asap
# 2. backward phase would change the start speed,
# usually slower than the current speed. this
# sudden change of speed would make the drone
# "jerking" between trajectory switch.
# for i in range(num_traj - 2):
# j = num_traj - i - 2 # iterate backwards, skipping both end points
# curvature = trajectory_points[j]['curvature']
# min_neighbor_speed = min(trajectory_points[j-1]['speed'],trajectory_points[j+1]['speed'])
# trajectory_points[j]['speed'] = safe_speed(curvature, min_neighbor_speed, trajectory_points[i+1]['ds'])
# Set velocity based on speed and direction
for point in trajectory_points:
point['velocity'] = geo.scalar_multiply(point['speed'],
point['speed_direction'])
# the relationship between angular velocity and linear velocity is:
# \omega = r x v / || r ||^2
# where r is the radius vector, v is linear velocity.
# see: https://en.wikipedia.org/wiki/Angular_velocity#Particle_in_three_dimensions
# note: with the anti-commutative law of cross product,
# unit binormal B = v x (-r) / (||v|| * ||r||) = r x v / (||v|| * ||r||)
# and curvature k = 1 / || r ||
# so, omega = || v || * k * B
omega = geo.scalar_multiply(
geo.magnitude_vector(point['velocity']) * point['curvature'],
point['unit_binormal'])
point['omega'] = omega
# Set time based on speed and distance
trajectory_points[0]['time'] = 0.0
for i in range(num_traj - 1):
ds = trajectory_points[i + 1]['ds']
prev_time = trajectory_points[i]['time']
# see Sprunk[2018] eq 3.20
ave_speed = 0.5 * (trajectory_points[i]['speed'] +
trajectory_points[i + 1]['speed'])
trajectory_points[i + 1]['time'] = prev_time + ds / ave_speed
# low pass filter on the designated speed.
lpf = LowPassFilter(trajectory_points[0]['speed'], 5)
for i in range(1, num_traj):
trajectory_points[i]['speed'] = lpf.update(
trajectory_points[i]['speed'], trajectory_points[i]['time'])
# print(geo.magnitude_vector(start_velocity))
# for pt in trajectory_points:
# print(pt['speed'])
# print("------")
# Set acceleration based on change in velocity
# we're assuming constant accelerations between waypoints,
# a is just \delta V / \delta t
for i in range(num_traj - 1):
t_current = trajectory_points[i]['time']
t_next = trajectory_points[i + 1]['time']
dt = t_next - t_current
v_current = trajectory_points[i]['velocity']
v_next = trajectory_points[i + 1]['velocity']
dv = geo.vector_from_to(v_current, v_next)
w_current = trajectory_points[i]['omega']
w_next = trajectory_points[i]['omega']
dw = geo.vector_from_to(w_current, w_next)
trajectory_points[i]['acceleration'] = geo.scalar_multiply(
1.0 / dt, dv)
trajectory_points[i]['acceleration_w'] = geo.scalar_multiply(
1.0 / dt, dw)
trajectory_points[-1]['acceleration'] = trajectory_points[-2][
'acceleration']
trajectory_points[-1]['acceleration_w'] = trajectory_points[-2][
'acceleration_w']
trajectory = MultiDOFJointTrajectory()
trajectory.header.frame_id = ''
trajectory.joint_names = ['base']
for idx in range(len(trajectory_points)):
point = trajectory_points[idx]
point['time'] = trajectory_points[idx]['time']
transform = Transform()
transform.translation = point['point']
transform.rotation = Quaternion(
*(geo.vector_to_quat(point['velocity']).tolist()))
velocity = Twist()
velocity.linear = point['velocity']
velocity.angular = point['omega']
acceleration = Twist()
acceleration.linear = point['acceleration']
acceleration.angular = point['acceleration_w']
trajectory.points.append(
MultiDOFJointTrajectoryPoint([transform], [velocity],
[acceleration],
rospy.Duration(point['time'])))
trajectory.header.stamp = rospy.Time.now()
return trajectory
