Esempio n. 1
0
 def __init__(self, name):
   rospy.loginfo("Starting %s", name)
   self.name = name
   self.board = FuriousBoard()
   self.throttle = Servo("throttle")
   self.steering = Servo("steering")
   self.pan = Servo("pan")
   self.tilt = Servo("tilt")
Esempio n. 2
0
 def __init__(self, name):
     rospy.loginfo("Starting %s", name)
     self.name = name
     self.board = FuriousBoard()
     self.throttle = Servo("throttle")
     self.steering = Servo("steering")
     self.pan = Servo("pan")
     self.tilt = Servo("tilt")
Esempio n. 3
0
class FuriousDriverNode(object):
  def __init__(self, name):
    rospy.loginfo("Starting %s", name)
    self.name = name
    self.board = FuriousBoard()
    self.throttle = Servo("throttle")
    self.steering = Servo("steering")
    self.pan = Servo("pan")
    self.tilt = Servo("tilt")

  def load_config(self, cmd_line_arg):
    cfg = find_config_file(cmd_line_arg)
    self.throttle.load_config(cfg)
    self.steering.load_config(cfg)
    self.pan.load_config(cfg)
    self.tilt.load_config(cfg)

  def detect_port(self, port):
    if not port:
      rospy.logwarn("No port specified.")
      try:
        if rospy.has_param(simulator.PARAM_NAME):
          port = rospy.get_param(simulator.PARAM_NAME)
          rospy.loginfo("Simulator detected: using port %s", port)
      except socket.error:
        rospy.logfatal("Unable to connnect to ROS master.  Is it running?")
        exit(1)
    if not port:
      rospy.loginfo("Scanning for Furious board...")
      port = self.board.probe()
    if not port:
      rospy.logfatal("ERROR: unable to detect Furious board.")
      exit(1)
    return port

  def connect(self, port):
    if self.board.is_valid_port(port):
      self.board.open(port)
      rospy.loginfo("Connected to furious board on port %s", port)
      # sweep the servos
#      self.board.servos[self.throttle.get_id()] = \
#        self.throttle.get_raw_value(-1.0)
#      sent = self.board.update()
#      rospy.sleep(0.1)

      rospy.loginfo("Booting ESC, please wait...")
      sent = self.stop_servos()
      rospy.loginfo("sent <%s> for init", sent)
      rospy.sleep(5)
      sent = self.stop_servos()
      rospy.loginfo("sent <%s> for init stop", sent)
      rospy.sleep(0.1)
      rospy.loginfo("Ready.")

#      rospy.sleep(0.1)
    else:
      rospy.logfatal("'%s' is not a valid port", port)
      exit(1)

  def init_ros(self, debug):
    """
    Establishes publishers, subscribers, and loop frequency, and
    calls rospy.init_node().
    """
    log_level = rospy.INFO
    if debug:
      log_level = rospy.DEBUG
    self.pub = rospy.Publisher(STATE_TOPIC, RobotState)
    rospy.init_node(self.name, log_level=log_level)
    rospy.Subscriber(SERVO_TOPIC, ServoCommand, self.on_servo_command)
    rospy.Subscriber(CAR_TOPIC, CarCommand, self.on_car_command)
    rospy.Subscriber(TURRET_TOPIC, TurretCommand, self.on_turret_command)
    self.timer = rospy.Rate(LOOP_FREQ)

  def on_servo_command(self, msg):
    self.board.servos[msg.id] = msg.pwm

  def on_car_command(self, msg):
    self.set_throttle(msg.throttle)
    self.set_steering(msg.steering)

  def on_turret_command(self, msg):
    self.board.servos[self.pan.get_id()] = \
        self.pan.get_raw_value(msg.pan)
    self.board.servos[self.tilt.get_id()] = \
        self.tilt.get_raw_value(msg.tilt)

  def set_throttle(self, val):
    self.board.servos[self.throttle.get_id()] = \
        self.throttle.get_raw_value(val)

  def set_steering(self, val):
    self.board.servos[self.steering.get_id()] = \
        self.steering.get_raw_value(val)

  def stop_servos(self):
    """
    Immediately returns all servos to their center positions.
    """
    self.set_throttle(0.0)
    self.set_steering(0.0)
    self.board.servos[self.pan.get_id()] = \
        self.pan.get_raw_value(0.0)
    self.board.servos[self.tilt.get_id()] = \
        self.tilt.get_raw_value(0.0)
    return self.board.update()

  def run(self):
    msg = self.init_state_msg()
    while self.board.is_ready() and (not rospy.is_shutdown()):
      try:
        sent = self.board.update()
        time = rospy.get_time() % 1000
        rospy.loginfo("Sent <%s> at time %.2f", sent, time)
        msg = self.update_state_msg(msg)
        self.pub.publish(msg)
        self.timer.sleep()
      except rospy.ROSInterruptException:
        rospy.logwarn("Caught ROSInterruptException")

  def init_state_msg(self):
    msg = RobotState()
    msg.portname = self.board.get_port_name()
    msg.num_analog = FuriousBoard.NUM_ANALOG
    msg.analog = [ 0 for x in range(msg.num_analog) ]
    msg.num_batteries = FuriousBoard.NUM_BATTERIES
    msg.battery_names = [ "motor", "logic" ]
    msg.batteries = [ 0 for x in range(msg.num_batteries) ]
    return msg

  def update_state_msg(self, msg):
    for i in range(FuriousBoard.NUM_ANALOG):
      msg.analog[i] = self.board.get_analog(i)
    msg.batteries[0] = self.board.get_motor_battery()
    msg.batteries[1] = self.board.get_logic_battery()
    msg.odometer = self.board.get_odometer()
    return msg

  def shutdown(self):
    rospy.loginfo("Shutting down %s", self.name)
    #sent = self.stop_servos()
    #rospy.logdebug("Sent <%s> for shutdown" % sent)
    sent = self.board.reset()
    rospy.loginfo("Sent <%s> for shutdown" % sent)
    self.board.close()
