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 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 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
def packageTwist(self, data): out = TwistWithCovariance() out.twist.linear = listVector3Conversion(self.lin_vel) out.twist.angular = data.angular_velocity out.covariance = OdometryFaker.covariance return out