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 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 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 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
def state_to_twist_with_cov(state_vec, cov):
out = TwistWithCovariance()
out.twist = state_2_twist(state_vec)
out.covariance = fill_twist_covariance(cov)
return out
