def state_cb(self, msg): if rospy.Time.now( ) - self.last_state_sub_time < self.state_sub_max_prd: return self.last_state_sub_time = rospy.Time.now() if self.target in msg.name: target_index = msg.name.index(self.target) twist = msg.twist[target_index] enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index]) self.last_ecef = self.enu_to_ecef(enu_pose[0]) self.last_enu = enu_pose self.last_odom = Odometry( header=navigator_tools.make_header(frame='/enu'), child_frame_id='/measurement', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( *self.last_enu)), twist=TwistWithCovariance(twist=twist)) self.last_absodom = Odometry( header=navigator_tools.make_header(frame='/ecef'), child_frame_id='/measurement', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( self.last_ecef, self.last_enu[1])), twist=TwistWithCovariance(twist=twist))
def publishPose(): covariance_ = [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 ] x = 0 y = 0 z = 0 qx = 0 qy = 0 qz = 0 qw = 1 time_now = rospy.Time.now() id_ = '/map' poseWCS = PoseWithCovarianceStamped() poseWCS.header.stamp = time_now poseWCS.header.frame_id = id_ poseWCS.pose.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)) poseWCS.pose.covariance = covariance_ initialpose_poseWCS_publisher.publish(poseWCS) poseWC = PoseWithCovariance() poseWC.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)) poseWC.covariance = covariance_ twistWC = TwistWithCovariance() twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) twistWC.covariance = covariance_ odom = Odometry() odom.header.stamp = time_now odom.header.frame_id = id_ odom.pose = poseWC odom.twist = twistWC fakelocalisation_poseWCS_publisher.publish(odom)
def publish_odom(self, *args): if self.last_odom is None or self.position_offset is None: return msg = self.last_odom if self.target in msg.name: header = mil_ros_tools.make_header(frame='/map') target_index = msg.name.index(self.target) twist = msg.twist[target_index] # Add position offset to make the start position (0, 0, -depth) position_np, orientation_np = mil_ros_tools.pose_to_numpy( msg.pose[target_index]) pose = mil_ros_tools.numpy_quat_pair_to_pose( position_np - self.position_offset, orientation_np) self.state_pub.publish(header=header, child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) header = mil_ros_tools.make_header(frame='/world') twist = msg.twist[target_index] pose = msg.pose[target_index] self.world_state_pub.publish( header=header, child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) else: # fail return
def state_cb(self, msg): if self.target in msg.name: target_index = msg.name.index(self.target) twist = msg.twist[target_index] enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index]) self.last_ecef = self.enu_to_ecef(enu_pose[0]) self.last_enu = enu_pose self.last_odom = Odometry( header=navigator_tools.make_header(frame='/enu'), child_frame_id='/base_link', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( *self.last_enu)), twist=TwistWithCovariance(twist=twist)) self.last_absodom = Odometry( header=navigator_tools.make_header(frame='/ecef'), child_frame_id='/base_link', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( self.last_ecef, self.last_enu[1])), twist=TwistWithCovariance(twist=twist))
def publish_sensors(self, msg): """ publishes noisy position values for each robot """ pwc = PoseWithCovariance() twc = TwistWithCovariance() auv = "akon" # X Meas rospy.loginfo_once("Normal Error on X") sigma = rospy.get_param("kalman/measurements/x_sigma") dot_sigma = rospy.get_param("kalman/measurements/x_dot_sigma") pwc.pose.position.x = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.x, sigma) pwc.covariance[0] = sigma**2 twc.twist.linear.x = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.x, dot_sigma) twc.covariance[0] = dot_sigma**2 # Y Meas rospy.loginfo_once("Normal Error on Y") sigma = rospy.get_param("kalman/measurements/y_sigma") dot_sigma = rospy.get_param("kalman/measurements/y_dot_sigma") pwc.pose.position.y = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.y, sigma) pwc.covariance[7] = sigma**2 twc.twist.linear.y = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.y, dot_sigma) twc.covariance[7] = dot_sigma**2 # Z Meas rospy.loginfo_once("Normal Error on Z") sigma = rospy.get_param("kalman/measurements/z_sigma") dot_sigma = rospy.get_param("kalman/measurements/z_dot_sigma") pwc.pose.position.z = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.z, sigma) pwc.covariance[14] = sigma**2 twc.twist.linear.z = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.z, dot_sigma) twc.covariance[14] = dot_sigma**2 pwc.pose.orientation = self.auvs[auv][ODOM_INDEX].pose.pose.orientation odom_msg = Odometry() odom_msg.header.seq = self.seq odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = "base_link" odom_msg.child_frame_id = odom_msg.header.frame_id odom_msg.pose = pwc odom_msg.twist = twc self.odom_sensor_pub.publish(odom_msg) self.seq += 1
def convert(data): global prevTime; global prevPose; global isInitIter# = True; currentTime = rospy.Time.now(); if (isInitIter): prevTime = currentTime; dt = 99999999999999999; else: dt = currentTime.to_sec() - prevTime.to_sec(); #Get delta time. if (isInitIter): prevPose = data.pose; #Note first pose is a PoseWithCovariance. isInitIter = False; currentPose = PoseWithCovariance(); currentPose = data.pose; #Note first pose is a PoseWithCovariance. currentTwist = TwistWithCovariance(); #Calculate velocities. currentTwist.twist.linear.x = (currentPose.pose.position.x - prevPose.pose.position.x)/dt; currentTwist.twist.linear.y = (currentPose.pose.position.y - prevPose.pose.position.y)/dt; currentTwist.twist.linear.z = (currentPose.pose.position.z - prevPose.pose.position.z)/dt; currentTwist.twist.angular.x = (currentPose.pose.orientation.x - prevPose.pose.orientation.x)/dt; currentTwist.twist.angular.y = (currentPose.pose.orientation.y - prevPose.pose.orientation.y)/dt; currentTwist.twist.angular.z = (currentPose.pose.orientation.z - prevPose.pose.orientation.z)/dt; currentTwist.covariance = data.pose.covariance; # Make the message to publish: odomMessage = Odometry(); odomMessage.header = data.header; #odomMessage.header.frame_id = "something custom?" odomMessage.child_frame_id = "base_link"; odomMessage.pose = currentPose; odomMessage.twist = currentTwist; # publish the message p.publish(odomMessage); prevPose = currentPose;
def __update_odometry(self, linear_offset, angular_offset, tf_delay): self.__heading = (self.__heading + angular_offset) % 360 q = tf.transformations.quaternion_from_euler( 0, 0, math.radians(self.__heading)) self.__pose.position.x += math.cos( math.radians(self.__heading )) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.y += math.sin( math.radians(self.__heading )) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.z = 0.33 self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) now = rospy.Time.now() + rospy.Duration(tf_delay) self.__tfb.sendTransform( (self.__pose.position.x, self.__pose.position.y, self.__pose.position.z), q, now, 'base_link', 'odom') o = Odometry() o.header.stamp = now o.header.frame_id = 'odom' o.child_frame_id = 'base_link' o.pose = PoseWithCovariance(self.__pose, None) o.twist = TwistWithCovariance() o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x o.twist.twist.angular.z = math.radians( self.__rotation_per_second) * self.__twist.angular.z
def __init__(self, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from geometry_msgs.msg import TwistWithCovariance self.twist = kwargs.get('twist', TwistWithCovariance())
def publish_estimates(self, timestamp, nav_estimate): ne = NetworkEstimate() for asset in self.asset2id.keys(): if "surface" in asset: continue # else: # print("publishing " + asset + "'s estimate") # Construct Odometry Msg for Asset nav_covpt = np.array(nav_estimate.pose.covariance).reshape(6, 6) nav_covtw = np.array(nav_estimate.twist.covariance).reshape(6, 6) mean, cov = self.filter.get_asset_estimate(asset) pose = Pose(Point(mean[0],mean[1],mean[2]), \ nav_estimate.pose.pose.orientation) pose_cov = np.zeros((6, 6)) pose_cov[:3, :3] = cov[:3, :3] pose_cov[3:, 3:] = nav_covpt[3:, 3:] pwc = PoseWithCovariance(pose, list(pose_cov.flatten())) tw = Twist(Vector3(mean[3], mean[4], mean[5]), nav_estimate.twist.twist.angular) twist_cov = np.zeros((6, 6)) twist_cov[:3, :3] = cov[3:6, 3:6] twist_cov[3:, 3:] = nav_covtw[3:, 3:] twc = TwistWithCovariance(tw, list(twist_cov.flatten())) h = Header(self.update_seq, timestamp, "map") o = Odometry(h, "map", pwc, twc) ae = AssetEstimate(o, asset) ne.assets.append(ae) self.asset_pub_dict[asset].publish(o) self.network_pub.publish(ne)
def transform_twist_with_covariance_to_global(self, pose_with_covariance, twist_with_covariance): global_twist = transform_local_twist_to_global( pose_with_covariance.pose, twist_with_covariance.twist) global_twist_cov = transform_local_twist_covariance_to_global( pose_with_covariance.pose, twist_with_covariance.covariance) return TwistWithCovariance(global_twist, global_twist_cov)
def __init__(self): rover_dimensions = rospy.get_param('/rover_dimensions', {"d1": 0.184, "d2": 0.267, "d3": 0.267, "d4": 0.256}) self.d1 = rover_dimensions["d1"] self.d2 = rover_dimensions["d2"] self.d3 = rover_dimensions["d3"] self.d4 = rover_dimensions["d4"] self.min_radius = 0.45 # [m] self.max_radius = 6.4 # [m] self.no_cmd_thresh = 0.05 # [rad] self.wheel_radius = rospy.get_param("/rover_dimensions/wheel_radius", 0.075) # [m] drive_no_load_rpm = rospy.get_param("/drive_no_load_rpm", 130) self.max_vel = self.wheel_radius * drive_no_load_rpm / 60 * 2 * math.pi # [m/s] self.should_calculate_odom = rospy.get_param("~enable_odometry", False) self.odometry = Odometry() self.odometry.header.stamp = rospy.Time.now() self.odometry.header.frame_id = "odom" self.odometry.child_frame_id = "base_link" self.odometry.pose.pose.orientation.z = 0. self.odometry.pose.pose.orientation.w = 1. self.curr_twist = TwistWithCovariance() self.curr_turning_radius = self.max_radius self.corner_cmd_pub = rospy.Publisher("/cmd_corner", CommandCorner, queue_size=1) self.drive_cmd_pub = rospy.Publisher("/cmd_drive", CommandDrive, queue_size=1) self.turning_radius_pub = rospy.Publisher("/turning_radius", Float64, queue_size=1) if self.should_calculate_odom: self.odometry_pub = rospy.Publisher("/odom", Odometry, queue_size=2) self.tf_pub = tf2_ros.TransformBroadcaster() rospy.Subscriber("/cmd_vel", Twist, self.cmd_cb, callback_args=False) rospy.Subscriber("/cmd_vel_intuitive", Twist, self.cmd_cb, callback_args=True) rospy.Subscriber("/encoder", JointState, self.enc_cb)
def Predict(self, dt, xVel, yawVel): dt = 0.004 # Fix dt to avoid computation issues # State-Transition Matrix A_t = np.array([[ 1.0, 0.0, 0.0, cos(self.X_t[2]) * dt, 0.0, cos(self.X_t[2]) * (dt**2) / 2 ], [ 0.0, 1.0, 0.0, sin(self.X_t[2]) * dt, 0.0, sin(self.X_t[2]) * (dt**2) / 2 ], [0.0, 0.0, 1.0, 0.0, dt, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0, dt], [0.0, 0.0, 0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) # Check control difference u_t = np.array([ 0.0, 0.0, 0.0, (xVel - self.X_t[3]), (yawVel - self.X_t[4]), -self.X_t[5] ]) # To ensure Zero Acceleration behaviour # Ground truth data self.X_t = A_t @ self.X_t + u_t self.length = self.length + xVel * dt if (self.test and self.ros): # Send the Update of the Ground Truth to Ros header = Header() header.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work header.frame_id = "odom" # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler( 0, 0, self.X_t[2]) # next, we'll publish the pose message over ROS pose = Pose(Point(self.X_t[0], self.X_t[1], 0.), Quaternion(*odom_quat)) pose_covariance = [0] * 36 pose_t = PoseWithCovariance(pose, pose_covariance) # next, we'll publish the twist message over ROS twist = Twist(Vector3(self.X_t[3], 0, self.X_t[5]), Vector3(0.0, 0.0, self.X_t[4])) twist_covariance = [0] * 36 twist_t = TwistWithCovariance(twist, twist_covariance) odom_t = Odometry(header, "base_link", pose_t, twist_t) # publish the message self.odom_t_pub.publish(odom_t)
def __init__(self): self.vel_pub = rospy.Publisher( '/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=0) self.odom_pub = rospy.Publisher('/odometry', Odometry, queue_size=10) self.tfb = TransformBroadcaster() rospy.sleep(0.5) startpose = PoseStamped() startpose.header.stamp = rospy.Time.now() startpose.header.frame_id = 'odom' startpose.pose.orientation.w = 1 self.publish_pose(startpose, TwistWithCovariance()) rospy.loginfo("Published initial tf") self.set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) self.set_rate = rospy.ServiceProxy('/mavros/set_stream_rate', StreamRate) self.land = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) self.takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL) rospy.loginfo('Services connected') self.vel = Twist() self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.on_vel) self.takeoff_sub = rospy.Subscriber('/takeoff', Empty, self.on_takeoff) self.land_sub = rospy.Subscriber('/land', Empty, self.on_land) self.last_pose = PoseStamped() self.position_sub = rospy.Subscriber('/mavros/global_position/local', Odometry, self.on_pose)
def source_odom_callback(self, msg): with self.lock: if self.offset_matrix is not None: source_odom_matrix = make_homogeneous_matrix([getattr(msg.pose.pose.position, attr) for attr in ["x", "y", "z"]], [getattr(msg.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"]]) new_odom = copy.deepcopy(msg) new_odom.header.frame_id = self.odom_frame new_odom.child_frame_id = self.base_link_frame twist_list = [new_odom.twist.twist.linear.x, new_odom.twist.twist.linear.y, new_odom.twist.twist.linear.z, new_odom.twist.twist.angular.x, new_odom.twist.twist.angular.y, new_odom.twist.twist.angular.z] # use median filter to cancel spike noise of twist when use_twist_filter is true if self.use_twist_filter: twist_list = self.median_filter(twist_list) new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6])) # overwrite twist covariance when calculate_covariance flag is True if self.overwrite_pdf: if not all([abs(x) < 1e-3 for x in twist_list]): # shift twist according to error mean when moving (stopping state is trusted) twist_list = [x + y for x, y in zip(twist_list, self.v_err_mean)] new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6])) # calculate twist covariance according to standard diviation if self.twist_proportional_sigma: current_sigma = [x * y for x, y in zip(twist_list, self.v_err_sigma)] else: if all([abs(x) < 1e-3 for x in twist_list]): current_sigma = [1e-6] * 6 # trust stopping state else: current_sigma = self.v_err_sigma new_odom.twist.covariance = update_twist_covariance(new_odom.twist, current_sigma) # offset coords new_odom_matrix = self.offset_matrix.dot(source_odom_matrix) new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3])) new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix))) if self.overwrite_pdf: if self.prev_odom is not None: dt = (new_odom.header.stamp - self.prev_odom.header.stamp).to_sec() global_twist_with_covariance = TwistWithCovariance(transform_local_twist_to_global(new_odom.pose.pose, new_odom.twist.twist), transform_local_twist_covariance_to_global(new_odom.pose.pose, new_odom.twist.covariance)) new_odom.pose.covariance = update_pose_covariance(self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt) else: new_odom.pose.covariance = numpy.diag([0.01**2] * 6).reshape(-1,).tolist() # initial covariance is assumed to be constant else: # only offset pose covariance new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6) rotation_matrix = self.offset_matrix[:3, :3] new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix)) new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist() # publish self.pub.publish(new_odom) if self.publish_tf: broadcast_transform(self.broadcast, new_odom, self.invert_tf) self.prev_odom = new_odom else: self.offset_matrix = self.calculate_offset(msg)
def pub_obstacle_info(): global sequence, var_abs_vel current_time = rospy.Time.now() new_obstacles_list = Obstacles() new_obstacles_list.header.stamp = current_time new_obstacles_list.header.frame_id = "map" new_obstacles_list.header.seq = sequence sequence += 1 obs_vel = [0.0, 0.0] obs_xy = [[2.8, -0.19], [3.35, 2.8]] for id_ in range(2): new_obstacle = Obstacle() new_obstacle.id = id_ new_obstacle.header.stamp = current_time new_obstacle.header.frame_id = "map" new_obstacle.header.seq = id_ var_abs_vel = obs_vel[id_] x = obs_xy[id_][0] #0.3*id_ +2.0 #id_*2.0#4.19#4.5 y = obs_xy[id_][1] #-2.48#0.0 #Obstacle odometry odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0) odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "map" odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat)) odom.child_frame_id = "base_link" odom.twist.twist = Twist(Vector3(var_abs_vel, 0, 0), Vector3(0, 0, 0)) new_obstacle.odom = odom #TwistWithCovariance abs_vel = TwistWithCovariance() abs_vel.twist.linear.x = var_abs_vel abs_vel.twist.linear.y = 0.0 new_obstacle.abs_velocity = abs_vel #reference_point - stable point on obstacle ref_pt = Point32() ref_pt.x = x ref_pt.y = y ref_pt.z = 0 new_obstacle.reference_point = ref_pt #Bounding box min, max wrt to center - given by odom pose pt_min = Point32() pt_min.x = -0.05 pt_min.y = -0.03 pt_min.z = 0 pt_max = Point32() pt_max.x = 0.05 pt_max.y = 0.03 pt_max.z = 0.1 new_obstacle.bounding_box_min = pt_min new_obstacle.bounding_box_max = pt_max new_obstacle.classification = 1 new_obstacle.classification_certainty = 0.9 new_obstacle.first_observed = current_time #- rospy.Duration(20) new_obstacle.last_observed = current_time new_obstacles_list.obstacles.append(new_obstacle) obst_pub.publish(new_obstacles_list)
def Control(self, cmd_vel): now = rospy.get_time() dt = now - self.control_t self.control_t = now z_x_dot = cmd_vel.linear.x z_yaw_dot = cmd_vel.angular.z z_x_dot_cov = 0.001 z_yaw_dot_cov = 0.01 """ # Make sure the execution is safe self.lock.acquire() try: self.Z = np.append(self.Z, np.array([z_x_dot, z_yaw_dot])) self.R = np.append(self.R, np.array([z_x_dot_cov,z_yaw_dot_cov])) self.H = np.column_stack([self.H, np.array([0,0,0,1,0,0]), np.array([0,0,0,0,1,0])]) self.J_H = np.column_stack([self.J_H, np.array([0,0,0,1,0,0]), np.array([0,0,0,0,1,0])]) self.control_measure = True finally: self.lock.release() # release self.lock, no matter what """ self.control_state = np.array([z_x_dot, z_yaw_dot]) #print("Control " + str(self.control_state)) # Send the Update to Ros header = Header() header.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work header.frame_id = "odom" # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.X_t[2]) # next, we'll publish the pose message over ROS pose = Pose(Point(self.X_t[0], self.X_t[1], 0.), Quaternion(*odom_quat)) pose_covariance = [0] * 36 pose_control = PoseWithCovariance(pose, pose_covariance) twist_covariance = [0] * 36 twist_covariance[0] = z_x_dot_cov twist_covariance[35] = z_yaw_dot_cov twist_control = TwistWithCovariance(cmd_vel, twist_covariance) odom_control = Odometry(header, "base_link", pose_control, twist_control) # publish the message self.odom_control_pub.publish(odom_control)
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from geometry_msgs.msg import TwistWithCovariance self.twist = kwargs.get('twist', TwistWithCovariance())
def prepare_odometry_msg(self): ''' Fill odometry message ''' odometry_msg = Odometry() pose = PoseWithCovariance(); twist = TwistWithCovariance();
def Transation(self, dt): # State-Transition Matrix A = np.array([[ 1.0, 0.0, 0.0, cos(self.X_t[2]) * dt, 0.0, cos(self.X_t[2]) * (dt**2) / 2 ], [ 0.0, 1.0, 0.0, sin(self.X_t[2]) * dt, 0.0, sin(self.X_t[2]) * (dt**2) / 2 ], [0.0, 0.0, 1.0, 0.0, dt, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0, dt], [0.0, 0.0, 0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) # Check control difference u = np.array([ 0.0, 0.0, 0.0, self.control_state[0] - self.X_t[3], self.control_state[1] - self.X_t[4], 0.0 ]) steps = 2 B = np.diag(np.array([0, 0, 0, 1 / steps, 1 / steps, 0])) # Prediction State self.X_t = A @ self.X_t + B @ u # Send the Update to Ros header = Header() header.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work header.frame_id = "odom" # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.X_t[2]) # next, we'll publish the pose message over ROS pose = Pose(Point(self.X_t[0], self.X_t[1], 0.), Quaternion(*odom_quat)) pose_covariance = [0] * 36 pose_ground_ekf = PoseWithCovariance(pose, pose_covariance) # next, we'll publish the pose message over ROS twist = Twist(Vector3(self.X_t[3], 0, self.X_t[5]), Vector3(0.0, 0.0, self.X_t[4])) twist_covariance = [0] * 36 twist_ground_ekf = TwistWithCovariance(twist, twist_covariance) odom_ground_ekf = Odometry(header, "base_link", pose_ground_ekf, twist_ground_ekf) # publish the message self.odom_ground_pub.publish(odom_ground_ekf)
def get_odom(self): '''get_odom: at a rate of _freq_, compute the motion of the vehicle from wheel odometry Each item (pose, odom) is formatted as [x, y, theta] Odometry messages are used because they contain covariances ''' freq = 10 r = rospy.Rate(freq) # 10hz while not rospy.is_shutdown(): odom_srv = self.odometry_proxy() wheel_odom = np.array([ odom_srv.wheel1, -odom_srv.wheel2, -odom_srv.wheel3, odom_srv.wheel4, ]) vehicle_twist = np.dot(self.mecanum_matrix, wheel_odom).A1 vehicle_twist[0] *= -1 vehicle_twist[1] *= -1 rot_mat = self.make_2D_rotation(self.pose[2]) x, y = np.dot(rot_mat, [vehicle_twist[0], vehicle_twist[1]]).A1 self.pose += [x, y, vehicle_twist[2]] orientation = tf_trans.quaternion_from_euler(0, 0, self.pose[2]) odom_msg = Odometry( header=Header( stamp=rospy.Time.now(), frame_id='/course', ), child_frame_id='/robot', pose=PoseWithCovariance( pose=Pose( position=Point( x=self.pose[0], y=self.pose[1], z=0.0, ), orientation=Quaternion(*orientation), ), covariance=np.diag( [0.3**2] * 6).flatten(), # No real covariance, just uncertainty ), twist=TwistWithCovariance(twist=Twist(linear=Vector3( x=vehicle_twist[0], y=vehicle_twist[1], z=0.0, ), angular=Vector3( x=0.0, y=0.0, z=vehicle_twist[2], )), covariance=np.diag([0.3**2] * 6).flatten())) self.odom_pub.publish(odom_msg) r.sleep()
def prepare_and_publish_odometry_msg(self): ''' Fill and publish odometry message Please, do it your self for practice ''' odometry_msg = Odometry() pose = PoseWithCovariance() twist = TwistWithCovariance()
def publishTagPos(self, x, y): now = rospy.Time.now() header = rospy.Header(seq=self.seq, stamp=now, frame_id=topic) self.seq += 1 # The position is encoded here. position = Point(x=x, y=y, z=0) # We have no orientation information with UWB ranging. orientation = Quaternion(x=0, y=0, z=0, w=1) pose = Pose(position=position, orientation=orientation) # Row-major representation of the 6x6 covariance matrix # The orientation parameters use a fixed-axis representation. # In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z # axis) covariance = [ 0, ] * 36 # Variances for X, Y position. covariance[idx6x6(0, 0)] = pos_var covariance[idx6x6(1, 1)] = pos_var # Our z, roll and pitch should be constant 0 (it's a car on flat ground). covariance[idx6x6(2, 2)] = known covariance[idx6x6(3, 3)] = known covariance[idx6x6(4, 4)] = known # We don't know the yaw. Might be anything. covariance[idx6x6(5, 5)] = unknown pose_w_cov = PoseWithCovariance(pose=pose, covariance=covariance) # Fill the Twist, but we actually have no velocity information. That's thus # all zeros. zero = Vector3(0, 0, 0) twist = Twist(zero, zero) covariance = [ 0, ] * 36 # X linear twist variance might be anything. # FIXME: we could have a more fine-tuned value by taking max linear # velocity into consideration. covariance[idx6x6(0, 0)] = unknown # Y linear twist variance covariance[idx6x6(1, 1)] = known # Z linear twist variance covariance[idx6x6(2, 2)] = known # X angular twist variance covariance[idx6x6(3, 3)] = known # Y angular twist variance covariance[idx6x6(4, 4)] = known # Z angular twist variance. Might be anything. # FIXME: we could have a more fine-tuned value by taking max rotational # velocity into consideration. covariance[idx6x6(5, 5)] = unknown twist_w_cov = TwistWithCovariance(twist=twist, covariance=covariance) self.pub.publish(header=header, child_frame_id=child_frame_id, pose=pose_w_cov, twist=twist_w_cov)
def publish(self): trans = self.setupTransform(None) odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.pose = PoseWithCovariance() odom.twist = TwistWithCovariance() self.tf_pub.sendTransform(trans) self.pub.publish(odom)
def updatePose(self, pose, encoderL, encoderR): """Update Position x,y,theta from encoder count L, R Return new Position x,y,theta""" global x_p print c now = rospy.Time.now() dL = encoderL - self.enc_L_prev dR = encoderR - self.enc_R_prev self.enc_L_prev = encoderL self.enc_R_prev = encoderR dT = (now - pose.timestamp) / 1000000.0 pose.timestamp = now x = pose.x y = pose.y theta = pose.theta R = 0.0 if (dR - dL) == 0: R = 0.0 else: R = self.config.WIDTH / 2.0 * (dL + dR) / (dR - dL) Wdt = (dR - dL) * self.config.encoder.Step / self.config.WIDTH ICCx = x - R * np.sin(theta) ICCy = y + R * np.cos(theta) pose.x = np.cos(Wdt) * (x - ICCx) - np.sin(Wdt) * (y - ICCy) + ICCx pose.y = np.sin(Wdt) * (x - ICCx) + np.cos(Wdt) * (y - ICCy) + ICCy pose.theta = theta + Wdt twist = TwistWithCovariance() twist.twist.linear.x = self.vel / 1000.0 twist.twist.linear.y = 0.0 twist.twist.angular.z = self.rot / 1000.0 Vx = twist.twist.linear.x Vy = twist.twist.linear.y Vth = twist.twist.angular.z odom_quat = quaternion_from_euler(0, 0, pose.theta) self.odom_broadcaster.sendTransform((pose.x, pose.y, 0.), odom_quat, now, 'base_link', 'odom') #self.odom_broadcaster.sendTransform((pose.x,pose.y,0.),odom_quat,now,'base_footprint','odom') odom = Odometry() odom.header.stamp = now odom.header.frame_id = 'odom' odom.pose.pose = Pose(Point(pose.x, pose.y, 0.), Quaternion(*odom_quat)) odom.child_frame_id = 'base_link' #odom.child_frame_id = 'base_footprint' odom.twist.twist = Twist(Vector3(Vx, Vy, 0), Vector3(0, 0, Vth)) print "x:{:.2f} y:{:.2f} theta:{:.2f}".format( pose.x, pose.y, pose.theta * 180 / math.pi) self.odom_pub.publish(odom) x_p = pose.x return pose
def callback(pub, data): odom = Odometry() pose = PoseWithCovariance() twist = TwistWithCovariance() #no shift in relation to boat position pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 0 #no rotation in relation to boat position pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 twist.twist = data odom.pose = pose odom.twist = twist odom.header.frame_id = "base_link" odom.child_frame_id = "base_link" pub.publish(odom)
def command_drive(data): # initialize the message components header = Header() foo = TwistWithCovarianceStamped() bar = TwistWithCovariance() baz = Twist() linear = Vector3() angular = Vector3() # get some data to publish # fake covariance until we know better covariance = [1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1] # to be overwritten when we decide what we want to go where linear.x = data.axes[1] * 15 linear.y = 0 linear.z = 0 angular.x = 0 angular.y = 0 angular.z = data.axes[0] * 10 # put it all together # Twist baz.linear = linear baz.angular = angular # TwistWithCovariance bar.covariance = covariance bar.twist = baz # Header header.seq = data.header.seq header.frame_id = frame_id header.stamp = stopwatch.now() # seq = seq + 1 # TwistWithCovarianceStamped foo.header = header foo.twist = bar # publish and log rospy.loginfo(foo) pub.publish(foo)
def process_jsb_input(self, buf, simtime): '''process FG FDM input from JSBSim''' fdm = self.fdm fdm.parse(buf) timestamp = int(simtime * 1.0e6) simbuf = struct.pack('<Q17dI', timestamp, fdm.get('latitude', units='degrees'), fdm.get('longitude', units='degrees'), fdm.get('altitude', units='meters'), fdm.get('psi', units='degrees'), fdm.get('v_north', units='mps'), fdm.get('v_east', units='mps'), fdm.get('v_down', units='mps'), fdm.get('A_X_pilot', units='mpss'), fdm.get('A_Y_pilot', units='mpss'), fdm.get('A_Z_pilot', units='mpss'), fdm.get('phidot', units='dps'), fdm.get('thetadot', units='dps'), fdm.get('psidot', units='dps'), fdm.get('phi', units='degrees'), fdm.get('theta', units='degrees'), fdm.get('psi', units='degrees'), fdm.get('vcas', units='mps'), 0x4c56414f) #lon E lat N latlondata = Pose2D(fdm.get('longitude', units='radians'), fdm.get('latitude', units='radians'), fdm.get('altitude', units="meters")) self.pos_pub.publish(latlondata) roll = fdm.get("phi", units='radians') pitch = fdm.get("theta", units='radians') yaw = fdm.get("psi", units='radians') quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) pose0 = Pose() pose0.orientation.x = quaternion[0] pose0.orientation.y = quaternion[1] pose0.orientation.z = quaternion[2] pose0.orientation.w = quaternion[3] if self.init_pos == 0: pose0.position.x = 0 pose0.position.y = 0 pose0.position.z = -fdm.get("altitude", units="meters") self.init_pos = latlondata else: d_lon = latlondata.x - self.init_pos.x d_lat = latlondata.y - self.init_pos.y pose0.position.x = d_lat * C_EARTH pose0.position.y = d_lon * C_EARTH * math.cos(self.init_pos.y) pose0.position.z = -fdm.get("altitude", units="meters") twist0 = Twist() odom = Odometry(pose=PoseWithCovariance(pose=pose0), twist=TwistWithCovariance(twist=twist0)) self.odom_pub.publish(odom) self.pose_pub.publish(pose0)
def receive_encoders(self, msg): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'base_link' point_msg = Point(0, 0, 0) orientation_quat = R.from_euler('xyz', [0, 0, 0]).as_quat() pose_cov = np.ravel(np.eye(6) * 0.0) quat_msg = Quaternion(orientation_quat[0], orientation_quat[1], orientation_quat[2], orientation_quat[3]) pose_with_cov = PoseWithCovariance() pose_with_cov.pose = Pose(point_msg, quat_msg) pose_with_cov.covariance = pose_cov x_dot = (((msg.left * math.pi / 30 * self.wheel_radius) + (msg.right * math.pi / 30 * self.wheel_radius)) / 2 ) # * math.cos(self.orientation[-1]) y_dot = 0 #(((self.port_encoder * math.pi / 30 * 0.2286) + (self.starboard_encoder * math.pi / 30 * 0.2286)) / 2) * math.sin(self.orientation[-1]) theta_dot = ( (msg.right * math.pi / 30 * self.wheel_radius) - (msg.left * math.pi / 30 * self.wheel_radius)) / self.robot_width linear_twist = Vector3(x_dot, y_dot, 0) angular_twist = Vector3(0, 0, theta_dot) twist_cov = np.diag([0.002, 0.02, 0, 0, 0, 0.001]).flatten() twist_with_cov = TwistWithCovariance() twist_with_cov.twist = Twist(linear_twist, angular_twist) twist_with_cov.covariance = twist_cov stamped_msg = Odometry() stamped_msg.header = header stamped_msg.child_frame_id = 'base_link' stamped_msg.pose = pose_with_cov stamped_msg.twist = twist_with_cov try: self.wheel_pub.publish(stamped_msg) except rospy.ROSInterruptException as e: rospy.logerr(e.getMessage())
def publish_odom(self, *args): if self.last_odom is None: return msg = self.last_odom if self.target in msg.name: target_index = msg.name.index(self.target) twist = msg.twist[target_index] twist = msg.twist[target_index] pose = msg.pose[target_index] self.state_pub.publish( header=navigator_tools.make_header(frame='/enu'), child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) self.absstate_pub.publish( header=navigator_tools.make_header(frame='/ecef'), child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist))
def odom_msg(self) -> Odometry: return Odometry( header=Header(stamp=self.clock.now().to_msg(), frame_id='odom'), child_frame_id='base_link', pose=PoseWithCovariance(pose=Pose( position=Point(x=self.x, y=self.y, z=0.), orientation=self.orientation, )), twist=TwistWithCovariance(twist=Twist( linear=self.linear, angular=self.angular, )), )
def call_services(): time_now = rospy.Time.now() poseWCS = PoseWithCovarianceStamped() poseWCS.header.stamp = time_now poseWCS.header.frame_id = '/map' poseWCS.pose.pose = Pose(Point(-1, 4, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0)) poseWCS.pose.covariance = [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 ] odom = Odometry() odom.header.stamp = time_now odom.header.frame_id = '/map' poseWC = PoseWithCovariance() poseWC.pose = Pose(Point(-1, 4, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0)) poseWC.covariance = [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 ] odom.pose = poseWC twistWC = TwistWithCovariance() twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) twistWC.covariance = [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 ] odom.twist = twistWC rospy.wait_for_service('/Pioneer2_vc_initialpose') try: initialpose_proxy = rospy.ServiceProxy('/Pioneer2_vc_initialpose', InitialPoseToRobot) resp = initialpose_proxy(poseWCS) except rospy.ServiceException, e: print '/Pioneer2_vc_initialpose service call failed: %s' % e