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)