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