Esempio n. 4
0
class FuriousDriverNode(object):
    def __init__(self, name):
        rospy.loginfo("Starting %s", name)
        self.name = name
        self.board = FuriousBoard()
        self.throttle = Servo("throttle")
        self.steering = Servo("steering")
        self.pan = Servo("pan")
        self.tilt = Servo("tilt")

    def load_config(self, cmd_line_arg):
        cfg = find_config_file(cmd_line_arg)
        self.throttle.load_config(cfg)
        self.steering.load_config(cfg)
        self.pan.load_config(cfg)
        self.tilt.load_config(cfg)

    def detect_port(self, port):
        if not port:
            rospy.logwarn("No port specified.")
            try:
                if rospy.has_param(simulator.PARAM_NAME):
                    port = rospy.get_param(simulator.PARAM_NAME)
                    rospy.loginfo("Simulator detected: using port %s", port)
            except socket.error:
                rospy.logfatal(
                    "Unable to connnect to ROS master.  Is it running?")
                exit(1)
        if not port:
            rospy.loginfo("Scanning for Furious board...")
            port = self.board.probe()
        if not port:
            rospy.logfatal("ERROR: unable to detect Furious board.")
            exit(1)
        return port

    def connect(self, port):
        if self.board.is_valid_port(port):
            self.board.open(port)
            rospy.loginfo("Connected to furious board on port %s", port)
            # sweep the servos
            #      self.board.servos[self.throttle.get_id()] = \
            #        self.throttle.get_raw_value(-1.0)
            #      sent = self.board.update()
            #      rospy.sleep(0.1)

            rospy.loginfo("Booting ESC, please wait...")
            sent = self.stop_servos()
            rospy.loginfo("sent <%s> for init", sent)
            rospy.sleep(5)
            sent = self.stop_servos()
            rospy.loginfo("sent <%s> for init stop", sent)
            rospy.sleep(0.1)
            rospy.loginfo("Ready.")

#      rospy.sleep(0.1)
        else:
            rospy.logfatal("'%s' is not a valid port", port)
            exit(1)

    def init_ros(self, debug):
        """
    Establishes publishers, subscribers, and loop frequency, and
    calls rospy.init_node().
    """
        log_level = rospy.INFO
        if debug:
            log_level = rospy.DEBUG
        self.pub = rospy.Publisher(STATE_TOPIC, RobotState)
        rospy.init_node(self.name, log_level=log_level)
        rospy.Subscriber(SERVO_TOPIC, ServoCommand, self.on_servo_command)
        rospy.Subscriber(CAR_TOPIC, CarCommand, self.on_car_command)
        rospy.Subscriber(TURRET_TOPIC, TurretCommand, self.on_turret_command)
        self.timer = rospy.Rate(LOOP_FREQ)

    def on_servo_command(self, msg):
        self.board.servos[msg.id] = msg.pwm

    def on_car_command(self, msg):
        self.set_throttle(msg.throttle)
        self.set_steering(msg.steering)

    def on_turret_command(self, msg):
        self.board.servos[self.pan.get_id()] = \
            self.pan.get_raw_value(msg.pan)
        self.board.servos[self.tilt.get_id()] = \
            self.tilt.get_raw_value(msg.tilt)

    def set_throttle(self, val):
        self.board.servos[self.throttle.get_id()] = \
            self.throttle.get_raw_value(val)

    def set_steering(self, val):
        self.board.servos[self.steering.get_id()] = \
            self.steering.get_raw_value(val)

    def stop_servos(self):
        """
    Immediately returns all servos to their center positions.
    """
        self.set_throttle(0.0)
        self.set_steering(0.0)
        self.board.servos[self.pan.get_id()] = \
            self.pan.get_raw_value(0.0)
        self.board.servos[self.tilt.get_id()] = \
            self.tilt.get_raw_value(0.0)
        return self.board.update()

    def run(self):
        msg = self.init_state_msg()
        while self.board.is_ready() and (not rospy.is_shutdown()):
            try:
                sent = self.board.update()
                time = rospy.get_time() % 1000
                rospy.loginfo("Sent <%s> at time %.2f", sent, time)
                msg = self.update_state_msg(msg)
                self.pub.publish(msg)
                self.timer.sleep()
            except rospy.ROSInterruptException:
                rospy.logwarn("Caught ROSInterruptException")

    def init_state_msg(self):
        msg = RobotState()
        msg.portname = self.board.get_port_name()
        msg.num_analog = FuriousBoard.NUM_ANALOG
        msg.analog = [0 for x in range(msg.num_analog)]
        msg.num_batteries = FuriousBoard.NUM_BATTERIES
        msg.battery_names = ["motor", "logic"]
        msg.batteries = [0 for x in range(msg.num_batteries)]
        return msg

    def update_state_msg(self, msg):
        for i in range(FuriousBoard.NUM_ANALOG):
            msg.analog[i] = self.board.get_analog(i)
        msg.batteries[0] = self.board.get_motor_battery()
        msg.batteries[1] = self.board.get_logic_battery()
        msg.odometer = self.board.get_odometer()
        return msg

    def shutdown(self):
        rospy.loginfo("Shutting down %s", self.name)
        #sent = self.stop_servos()
        #rospy.logdebug("Sent <%s> for shutdown" % sent)
        sent = self.board.reset()
        rospy.loginfo("Sent <%s> for shutdown" % sent)
        self.board.close()