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
