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 )