def __init__(self, odometry_topic='/odom', pose_topic='/pose', duration=5):
super(MoveBase,
self).__init__(action_name="move_base",
action_type=move_base_msgs.MoveBaseAction,
worker=self.worker,
duration=duration)
self.odometry = nav_msgs.Odometry()
self.odometry.pose.pose.position = geometry_msgs.Point(0, 0, 0)
self.pose = geometry_msgs.PoseWithCovarianceStamped()
self.pose.pose.pose.position = geometry_msgs.Point(0, 0, 0)
latched = True
queue_size_five = 1
self.publishers = py_trees_ros.utilities.Publishers([
('pose', pose_topic, geometry_msgs.PoseWithCovarianceStamped,
latched, queue_size_five),
('odometry', odometry_topic, nav_msgs.Odometry, latched,
queue_size_five)
])
self.publishers.pose.publish(self.pose)
self.publishers.odometry.publish(self.odometry)
self.publishing_timer = rospy.Timer(period=rospy.Duration(0.5),
callback=self.publish,
oneshot=False)
self.start()
def initialise_messages():
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear.x = 0.0
cmd_vel.linear.y = 0.0
cmd_vel.linear.z = 0.0
cmd_vel.angular.x = 0.0
cmd_vel.angular.y = 0.0
cmd_vel.angular.z = 0.0
odom = nav_msgs.Odometry()
odom.header.frame_id = "base_link"
odom.pose.pose.position.x = 0.0
odom.pose.pose.position.y = 0.0
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.x = 0.0
odom.pose.pose.orientation.y = 0.0
odom.pose.pose.orientation.z = 0.0
odom.pose.pose.orientation.w = 1.0
odom.pose.covariance[0] = 0.1
odom.pose.covariance[7] = 0.1
odom.pose.covariance[35] = 0.2
odom.pose.covariance[14] = 10.0
odom.pose.covariance[21] = 10.0
odom.pose.covariance[28] = 10.0
odom.twist.twist.linear.x = 0.0
odom.twist.twist.linear.y = 0.0
odom.twist.twist.linear.z = 0.0
odom.twist.twist.angular.x = 0.0
odom.twist.twist.angular.y = 0.0
odom.twist.twist.angular.z = 0.0
return cmd_vel, odom
def __init__(self):
# state variables
self.last_stand = RapiroOdom.BOTH
self.last_r_yaw = math.pi / 2.
self.last_l_yaw = math.pi / 2.
self.last_time = rospy.Time.now()
# all TF stuff
self.tf_pub = tf2.TransformBroadcaster()
self.tf_msg = geometry_msgs.TransformStamped()
self.tf_msg.header.frame_id = 'odom'
self.tf_msg.child_frame_id = 'base_link'
self.tf_msg.transform.rotation.w = 1.
# all Odometry stuff
self.odom_pub = rospy.Publisher("odom", nav_msgs.Odometry, queue_size=10)
self.odom_msg = nav_msgs.Odometry()
self.odom_msg.header.frame_id = 'odom'
self.odom_msg.child_frame_id = 'base_link'
self.odom_msg.pose.pose.orientation.w = 1.
# the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations under # the License. """Prints the serialized bytese of a nav_msgs.Odometry message in hex. This can be modified slightly to print the serialized bytes in hex for arbitrary ROS messages and is useful for generating test cases for rosjava message serialization. """ __author__ = '[email protected] (Damon Kohler)' import io import roslib roslib.load_manifest('rosjava_test') import rospy import nav_msgs.msg as nav_msgs message = nav_msgs.Odometry() buf = io.StringIO() message.serialize(buf) print(''.join('0x%02x,' % ord(c) for c in buf.getvalue())[:-1])
def odom():
return navMsg.Odometry()