def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing"); 

def load_robot_description():
    sensors_config_file =   rospy.get_param('/fssim/sensors_config_file')
    car_config_file     =   rospy.get_param('/fssim/car_config_file')
    car_dimensions_file =   rospy.get_param('/fssim/car_structure_file')
    robot_name          =   rospy.get_param('/fssim/robot_name')

    model_filepath = rospy.get_param('/fssim/model_filepath')

    print sensors_config_file
    print car_config_file
    print car_dimensions_file
    print model_filepath

    try:
        command_string = "rosrun xacro xacro --inorder {} robot_name:='{}' sensors_config_file:='{}' car_config_file:='{}' car_dimensions_file:='{}".format(model_filepath, 
            robot_name, sensors_config_file, car_config_file, car_dimensions_file)
        robot_description = subprocess.check_output(command_string, shell=True, stderr=subprocess.STDOUT)   
    except subprocess.CalledProcessError as process_error:
        rospy.logfatal('Failed to run xacro command with error: \n%s', process_error.output)
        sys.exit(1)

    rospy.set_param("/robot_description", robot_description) 

def create_camera(self, ego_actor):
        """
        Attach the camera to the ego vehicle
        """
        bp_library = self.world.get_blueprint_library()
        try:
            bp = bp_library.find("sensor.camera.rgb")
            bp.set_attribute('role_name', "spectator_view")
            rospy.loginfo("Creating camera with resolution {}x{}, fov {}".format(
                self.camera_resolution_x,
                self.camera_resolution_y,
                self.camera_fov))
            bp.set_attribute('image_size_x', str(self.camera_resolution_x))
            bp.set_attribute('image_size_y', str(self.camera_resolution_y))
            bp.set_attribute('fov', str(self.camera_fov))
        except KeyError as e:
            rospy.logfatal("Camera could not be spawned: '{}'".format(e))
        transform = self.get_camera_transform()
        if not transform:
            transform = carla.Transform()
        self.camera_actor = self.world.spawn_actor(bp, transform,
                                                   attach_to=ego_actor) 

def start_countdown(seconds):
    def timeout_countdown(seconds):
        time.sleep(seconds)
        rospy.logfatal("COUNTDOWN ERROR RUNTIME TEST")
        os.kill(os.getpid(), signal.SIGUSR1)
        sys.exit(1)
        
    t= Thread(target = lambda: timeout_countdown(seconds))
    t.start()
#---------------------------------------------
# launch countdown 

def __handle_mln_query(self, request):
        try:
            rospy.loginfo("Processing request...")
            config = self.__get_config(request)
            if self.__config_changed(config):
                rospy.loginfo("Configuration changed")
                self.__mln = MLN(config.logic, config.grammar, config.mlnFiles)
                self.__mln_date = os.path.getmtime(config.mlnFiles)
                self.__config = config
            db = self.__get_db(request, config, self.__mln)
            materialized_mln = self.__mln.materialize(db)
            mrf = materialized_mln.ground(db)
            if not request.query:
                raise Exception("No query provided!")
            inference = InferenceMethods.clazz(config.method)(mrf, request.query.queries)
            result = inference.run()
            self.__save_results(config, result)
            tuple_list = []
            for atom, probability in inference.results.items():
                tuple_list.append(AtomProbPair(str(atom), float(probability)))
            tuple_list.sort(key=lambda item: item.prob, reverse=True)
            to_return = MLNInterfaceResponse(MLNDatabase(tuple_list))
            rospy.loginfo("Done!")
            return to_return
        except Exception:
            rospy.logfatal(traceback.format_exc())
            return MLNDatabase([]) 

def connect(self):
        try:
            self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo)
            self.__find_motors()
        except dynamixel_io.SerialOpenError, e:
            rospy.logfatal(e.message)
            sys.exit(1) 

def reset(self):
        """
        Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable
        the robot.
        """
        error_not_stopped = """\
Robot is not in a Error State. Cannot perform Reset.
"""
        error_estop = """\
E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
"""
        error_nonfatal = """Non-fatal Robot Error on reset.
Robot reset cleared stopped state and robot can be enabled, but a non-fatal
error persists. Check diagnostics or rethink.log for more info.
"""
        error_env = """Failed to reset robot.
Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
and resolvable. For more information please visit:
http://sdk.rethinkrobotics.com/intera/SDK_Shell
"""


        is_reset = lambda: (self._state.stopped == False and
                            self._state.error == False and
                            self._state.estop_button == 0 and
                            self._state.estop_source == 0)
        pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10)

        if (not self._state.stopped):
            rospy.logfatal(error_not_stopped)
            raise IOError(errno.EREMOTEIO, "Failed to Reset due to lack of Error State.")

        if (self._state.stopped and
              self._state.estop_button == RobotAssemblyState.ESTOP_BUTTON_PRESSED):
            rospy.logfatal(error_estop)
            raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")

        rospy.loginfo("Resetting robot...")
        try:
            intera_dataflow.wait_for(
                test=is_reset,
                timeout=5.0,
                timeout_msg=error_env,
                body=pub.publish
            )
        except OSError, e:
            if e.errno == errno.ETIMEDOUT:
                if self._state.error == True and self._state.stopped == False:
                    rospy.logwarn(error_nonfatal)
                    return False
            raise 

