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"
}
