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 odom2twsitStamped(self, odom):
odomTwist = TwistWithCovarianceStamped()
odomTwist.twist = odom.twist
odomTwist.twist = odom.twist
odomTwist.header = odom.header
return odomTwist
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)