def __find_motors(self):
        rospy.loginfo('%s: Pinging motor IDs %d through %d...' % (self.port_namespace, self.min_motor_id, self.max_motor_id))
        self.motors = []
        self.motor_static_info = {}
        
        for motor_id in range(self.min_motor_id, self.max_motor_id + 1):
            for trial in range(self.num_ping_retries):
                try:
                    result = self.dxl_io.ping(motor_id)
                except Exception as ex:
                    rospy.logerr('Exception thrown while pinging motor %d - %s' % (motor_id, ex))
                    continue
                    
                if result:
                    self.motors.append(motor_id)
                    break
                    
        if not self.motors:
            rospy.logfatal('%s: No motors found.' % self.port_namespace)
            sys.exit(1)
            
        counts = defaultdict(int)
        
        to_delete_if_error = []
        for motor_id in self.motors:
            for trial in range(self.num_ping_retries):
                try:
                    model_number = self.dxl_io.get_model_number(motor_id)
                    self.__fill_motor_parameters(motor_id, model_number)
                except Exception as ex:
                    rospy.logerr('Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex))
                    if trial == self.num_ping_retries - 1: to_delete_if_error.append(motor_id)
                    continue
                    
                counts[model_number] += 1
                break
                
        for motor_id in to_delete_if_error:
            self.motors.remove(motor_id)
            
        rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors)
        
        status_str = '%s: Found %d motors - ' % (self.port_namespace, len(self.motors))
        for model_number,count in counts.items():
            if count:
                model_name = DXL_MODEL_TO_PARAMS[model_number]['name']
                status_str += '%d %s [' % (count, model_name)
                
                for motor_id in self.motors:
                    if self.motor_static_info[motor_id]['model'] == model_name:
                        status_str += '%d, ' % motor_id
                        
                status_str = status_str[:-2] + '], '
                
        rospy.loginfo('%s, initialization complete.' % status_str[:-2]) 

def main():
    """
    main function for carla simulator ROS bridge
    maintaining the communication client and the CarlaBridge object
    """
    rospy.init_node("carla_bridge", anonymous=True)
    parameters = rospy.get_param('carla')
    rospy.loginfo("Trying to connect to {host}:{port}".format(
        host=parameters['host'], port=parameters['port']))

    carla_bridge = None
    carla_world = None
    carla_client = None
    try:
        carla_client = carla.Client(
            host=parameters['host'],
            port=parameters['port'])
        carla_client.set_timeout(parameters['timeout'])

        # check carla version
        dist = pkg_resources.get_distribution("carla")
        if LooseVersion(dist.version) < LooseVersion(CarlaRosBridge.CARLA_VERSION):
            rospy.logfatal("CARLA python module version {} required. Found: {}".format(
                CarlaRosBridge.CARLA_VERSION, dist.version))
            sys.exit(1)

        if LooseVersion(carla_client.get_server_version()) < \
           LooseVersion(CarlaRosBridge.CARLA_VERSION):
            rospy.logfatal("CARLA Server version {} required. Found: {}".format(
                CarlaRosBridge.CARLA_VERSION, carla_client.get_server_version()))
            sys.exit(1)

        carla_world = carla_client.get_world()

        if "town" in parameters:
            if parameters["town"].endswith(".xodr"):
                rospy.loginfo("Loading opendrive world from file '{}'".format(parameters["town"]))
                with open(parameters["town"]) as od_file:
                    data = od_file.read()
                carla_world = carla_client.generate_opendrive_world(str(data))
            else:
                if carla_world.get_map().name != parameters["town"]:
                    rospy.loginfo("Loading town '{}' (previous: '{}').".format(
                        parameters["town"], carla_world.get_map().name))
                    carla_world = carla_client.load_world(parameters["town"])
            carla_world.tick()

        carla_bridge = CarlaRosBridge(carla_client.get_world(), parameters)
        carla_bridge.run()
    except (IOError, RuntimeError) as e:
        rospy.logerr("Error: {}".format(e))
    finally:
        del carla_world
        del carla_client