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