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 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 odom2twsitStamped(self, odom):
odomTwist = TwistWithCovarianceStamped()
odomTwist.twist = odom.twist
odomTwist.twist = odom.twist
odomTwist.header = odom.header
return odomTwist
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 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 publish_velocity(msg, num):
global i_odometry
if msg.name() != 'NAV_VELNED':
print "error: invalid type of message!!"
return
odometry = TwistWithCovarianceStamped()
odometry.header.stamp = rospy.Time.now()
if num == 1:
odometry.header.frame_id = "/left_gnss"
elif num == 2:
odometry.header.frame_id = "/right_gnss"
odometry.header.seq = i_odometry[num - 1]
odometry.twist.twist.linear.x = msg.velE * 1e-2
odometry.twist.twist.linear.y = msg.velN * 1e-2
odometry.twist.twist.linear.z = -msg.velD * 1e-2
odometry.twist.covariance = [
msg.sAcc * 1e-2 / math.sqrt(3.), 0., 0., 0., 0., 0., 0.,
msg.sAcc * 1e-2 / math.sqrt(3.), 0., 0., 0., 0., 0., 0.,
msg.sAcc * 1e-2 / math.sqrt(3.), 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.
]
if num == 1:
pub_leftvel.publish(odometry)
elif num == 2:
pub_rightvel.publish(odometry)
i_odometry[num - 1] += 1
def run(self):
"""Does publishing of thrust odometry"""
rospy.init_node('depth_sensor_gazebo', anonymous=True)
thrust_odom_pub = rospy.Publisher('thrust_odom',
TwistWithCovarianceStamped,
queue_size=1)
thrust_odom = TwistWithCovarianceStamped()
thrust_odom.header.seq = 0
thrust_odom.header.frame_id = 'base_link'
thrust_odom.twist.twist.angular.x = 0
thrust_odom.twist.twist.angular.y = 0
thrust_odom.twist.twist.angular.z = 0
thrust_odom.twist.twist.linear.x = 0
thrust_odom.twist.twist.linear.y = 0
thrust_odom.twist.twist.linear.z = 0
thrust_odom.twist.covariance = [
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, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0
]
rate = rospy.Rate(30)
while not rospy.is_shutdown():
thrust_odom.header.seq += 1
thrust_odom.header.stamp = rospy.get_rostime()
thrust_odom_pub.publish(thrust_odom)
rate.sleep()
def twist_callback(msg):
global twist
vx = msg.twist.linear.y
vy = msg.twist.linear.x
vz = -msg.twist.linear.z
print vx, vy, vz, 'twist'
current_time = rospy.Time.now()
twist = TwistWithCovarianceStamped()
twist.header.stamp = current_time
twist.header.frame_id = "base_link"
twist.twist.twist.linear.x = vx
twist.twist.twist.linear.y = vy
twist.twist.twist.linear.z = vz
twist.twist.covariance = [0.2, 0, 0, 0, 0, 0,
0, 0.2, 0, 0, 0, 0,
0, 0, 0.2, 0, 0, 0,
0, 0, 0, 0.2, 0, 0,
0, 0, 0, 0, 0.2, 0,
0, 0, 0, 0, 0, 0.2]
twist_pub.publish(twist)
def publish_twist_estimate(self, radar_msg):
twist_estimate = TwistWithCovarianceStamped()
ns = rospy.get_namespace()
if ns == '/':
## single radar frame_id to comply with TI naming convention
twist_estimate.header.frame_id = "base_radar_link"
else:
## multi-radar frame_id
twist_estimate.header.frame_id = ns[1:] + "_link"
twist_estimate.header.stamp = radar_msg.header.stamp
twist_estimate.twist.twist.linear.x = self.vel_estimate_[0]
twist_estimate.twist.twist.linear.y = self.vel_estimate_[1]
twist_estimate.twist.twist.linear.z = self.vel_estimate_[2]
K = self.cov_scale_factor_
if self.use_const_cov_:
twist_estimate.twiist.covariance[0] = K * 0.01
twist_estimate.twiist.covariance[7] = K * 0.015
twist_estimate.twiist.covariance[14] = K * 0.05
else:
for i in range(self.vel_covariance_.shape[0]):
for j in range(self.vel_covariance_.shape[1]):
twist_estimate.twist.covariance[(i*6)+j] = K*self.vel_covariance_[i,j]
self.twist_pub.publish(twist_estimate)
def main():
rospy.init_node('topic_test', anonymous=True)
rate = rospy.Rate(10)
# Sensor topics
sonar_pub = rospy.Publisher('sonar_test', Float32, queue_size=10)
imu_pub = rospy.Publisher('imu_test', Imu, queue_size=1)
nn_pub = rospy.Publisher('nn_test',
TwistWithCovarianceStamped,
queue_size=1)
# ControlLoop Topic
cl_pub = rospy.Publisher('control_test', Float32MultiArray, queue_size=1)
# Sensor Messages
sonar_msg = Float32()
sonar_msg.data = 1234
imu_msg = Imu()
imu_msg.linear_acceleration.x = 10
imu_msg.angular_velocity.z = 10
nn_msg = TwistWithCovarianceStamped()
cl_msg = Float32MultiArray()
cl_msg.data = [1.1, 2.2]
while not rospy.is_shutdown():
sonar_pub.publish(sonar_msg)
imu_pub.publish(imu_msg)
nn_pub.publish(nn_msg)
cl_pub.publish(cl_msg)
rate.sleep()
def wrenchCallback(self, wrenchMsg):
twistMsg = TwistWithCovarianceStamped()
twistMsg.header.stamp = wrenchMsg.header.stamp
twistMsg.header.frame_id = 'ucat0/ekf_base_link'
twistMsg.twist.twist.linear.x = 0.050 * wrenchMsg.wrench.force.x
twistMsg.twist.twist.linear.y = 0.015 * wrenchMsg.wrench.force.y
twistMsg.twist.twist.angular.z = 1.1 * wrenchMsg.wrench.torque.z
self.twistPub.publish(twistMsg)
def callback(data):
pub = rospy.Publisher('twist', TwistWithCovarianceStamped, queue_size=1)
t = TwistWithCovarianceStamped()
t.twist.twist = data
t.header.frame_id = "base_link"
t.header.stamp = rospy.Time.now()
t.twist.covariance[0] = 0.1
pub.publish(t)
def callback(data):
t = TwistWithCovarianceStamped()
newData = Twist()
newData.linear.x = 2 * data.linear.x
newData.angular = data.angular
t.twist.twist = newData
t.header.frame_id = "base_link"
t.header.stamp = rospy.Time.now()
t.twist.covariance[0] = 0.1
pub.publish(t)
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 make_twist_cov(self):
twcs = TwistWithCovarianceStamped()
twcs.header.frame_id = "base_link"
twcs.header.stamp = rospy.Time.now()
twcs.twist.twist = self.twist
cov = np.diag([0.1, -1, -1, -1, -1, 0.1])
twcs.twist.covariance = list(cov.flatten())
return twcs
def callback(self, msg):
# if not self.R_NED_SHIP is None:
# TODO: take into account p_b2g and body omega...
v_NED = 1.0e-2*np.array([msg.velN, msg.velE, msg.velD])
# v_SHIP = np.dot(self.R_NED_SHIP, v_NED)
sACC = 1.0e-2*msg.sAcc
output = TwistWithCovarianceStamped()
output.header.stamp = rospy.Time.now()
output.twist.twist.linear.x = v_NED[0]
output.twist.twist.linear.y = v_NED[1]
output.twist.twist.linear.z = v_NED[2]
output.twist.covariance[0] = sACC
self.publisher.publish(output)
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 get_initial_pose(self):
time.sleep(2)
self.poses_dict["pose"] = self._oriy, self._oriz, self._posew
rospy.loginfo("Written posex")
time.sleep(2)
self.poses_dict["twist"] = self._twistz, self._twisty, self._twistx
rospy.loginfo("Written twistz")
with open('poses.txt', 'w') as file:
for key, value in self.poses_dict.iteritems():
if value:
file.write(
str(key) + ':\n----------\n' + str(value) +
'\n===========\n')
rospy.loginfo("auto generate pose")
# rospy.init_node('check_odometry')
#odom_sub = rospy.Subscriber('/odom', Odometry, self.sub_callback)
#self.goal_sent = False
initial_pose = PoseWithCovarianceStamped()
initial_twist = TwistWithCovarianceStamped()
initialtrans = (trans, rot) = self.listener.lookupTransform(
'/map', '/base_link', rospy.Time(0))
initialPose_pub = rospy.Publisher("/initialpose",
PoseWithCovarianceStamped,
queue_size=10)
initial_pose.header.stamp = rospy.Time.now()
initial_pose.header.frame_id = "map"
initial_pose.pose.pose.position.x = trans[
0] #self._posex ##chnage these to be based on baselink
initial_pose.pose.pose.position.y = trans[
1] #self._posey ##chnage these to be based on baskelink
initial_pose.pose.pose.orientation.y = self._oriy
initial_pose.pose.pose.orientation.z = self._oriz
initial_pose.pose.pose.orientation.w = self._posew ##dont chnage this only change the x and y
initial_twist.twist.twist.linear.x = self._twistx
initial_twist.twist.twist.linear.y = self._twisty
initial_twist.twist.twist.angular.z = self._twistz
initial_pose.pose.covariance[0] = 0.25
initial_pose.pose.covariance[7] = 0.25
initial_pose.pose.covariance[35] = 0.06
rate = rospy.Rate(10)
i = 1
while i < 8:
initialPose_pub.publish(initial_pose)
i += 1
rate.sleep()
def pose_call_back(msg):
twist_to_send = TwistWithCovarianceStamped()
twist_to_send.header.seq = twist_to_send.header.seq + 1
twist_to_send.header.stamp = rospy.Time.now()
twist_to_send.header.frame_id = "base_link"
twist_to_send.twist.twist.linear.x = msg.linear_velocity * (
1 + random.uniform(-random_noise_linear, random_noise_linear))
twist_to_send.twist.twist.linear.y = 0.0
twist_to_send.twist.twist.linear.z = 0.0
twist_to_send.twist.twist.angular.x = 0.0
twist_to_send.twist.twist.angular.y = 0.0
twist_to_send.twist.twist.angular.z = msg.angular_velocity * (
1 + random.uniform(-random_noise_angular, random_noise_angular))
twist_to_send.twist.covariance = 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
pub_twist.publish(twist_to_send)
def twist_callback(self, msg):
''' Publish the twist of the drone based on the Motion Tracker data '''
# NOTICE: the velocities are modified because the axes
# of the mocap are set up as right-handed y-up. This means that the y
# and z axes are switched, and positive x is to the left.
twist_to_pub = TwistWithCovarianceStamped()
twist_to_pub.header.stamp = msg.header.stamp
twist_to_pub.header.frame_id = 'World'
twist_to_pub.twist.twist.linear.x = -msg.twist.linear.x
twist_to_pub.twist.twist.linear.y = msg.twist.linear.z
twist_to_pub.twist.twist.linear.z = msg.twist.linear.y
twist_to_pub.twist.twist.angular.x = -msg.twist.angular.x
twist_to_pub.twist.twist.angular.y = msg.twist.angular.z
twist_to_pub.twist.twist.angular.z = msg.twist.angular.y
self.twistpub.publish(twist_to_pub)
def _initialize_msgs(self):
"""Initialize messages when they are needed"""
self._state_msg = svea_msgs.msg.VehicleState()
self._odometry_msg = Odometry()
self._pose_msg = PoseWithCovarianceStamped()
self._pose_msg.pose = self._odometry_msg.pose
self._pose_msg.header = self._state_msg.header
self._state_msg.child_frame_id = self.child_frame
self._odometry_msg.child_frame_id = self.child_frame
self._odometry_msg.header = self._state_msg.header
self._twist_msg = TwistWithCovarianceStamped()
self._twist_msg.twist = self._odometry_msg.twist
_set_placehoder_covariance(self._twist_msg.twist.covariance)
self._msgs_are_initialized = True
# self._msgs_are_updated = False
self._pose_is_updated = False
self._twist_is_updated = False
self._covariance_is_updated = False
def pose_callback(msg):
twist_to_send = TwistWithCovarianceStamped()
twist_to_send.header.seq = twist_to_send.header.seq + 1
twist_to_send.header.stamp = rospy.Time.now()
twist_to_send.header.frame_id = 'base_link'
# turtlesim/Pose.msg -> msg.linear_velocity
twist_to_send.twist.twist.linear.x = msg.linear_velocity * (
1 + systemic_noise_linear + random.uniform(0, random_noise_linear))
twist_to_send.twist.twist.linear.y = 0.0
twist_to_send.twist.twist.linear.z = 0.0
twist_to_send.twist.twist.angular.x = 0.0
twist_to_send.twist.twist.angular.y = 0.0
twist_to_send.twist.twist.angular.z = msg.angular_velocity * (
1 + systemic_noise_angular + random.uniform(0, random_noise_angular))
twist_to_send.twist.covariance = systemic_noise_linear**2, 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, systemic_noise_angular**2
pub_twist.publish(twist_to_send)
def GetOdomTwistFromState(state, spot_wrapper):
"""Maps odometry data from robot state proto to ROS TwistWithCovarianceStamped message
Args:
data: Robot State proto
spot_wrapper: A SpotWrapper object
Returns:
TwistWithCovarianceStamped message
"""
twist_odom_msg = TwistWithCovarianceStamped()
local_time = spot_wrapper.robotToLocalTime(state.kinematic_state.acquisition_timestamp)
twist_odom_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos)
twist_odom_msg.twist.twist.linear.x = state.kinematic_state.velocity_of_body_in_odom.linear.x
twist_odom_msg.twist.twist.linear.y = state.kinematic_state.velocity_of_body_in_odom.linear.y
twist_odom_msg.twist.twist.linear.z = state.kinematic_state.velocity_of_body_in_odom.linear.z
twist_odom_msg.twist.twist.angular.x = state.kinematic_state.velocity_of_body_in_odom.angular.x
twist_odom_msg.twist.twist.angular.y = state.kinematic_state.velocity_of_body_in_odom.angular.y
twist_odom_msg.twist.twist.angular.z = state.kinematic_state.velocity_of_body_in_odom.angular.z
return twist_odom_msg
def __init__(self):
rospy.init_node('velocity_filter')
self._lock = threading.Lock()
self._optical_flow_sub = rospy.Subscriber(
'/optical_flow_estimator/twist', TwistWithCovarianceStamped,
lambda msg: self._callback('optical', msg))
# This topic is assumed to have rotation around +z
# (angle from +x with +y being +pi/2)
self._kalman_filter_sub = rospy.Subscriber(
'/odometry/filtered', Odometry,
lambda msg: self._callback('kalman', msg))
self._kalman_weight = rospy.get_param('~kalman_weight')
self._message_queue_length = rospy.get_param('~message_queue_length')
self._position_passthrough = rospy.get_param('~position_passthrough')
initial_msg = TwistWithCovarianceStamped()
initial_msg.header.stamp = rospy.Time()
# This queue contains 3-tuples, sorted by timestamp, oldest at index 0
#
# First item of each 3-tuple is a list with 3 items containing the x,
# y, and z velocities. The x and y values are the current values of the
# filter at that point, the z is too except that it's simply propogated
# forward from the last 'kalman' message.
#
# Second item of each 3-tuple is the message itself
#
# Third item of each 3-tuple is a string indicating the type of the
# message, either 'optical' or 'kalman'
self._queue = [([0.0, 0.0, 0.0], initial_msg, 'optical')]
self._vel_pub = rospy.Publisher('double_filtered_vel',
Odometry,
queue_size=10)
self._last_published_stamp = rospy.Time(0)
def __init__(self):
## Pull necessary ROS parameters from launch file:
# Read control message topic
self.ctrl_msg_top = rospy.get_param("~ctrl_message_topic",
"lli/ctrl_actuated")
# Read twist message topic
self.twist_msg_top = rospy.get_param("~twist_message_topic",
"actuation_twist")
# Read vehicle frame id topic
self.vehicle_frame_id = rospy.get_param("~frame_id", "base_link")
# Read max speed for gear 0 and 1
self.max_speed_0 = rospy.get_param("~max_speed_0", self.MAX_SPEED_0)
self.max_speed_1 = rospy.get_param("~max_speed_1", self.MAX_SPEED_1)
# Read max steering angle
self.max_steering_angle = rospy.get_param("~max_steering_angle",
self.MAX_STEERING_ANGLE)
# Read covariance values
self.lin_cov = rospy.get_param("~linear_covariance", 0.1)
self.ang_cov = rospy.get_param("~angular_covariance", 0.1)
# Publishing rate
self.rate = rospy.get_param("~rate", 50)
# Acceleration coefficient for gear 0 and gear 1
self.tau0 = rospy.get_param("~tau0", self.TAU0)
self.tau1 = rospy.get_param("~tau1", self.TAU1)
# Initialize class variables
self.twist_msg = TwistWithCovarianceStamped()
self.twist_msg.header.frame_id = self.vehicle_frame_id
self.twist_msg.twist.covariance = self.cov_matrix_build()
self._is_reverse = False
self._last_calc_time = None
self._actuation_values = SVEAControlValues()
self.velocity = 0.0
# Establish subscription to control message
rospy.Subscriber(self.ctrl_msg_top, lli_ctrl, self.ctrl_msg_callback)
# Establish publisher of converted Twist message
self.twist_pub = rospy.Publisher(self.twist_msg_top,
TwistWithCovarianceStamped,
queue_size=10)
def transfer(data, pub):
global prev_x, prev_time, frame_no
frame_no += 1
to_send = PoseWithCovarianceStamped()
to_send.header.seq = frame_no
to_send.header.stamp = rospy.Time.now()
to_send.header.frame_id = "map"
to_send.pose.pose.position.x = 0
to_send.pose.pose.position.y = 0
to_send.pose.pose.position.z = 0
to_send.pose.pose.orientation.w = 1
to_send.pose.pose.orientation.x = 0
to_send.pose.pose.orientation.y = 0
to_send.pose.pose.orientation.z = 0
to_send.pose.covariance[0] = 1
# to_send.pose.covariance[1:] = 0
to_send_twist = TwistWithCovarianceStamped()
to_send_twist.header.seq = frame_no
to_send_twist.header.stamp = rospy.Time.now()
to_send_twist.header.frame_id = "base_link"
to_send_twist.twist.twist.linear.x = data.linear_velocity + random.random(
) - random.random()
to_send_twist.twist.twist.linear.y = 0
to_send_twist.twist.twist.linear.z = 0
to_send_twist.twist.twist.angular.x = 0
to_send_twist.twist.twist.angular.y = 0
to_send_twist.twist.twist.angular.z = 0
to_send_twist.twist.covariance[0] = 1
# to_send_twist.twist.covariance[1:] = 0
prev_x = to_send.pose.pose.position.x
prev_time = time.time()
pub[0].publish(to_send)
pub[1].publish(to_send_twist)
def main():
rospy.init_node('wheel_encoder_reader', anonymous=False)
arg_dict = dict(
encoder_frame=rospy.get_param('~encoder_frame', 'base_link'),
encoder_topic=rospy.get_param('~encoder_topic', 'lli/encoder'),
direction_topic=rospy.get_param('~direction_topic', 'actuation_twist'),
axle_track=rospy.get_param('axle_track', DEFAULT_AXLE_TRACK),
wheel_radius=rospy.get_param('wheel_radius', DEFAULT_WHEEL_RADIUS),
linear_covariance=rospy.get_param('linear_covariance',
DEFAULT_LINEAR_COVARIANCE),
angular_covariance=rospy.get_param('angular_covariance',
DEFAULT_ANGULAR_COVARIANCE),
)
encoder_interface = WheelEncoder(**arg_dict)
twist_publisher = rospy.Publisher('wheel_encoder_twist',
TwistWithCovarianceStamped,
queue_size=1,
tcp_nodelay=True)
twist_msg = TwistWithCovarianceStamped()
set_covariance(
twist_msg.twist.covariance,
linear_covariance=encoder_interface.linear_covariance,
angular_covariance=encoder_interface.angular_covariance,
)
twist_msg.header.frame_id = encoder_interface.frame_id
def publish_twist(wheel_encoder):
twist_msg.header.stamp = rospy.Time.now()
twist = twist_msg.twist.twist
twist.linear.x = wheel_encoder.linear_velocity
twist.angular.z = wheel_encoder.angular_velocity
twist_publisher.publish(twist_msg)
encoder_interface.add_callback(publish_twist)
encoder_interface.start()
rospy.spin()
def pubVel(self, event):
v = TwistWithCovarianceStamped()
v.header.stamp = rospy.Time.now()
v.header.frame_id = 'vel'
# create a new velocity
v.twist.twist.linear.x = 0.9
v.twist.twist.linear.y = 0.0
v.twist.twist.linear.z = 0.0
# Only the number in the covariance matrix diagonal
# are used for the updates!
v.twist.covariance[0] = 0.01
v.twist.covariance[7] = 0.01
v.twist.covariance[14] = 0.01
print 'twist msg:', v
self.pub_vel.publish(v)
# Publish TF
vel_tf = tf.TransformBroadcaster()
o = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0, 'sxyz')
vel_tf.sendTransform((0.0, 0.0, 0.0), o, v.header.stamp,
v.header.frame_id, 'robot')
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 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 )