def odom2twsitStamped(self, odom): odomTwist = TwistWithCovarianceStamped() odomTwist.twist = odom.twist odomTwist.twist = odom.twist odomTwist.header = odom.header return odomTwist
def listen(self): while not rospy.is_shutdown(): if self._mbedSerial.inWaiting(): #bytesToRead = self._mbedSerial.inWaiting() x = self._mbedSerial.read_until() self.buf.append(x) if len(self.buf) > self.incomingStringLength: self._mbedSerial.flush() msg = self.convert(self.buf) data = self.parseSensorData(msg) rospy.loginfo(data) #quat_array = tf.transformations.quaternion_from_euler(data[1] * math.pi/180, data[0] * math.pi/180, data[2] * math.pi/180) twist_msg = TwistWithCovarianceStamped() h = Header() h.stamp = rospy.Time.now() h.frame_id = "sofi_cam" #TODO convert to base_link frame twist_msg.header = h self.twist.twist.linear.x = self.twist.twist.linear.y = self.twist.twist.linear.z = self.twist.twist.angular.x = 0 self.twist.twist.angular.y = 0 self.twist.twist.angular.z = 0 self.twist.covariance = # TODO determine covariance matrix self.twist_pub.publish(twist_msg) angle_to_true_north = Float64() angle_to_true_north.data = data[9] self.compass_pub.publish(angle_to_true_north) self.buf = []
def callback(msg): pub = rospy.Publisher('twist0', TwistWithCovarianceStamped, queue_size=10) twist = TwistWithCovarianceStamped() twist.header = msg.header twist.header.frame_id = "base_link" twist.twist = msg.twist pub.publish(twist)
def fillSpeed(human): dt = (human.current_pose.header.stamp - human.last_pose.header.stamp).to_sec() diffSpeed = TwistWithCovarianceStamped() diffSpeed.header = human.current_pose.header diffSpeed.twist.twist.linear.x = (human.current_pose.pose.pose.position.x - human.last_pose.pose.pose.position.x) / dt diffSpeed.twist.twist.linear.y = (human.current_pose.pose.pose.position.y - human.last_pose.pose.pose.position.y) / dt diffSpeed.twist.twist.linear.z = (human.current_pose.pose.pose.position.z - human.last_pose.pose.pose.position.z) / dt last_rotation = tf.transformations.inverse_matrix(tf.transformations.quaternion_matrix([human.last_pose.pose.pose.orientation.x, human.last_pose.pose.pose.orientation.y, human.last_pose.pose.pose.orientation.z, human.last_pose.pose.pose.orientation.w])) current_rotation = tf.transformations.quaternion_matrix([human.current_pose.pose.pose.orientation.x, human.current_pose.pose.pose.orientation.y, human.current_pose.pose.pose.orientation.z, human.current_pose.pose.pose.orientation.w]) rot_diff = current_rotation * last_rotation roll, pitch, yaw = tf.transformations.euler_from_matrix(rot_diff) diffSpeed.twist.twist.angular.x = roll / dt diffSpeed.twist.twist.angular.y = pitch / dt diffSpeed.twist.twist.angular.z = yaw / dt norm2d = math.hypot(diffSpeed.twist.twist.linear.x, diffSpeed.twist.twist.linear.y) if norm2d >= 3.0: diffSpeed.twist.twist.linear.x = diffSpeed.twist.twist.linear.x / norm2d * 3.0 diffSpeed.twist.twist.linear.y = diffSpeed.twist.twist.linear.y / norm2d * 3.0 human.last_speed = human.current_speed human.current_speed = diffSpeed
def navsat_cb(msg): global vel_pub global covariance new_msg = TwistWithCovarianceStamped() new_msg.header = msg.header new_msg.twist.twist = msg.twist new_msg.twist.covariance = covariance vel_pub.publish(new_msg)
def twist_cb(self, twist_msg): new_twist_msg = TwistWithCovarianceStamped() new_twist_msg.header = twist_msg.header new_twist_msg.header.frame_id = "base_link" new_twist_msg.twist.twist.linear.x = twist_msg.twist.twist.linear.x * self.twist_lin_x_peak new_twist_msg.twist.twist.angular.z =twist_msg.twist.twist.angular.z * self.twist_ang_z_peak new_twist_msg.twist.covariance = self.covariance_euler self.twist_pub.publish(new_twist_msg)
def __init__(self): # ROS parameters self.std_position = rospy.get_param('~std_position', 0.0) self.std_velocity = rospy.get_param('~std_velocity', 0.0) self.publish_frequency = rospy.get_param('~publish_frequency', 0.0) # Init variables self.last_position = None self.last_velocity = None # ROS publishers self.pose_publisher = rospy.Publisher('gps_pose', PoseWithCovarianceStamped, queue_size=1) self.twist_publisher = rospy.Publisher('gps_twist', TwistWithCovarianceStamped, queue_size=1) # ROS subscribers rospy.Subscriber('gps_position', PointStamped, self.position_callback) rospy.Subscriber('gps_velocity', Vector3Stamped, self.velocity_callback) # Dynamic reconfigure self.configServer = dynamic_reconfigure.server.Server( gpsPublisherConfig, self.dynamicReconfigure_callback) # Main loop self.rate = rospy.Rate(self.publish_frequency) while not rospy.is_shutdown(): if (self.last_position is not None) and (self.last_velocity is not None): # Pose message pose_msg = PoseWithCovarianceStamped() pose_msg.header = self.last_position.header pose_msg.pose.pose.position = self.last_position.point pose_msg.pose.covariance[0] = self.std_position**2 pose_msg.pose.covariance[7] = self.std_position**2 self.pose_publisher.publish(pose_msg) # Twist message (the twist is not in the same frame in hector_gazebo plugin)(why??) twist_msg = TwistWithCovarianceStamped() twist_msg.header = self.last_velocity.header twist_msg.twist.twist.linear.x = -self.last_velocity.vector.y twist_msg.twist.twist.linear.y = self.last_velocity.vector.x twist_msg.twist.covariance[0] = self.std_velocity**2 twist_msg.twist.covariance[7] = self.std_velocity**2 self.twist_publisher.publish(twist_msg) self.rate.sleep()
def getTwistInBaseFrame(self, twistWC, header): """Returns a TwistWithCovarianceStamped in base frame""" # Create the stamped object twistS = TwistStamped() twistS.header = header twistS.twist = twistWC.twist twistS_base = self.transformTwist(self.base_frame_id, twistS) twistWC_out = TwistWithCovarianceStamped() twistWC_out.header = twistS_base.header twistWC_out.twist.twist = twistS_base.twist twistWC_out.twist.covariance = twistWC.covariance return twistWC_out
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 publish_data(self, state: np.array, cov_mat: np.array) -> None: """ Publishes ball position and velocity to ros nodes :param state: current state of kalmanfilter :param cov_mat: current covariance matrix :return: """ # position pose_msg = PoseWithCovarianceStamped() pose_msg.header.frame_id = self.filter_frame pose_msg.header.stamp = rospy.Time.now() pose_msg.pose.pose.position.x = state[0] pose_msg.pose.pose.position.y = state[1] pose_msg.pose.covariance = np.eye(6).reshape((36)) pose_msg.pose.covariance[0] = cov_mat[0][0] pose_msg.pose.covariance[1] = cov_mat[0][1] pose_msg.pose.covariance[6] = cov_mat[1][0] pose_msg.pose.covariance[7] = cov_mat[1][1] pose_msg.pose.pose.orientation.w = 1 self.ball_pose_publisher.publish(pose_msg) # velocity movement_msg = TwistWithCovarianceStamped() movement_msg.header = pose_msg.header movement_msg.twist.twist.linear.x = state[2] * self.filter_rate movement_msg.twist.twist.linear.y = state[3] * self.filter_rate movement_msg.twist.covariance = np.eye(6).reshape((36)) movement_msg.twist.covariance[0] = cov_mat[2][2] movement_msg.twist.covariance[1] = cov_mat[2][3] movement_msg.twist.covariance[6] = cov_mat[3][2] movement_msg.twist.covariance[7] = cov_mat[3][3] self.ball_movement_publisher.publish(movement_msg) # ball ball_msg = PoseWithCertaintyStamped() ball_msg.header = pose_msg.header ball_msg.pose.confidence = self.last_ball_msg.confidence ball_msg.pose.pose.pose.position = pose_msg.pose.pose.position ball_msg.pose.pose.covariance = pose_msg.pose.covariance self.ball_publisher.publish(ball_msg)
def estimRobotVelocityPublish(self): # TODO Finish! # header_wrt_world_msg = Header() header_wrt_world_msg.stamp = self.msf_state_estimator.estim_state_timestamp header_wrt_world_msg.frame_id = self.world_frame # header_wrt_robot_msg = Header() header_wrt_robot_msg.stamp = self.msf_state_estimator.estim_state_timestamp header_wrt_robot_msg.frame_id = self.robot_frame # robot_velocity_world_msg = Twist() # robot_velocity_world_msg.linear.x = self.msf_state_estimator.estim_robot_velo_lin_world[ 0] robot_velocity_world_msg.linear.y = self.msf_state_estimator.estim_robot_velo_lin_world[ 1] robot_velocity_world_msg.linear.z = self.msf_state_estimator.estim_robot_velo_lin_world[ 2] # robot_velocity_world_msg.angular.x = 0.0 robot_velocity_world_msg.angular.y = 0.0 robot_velocity_world_msg.angular.z = self.msf_state_estimator.estim_robot_velo_ang_world[ 0] # # TODO Cov # # TODO wrt robot # robot_velocity_world_stamp_msg = TwistStamped() robot_velocity_world_stamp_msg.header = header_wrt_world_msg robot_velocity_world_stamp_msg.twist = robot_velocity_world_msg # robot_velocity_world_cov_stamp_msg = TwistWithCovarianceStamped() robot_velocity_world_cov_stamp_msg.header = header_wrt_world_msg robot_velocity_world_cov_stamp_msg.twist.twist = robot_velocity_world_msg # robot_velocity_world_cov_stamp_msg.twist.covariance # # TODO robot_velocity_robot_stamp_msg = TwistStamped() robot_velocity_robot_stamp_msg.header = header_wrt_robot_msg #robot_velocity_robot_stamp_msg.twist = robot_velocity_robot_msg # # TODO robot_velocity_robot_cov_stamp_msg = TwistWithCovarianceStamped() robot_velocity_robot_cov_stamp_msg.header = header_wrt_robot_msg #robot_velocity_robot_cov_stamp_msg.twist.twist = robot_velocity_robot_msg # robot_velocity_robot_cov_stamp_msg.twist.covariance # self.estim_robot_vel_world_pub.publish(robot_velocity_world_stamp_msg) # self.estim_robot_vel_world_cov_pub.publish( robot_velocity_world_cov_stamp_msg) # self.estim_robot_vel_robot_pub.publish(robot_velocity_robot_stamp_msg) # self.estim_robot_vel_robot_cov_pub.publish( robot_velocity_robot_cov_stamp_msg) # return
def talker(): pubGPS = rospy.Publisher('poseGPS', NavSatFix, queue_size=1000) pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000) pubTwist = rospy.Publisher('twistGPS', TwistWithCovarianceStamped, queue_size=1000) rospy.init_node('imugpspublisher', anonymous=True) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): data = ser.readline() if data[0] == 'Z': data = data.replace("Z","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 4: try: ##data_list structure: lat, lon, heading, vel float_list = [float(i) for i in data_list] poseGPS = NavSatFix() poseGPS.header.frame_id = "world" poseGPS.header.stamp = rospy.Time.now() poseGPS.status.status = 0 poseGPS.status.service = 1 poseGPS.latitude = float_list[0] poseGPS.longitude = float_list[1] poseGPS.altitude = 0.0 poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0] poseGPS.position_covariance_type = 2 pubGPS.publish(poseGPS) twistGPS = TwistWithCovarianceStamped() twistGPS.header = poseGPS.header twistGPS.twist.twist.linear.x = float_list[3]*knots*math.cos(latest_yaw) twistGPS.twist.twist.linear.y = float_list[3]*knots*math.sin(latest_yaw) twistGPS.twist.twist.linear.z = 0.0 ##angular data not used here twistGPS.twist.covariance = [0.01, 0.01, 0, 0, 0, 0] pubTwist.publish(twistGPS) log = "GPS Data: %f %f %f %f Publish at Time: %s" % (float_list[0], float_list[1], float_list[2], float_list[3], rospy.get_time()) except: log = "GPS Data Error! Data : %s" % data else: log = "GPS Data Error! Data : %s" % data rospy.loginfo(log) elif data[0] == 'Y': data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "base_link" imuData.header.stamp = rospy.Time.now() latest_yaw = float_list[3] ##data form in yaw, pitch, roll quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6] imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7] imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3] pubIMU.publish(imuData) log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ % (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time()) except: log = "IMU Data Error! Data : %s" % data else: log = "Data Error! Data : %s" % data rospy.loginfo(log) else: log = "Data Error or Message: %s" % data rospy.loginfo(log) rate.sleep()
def twistCallback(msg): twist_covariance = TwistWithCovarianceStamped() twist_covariance.header = msg.header twist_covariance.twist.twist = msg.twist msg_pub.publish(twist_covariance)
def measSensorLoopTimerCallback(self, timer_msg): # Get time time_stamp_current = rospy.Time.now() # if (self.flag_robot_velocity_set == False): return # Computing the measurement # meas_vel_lin = np.zeros((3, ), dtype=float) meas_vel_ang = np.zeros((3, ), dtype=float) # meas_vel_lin[0] = self.robot_vel_lin[0] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_vel_lin['x'])) meas_vel_lin[1] = self.robot_vel_lin[1] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_vel_lin['y'])) meas_vel_lin[2] = self.robot_vel_lin[2] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_vel_lin['z'])) # meas_vel_ang[0] = self.robot_vel_ang[0] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_vel_ang['x'])) meas_vel_ang[1] = self.robot_vel_ang[1] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_vel_ang['y'])) meas_vel_ang[2] = self.robot_vel_ang[2] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_vel_ang['z'])) # Covariance meas_cov_vel = np.diag([ self.cov_meas_vel_lin['x'], self.cov_meas_vel_lin['y'], self.cov_meas_vel_lin['z'], self.cov_meas_vel_ang['x'], self.cov_meas_vel_ang['y'], self.cov_meas_vel_ang['z'] ]) # Filling the message # meas_header_msg = Header() meas_robot_velocity_msg = Twist() meas_robot_velocity_stamp_msg = TwistStamped() meas_robot_velocity_stamp_cov_msg = TwistWithCovarianceStamped() # meas_header_msg.frame_id = self.robot_frame_id meas_header_msg.stamp = self.robot_velocity_timestamp # Linear meas_robot_velocity_msg.linear.x = meas_vel_lin[0] meas_robot_velocity_msg.linear.y = meas_vel_lin[1] meas_robot_velocity_msg.linear.z = meas_vel_lin[2] # Angular meas_robot_velocity_msg.angular.x = meas_vel_ang[0] meas_robot_velocity_msg.angular.y = meas_vel_ang[1] meas_robot_velocity_msg.angular.z = meas_vel_ang[2] # meas_robot_velocity_stamp_msg.header = meas_header_msg meas_robot_velocity_stamp_msg.twist = meas_robot_velocity_msg # meas_robot_velocity_stamp_cov_msg.header = meas_header_msg meas_robot_velocity_stamp_cov_msg.twist.covariance = meas_cov_vel.reshape( (36, 1)) meas_robot_velocity_stamp_cov_msg.twist.twist = meas_robot_velocity_msg # self.meas_robot_velocity_pub.publish(meas_robot_velocity_stamp_msg) self.meas_robot_velocity_cov_pub.publish( meas_robot_velocity_stamp_cov_msg) # return
def point_tracker(self, pointcloud): # Assume there there is, at most, 1 object (Bonus: get code to work for multiple objects! Hint: use a new kalman filter for each object) if len(pointcloud.points) > 0: point = pointcloud.points[0] measurement = np.matrix([point.x, point.y]).T # note: the shape is critical; this is a matrix, not an array (so that matrix mult. works) else: measurement = None nmeasurements = 2 if measurement is None: # propagate a blank measurement m = np.matrix([np.nan for i in range(2) ]).T xhat, P, K = self.kalman_filter.update( None ) else: # propagate the measurement xhat, P, K = self.kalman_filter.update( measurement ) # run kalman filter ### Publish the results as a simple array float_time = float(pointcloud.header.stamp.secs) + float(pointcloud.header.stamp.nsecs)*1e-9 x = xhat.item(0) # xhat is a matrix, .item() gives you the actual value xdot = xhat.item(1) y = xhat.item(2) ydot = xhat.item(3) p_vector = P.reshape(16).tolist()[0] data = [float_time, x, xdot, y, ydot] data.extend(p_vector) float64msg = Float64MultiArray() float64msg.data = data now = rospy.get_time() self.time_start = now self.pubTrackedObjects_Float64MultiArray.publish( float64msg ) ### ### Publish the results as a ROS type pose (positional information) # see: http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html pose = PoseWithCovarianceStamped() pose.header = pointcloud.header pose.pose.pose.position.x = x pose.pose.pose.position.y = y pose.pose.pose.position.z = 0 pose.pose.pose.orientation.x = 0 pose.pose.pose.orientation.y = 0 pose.pose.pose.orientation.z = 0 pose.pose.pose.orientation.w = 0 x_x = P[0,0] x_y = P[0,2] y_y = P[2,2] position_covariance = [x_x, x_y, 0, 0, 0, 0, x_y, y_y, 0, 0, 0, 0] position_covariance.extend([0]*24) # position_covariance is the row-major representation of a 6x6 covariance matrix pose.pose.covariance = position_covariance self.pubTrackedObjects_Pose.publish( pose ) ### ### Publish the results as a ROS type twist (velocity information) # see: http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html twist = TwistWithCovarianceStamped() twist.header = pointcloud.header twist.twist.twist.linear.x = xdot twist.twist.twist.linear.y = ydot twist.twist.twist.linear.z = 0 twist.twist.twist.angular.x = 0 twist.twist.twist.angular.y = 0 twist.twist.twist.angular.z = 0 dx_dx = P[1,1] dx_dy = P[1,3] dy_dy = P[3,3] velocity_covariance = [x_x, x_y, 0, 0, 0, 0, x_y, y_y, 0, 0, 0, 0] velocity_covariance.extend([0]*24) # position_covariance is the row-major representation of a 6x6 covariance matrix twist.twist.covariance = velocity_covariance self.pubTrackedObjects_Twist.publish( twist )