def __init__(self): self.logger = logging.getLogger('Borg.Brain.BodyController.Alice') self.obstacleAvoider = ObstacleAvoider() self.emergency = False self.maxSpeed = 0.2 self.doormaxSpeed = 0.5 self.odo_offset = {"x": 0, "y": 0, "angle": 0} self.last_odometry = { "x": 0, "y": 0, "angle": 0, "time": time.time(), "frame_id": "odom_frame" } self.body = body.bodycontroller.BodyController() self.config = self.body.get_config() self.waypoint_skip_time = None self.detections = [] self.VERSION = 2.0 # ROS node initialization self.ros_node_name = "Brain" self.ros_alice_odometry_topic = "odom" self.ros_alice_status_topic = "alice_status" self.ros_alice_twist_command_topic = "cmd_vel" #self.ros_alice_command_topic = "alicecontroller" #self.ros_output_topic = "odom" self.use_ros = ROS_ENABLED if not self.ros_init_topic(): self.logger.error("ROS import succeeded, but setting up " \ "publisher failed. Is roscore running?") self.use_ros = False self.transformer = tf.TransformListener() if self.use_ros and not self.ros_init_subscription(): self.logger.error( "ROS publishing enabled, but ROS subscription failed.") #Simple subscriber to indicate emergency state of the volksbot (TODO:should be done using ROS<->Memory interface?)). rospy.Subscriber("/emergency", Bool, self.emergency_callback)
def __init__(self, ip_address, port, start_pose=False): self.logger = logging.getLogger('Borg.Brain.BodyController.Pioneer') self.__ip_address = ip_address self.__port = int(port) self.logger.info("Connecting to %s:%d" % (self.__ip_address, self.__port)) self.obstacleAvoider = ObstacleAvoider() self.emergency = False self.maxSpeed = 400 self.doormaxSpeed = 500 self.odo_offset = {"x": 0, "y": 0, "angle": 0} self.last_odometry = { "x": 0, "y": 0, "angle": 0, "time": time.time(), "frame_id": "odom_frame" } self.body = body.bodycontroller.BodyController() self.config = self.body.get_config() self.waypoint_skip_time = None self.detections = [] self.VERSION = 1.0 # ROS node initialization self.ros_node_name = "Brain" self.ros_input_topic = "amcl_pose" self.ros_pioneer_odometry_topic = "pioneer_odometry" self.ros_pioneer_status_topic = "pioneer_status" self.ros_pioneer_twist_command_topic = "cmd_vel" self.ros_pioneer_command_topic = "pioneercontroller" self.ros_init_pose_topic = "initialpose" self.ros_output_topic = "odom" self.use_ros = ROS_ENABLED if self.use_ros: if not self.ros_init_topic(): self.logger.error("ROS import succeeded, but setting up " \ "publisher failed. Is roscore running?") self.use_ros = False self.transformer = tf.TransformListener() if self.use_ros and not self.ros_init_subscription(): self.logger.error( "ROS publishing enabled, but ROS subscription failed.") else: self.logger.warning("Not publishing to ROS as ROS modules cannot " \ "be imported") if start_pose and len(start_pose) == 3: self.set_odometry_offset(float(start_pose[0]), float(start_pose[1]), float(start_pose[2])) odo = { 'x': self.odo_offset['x'], 'y': self.odo_offset['y'], 'angle': self.odo_offset['angle'] } self.ros_publish_initial_pose(odo) self.last_odometry = { "x": self.odo_offset['x'], "y": self.odo_offset['y'], "angle": self.odo_offset['angle'], "time": time.time(), "frame_id": "odom_frame" }