def __init__(self, settings_file=None, *args, **kwargs): ''' init ''' BaseController.__init__(self, settings_file=settings_file, *args, **kwargs) # Setup Default value self.flash_params = [] self.dialog = ConsoleDialog() if len(self.baseUrl) == 0: self.baseUrl = 'https://pvtbuilds.mozilla.org/pvt/mozilla.org/b2gotoro/nightly/' if len(self.destRootFolder) == 0: self.destRootFolder = 'pvt' self.destFolder = '' self.target_device = '' self.target_serial = '' self.target_branch = '' self.target_build = '' self.target_build_id = '' self.target_keep_profile = False # Load options from input argvs self.options = Parser.pvtArgParse(sys.argv[1:]) self._load_options()
def __init__(self): BaseController.__init__(self) self.rpm_lstm_obj = None self.oiltemp_lstm_obj = None self.y_hat, self.y_gt, self.gh_err = list(), list(), list() self.reset() self.chart_type = 0 self.chart_type_list = [self.rpm_lstm_obj, self.oiltemp_lstm_obj]
class ArduinoROS(): def __init__(self): rospy.init_node('Arduino', log_level=rospy.DEBUG) # Cleanup when terminating the node rospy.on_shutdown(self.shutdown) # A cmd_vel publisher so we can stop the robot when shutting down self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) # loop rate self.node_rate = int(rospy.get_param("~node_rate", 50)) r = rospy.Rate(self.node_rate) # create instance of the Arduino driver self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 115200)) self.com_timeout = rospy.get_param("~com_timeout", 0.5) rospy.loginfo("Requesting Arduino on port " + self.port + " at " + str(self.baud) + " baud with communication timeout " + str(self.com_timeout) + " s") self.controller = Arduino(self.port, self.baud, self.com_timeout) self.controller.connect() self.controller.reset_odometry() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # inform Arduino about IP address ip = socket.gethostbyname(socket.gethostname()) rospy.loginfo("send IP to Arduino: " + ip) self.controller.set_ip(ip) # Initialize the base controller self.base_frame = rospy.get_param("~base_frame", 'base_link') self.myBaseController = BaseController(self.controller, self.base_frame) # Reserve a thread lock mutex = thread.allocate_lock() # Start polling the base controller while not rospy.is_shutdown(): mutex.acquire() self.myBaseController.poll() mutex.release() r.sleep() def shutdown(self): # Stop the robot try: rospy.loginfo("Stopping the robot...") self.cmd_vel_pub.Publish(Twist()) rospy.sleep(2) except: pass rospy.loginfo("Shutting down Arduino Node...")
def __init__(self, settings_file=None, *args, **kwargs): ''' Generate base frame and each page, bind them in a list ''' BaseController.__init__(self, settings_file=settings_file, *args, **kwargs) self.root = Tk() self.frames = [] container = Frame(master=self.root) container.grid_rowconfigure(0, weight=1) container.grid_columnconfigure(0, weight=1) container.pack(side="top", fill="x", expand=False) self.container = container
def __init__(self, *args, **kwargs): ''' init ''' BaseController.__init__(self, *args, **kwargs) # Setup Default value self.flash_params = [] self.dialog = ConsoleDialog() if len(self.baseUrl) == 0: self.baseUrl = 'https://pvtbuilds.mozilla.org/pvt/mozilla.org/b2gotoro/nightly/' if len(self.destRootFolder) == 0: self.destRootFolder = 'pvt' self.destFolder = '' self.target_device = '' self.target_branch = '' self.target_build = '' self.target_build_id = '' # Load options from input argvs self.options = Parser.pvtArgParse(sys.argv[1:]) self._load_options()
def __init__(self, tag_poses, poselist_base2cam, pose_init, wheelbase, wheel_radius): """ Initialize the class Inputs: (all loaded from the parameter YAML file) pos_init - (3,) Numpy array specifying the initial position of the robot, formatted as usual as (x,y,theta) pos_goal - (3,) Numpy array specifying the final position of the robot, also formatted as (x,y,theta) """ # Initialize BaseController object: sends velocity commands and receives odometry measurements from Arduino self._base_controller = BaseController() # Initialize AprilTagLocalization object: broadcasts the static pose transforms # and calculates the base_link pose in map frame from on Apriltag detections self._apriltag_localization = AprilTagLocalization(tag_poses, poselist_base2cam) self._kalman_filter = KalmanFilter(pose_init, wheelbase, wheel_radius) self._diff_drive_controller = DiffDriveController() # Initialize cmd_vel publisher self._cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10) self._br = tf.TransformBroadcaster()
def __init__(self): rospy.init_node('Arduino', log_level=rospy.DEBUG) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 57600)) self.timeout = rospy.get_param("~timeout", 0.5) # Overall loop rate: should be faster than fastest sensor rate self.rate = int(rospy.get_param("~rate", 50)) r = rospy.Rate(self.rate) # Rate at which summary SensorState message is published. Individual sensors publish # at their own rates. self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) self.use_base_controller = rospy.get_param("~use_base_controller", False) # Set up the time for publishing the next SensorState message now = rospy.Time.now() self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) self.t_next_sensors = now + self.t_delta_sensors # Initialize a Twist message self.cmd_vel = Twist() # A cmd_vel publisher so we can stop the robot when shutting down self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # The SensorState publisher periodically publishes the values of all sensors on # a single topic. self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState) # A service to position a PWM servo rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler) # A service to read the position of a PWM servo rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) # A service to turn set the direction of a digital pin (0 = input, 1 = output) rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) # A service to turn a digital sensor on or off rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler) # Initialize the controlller self.controller = Arduino(self.port, self.baud, self.timeout, rospy.loginfo) # Make the connection self.controller.connect() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # Reserve a thread lock mutex = thread.allocate_lock() # Initialize any sensors self.mySensors = list() sensor_params = rospy.get_param("~sensors", dict({})) for name, params in sensor_params.iteritems(): # Set the direction to input if not specified try: params['direction'] except: params['direction'] = 'input' if params['type'] == "Ping": sensor = Ping(self.controller, name, params['pin'], params['rate']) elif params['type'] == "GP2D12": sensor = GP2D12(self.controller, name, params['pin'], params['rate']) elif params['type'] == 'Digital': sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction']) elif params['type'] == 'Analog': sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction']) elif params['type'] == 'PololuMotorCurrent': sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate']) elif params['type'] == 'PhidgetsVoltage': sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate']) elif params['type'] == 'PhidgetsCurrent': sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate']) # if params['type'] == "MaxEZ1": # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] self.mySensors.append(sensor) rospy.loginfo(name + " " + str(params)) self.myBaseController = BaseController(self.controller) # Start polling the sensors and base controller while not rospy.is_shutdown(): for sensor in self.mySensors: mutex.acquire() sensor.poll() mutex.release() if self.use_base_controller: mutex.acquire() self.myBaseController.poll() mutex.release() # Publish all sensor values on a single topic for convenience now = rospy.Time.now() if now > self.t_next_sensors: msg = SensorState() msg.header.frame_id = 'base_link' msg.header.stamp = now for i in range(len(self.mySensors)): msg.name.append(self.mySensors[i].name) msg.value.append(self.mySensors[i].value) try: self.sensorStatePub.publish(msg) except: pass self.t_next_sensors = now + self.t_delta_sensors r.sleep()
def __init__(self): rospy.init_node('Arduino', log_level=rospy.DEBUG) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) self.port = rospy.get_param("~port", "/dev/ttyACM0") self.baud = int(rospy.get_param("~baud", 115200)) self.timeout = rospy.get_param("~timeout", 0.5) self.base_frame = rospy.get_param("~base_frame", 'base_link') # Overall loop rate: should be faster than fastest sensor rate self.rate = int(rospy.get_param("~rate", 50)) r = rospy.Rate(self.rate) # Rate at which summary SensorState message is published. Individual sensors publish # at their own rates. #self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) self.use_base_controller = rospy.get_param("~use_base_controller", False) # Set up the time for publishing the next SensorState message now = rospy.Time.now() #self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) #self.t_next_sensors = now + self.t_delta_sensors # Initialize a Twist message self.cmd_vel = Twist() # A cmd_vel publisher so we can stop the robot when shutting down self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # The SensorState publisher periodically publishes the values of all sensors on # a single topic. #self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState) # A service to position a PWM servo #rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler) # A service to read the position of a PWM servo #rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) # A service to turn set the direction of a digital pin (0 = input, 1 = output) #rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) # A service to turn a digital sensor on or off #rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler) # A service to set pwm values for the pins #rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler) # Initialize the controlller self.controller = Arduino(self.port, self.baud, self.timeout) # Make the connection self.controller.connect() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # Reserve a thread lock mutex = thread.allocate_lock() # Initialize any sensors #self.mySensors = list() #sensor_params = rospy.get_param("~sensors", dict({})) #for name, params in sensor_params.iteritems(): # Set the direction to input if not specified # try: # params['direction'] # except: # params['direction'] = 'input' # if params['type'] == "Ping": # sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame) # elif params['type'] == "GP2D12": # sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame) # elif params['type'] == 'Digital': # sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) # elif params['type'] == 'Analog': # sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) # elif params['type'] == 'PololuMotorCurrent': # sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) # elif params['type'] == 'PhidgetsVoltage': # sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame) # elif params['type'] == 'PhidgetsCurrent': # sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) # if params['type'] == "MaxEZ1": # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] # self.mySensors.append(sensor) # rospy.loginfo(name + " " + str(params)) # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.controller, self.base_frame) # Start polling the sensors and base controller while not rospy.is_shutdown(): # for sensor in self.mySensors: # mutex.acquire() # sensor.poll() # mutex.release() if self.use_base_controller: mutex.acquire() self.myBaseController.poll() mutex.release() # Publish all sensor values on a single topic for convenience now = rospy.Time.now() # if now > self.t_next_sensors: # msg = SensorState() # msg.header.frame_id = self.base_frame # msg.header.stamp = now # for i in range(len(self.mySensors)): # msg.name.append(self.mySensors[i].name) # msg.value.append(self.mySensors[i].value) # try: # self.sensorStatePub.publish(msg) # except: # pass # self.t_next_sensors = now + self.t_delta_sensors r.sleep()
def prepare(): return BaseController(request)
class RobotControl(object): """ Class used to interface with the rover. Gets sensor measurements through ROS subscribers, and transforms them into the 2D plane, and publishes velocity commands. """ def __init__(self, tag_poses, poselist_base2cam, pose_init, wheelbase, wheel_radius): """ Initialize the class Inputs: (all loaded from the parameter YAML file) pos_init - (3,) Numpy array specifying the initial position of the robot, formatted as usual as (x,y,theta) pos_goal - (3,) Numpy array specifying the final position of the robot, also formatted as (x,y,theta) """ # Initialize BaseController object: sends velocity commands and receives odometry measurements from Arduino self._base_controller = BaseController() # Initialize AprilTagLocalization object: broadcasts the static pose transforms # and calculates the base_link pose in map frame from on Apriltag detections self._apriltag_localization = AprilTagLocalization(tag_poses, poselist_base2cam) self._kalman_filter = KalmanFilter(pose_init, wheelbase, wheel_radius) self._diff_drive_controller = DiffDriveController() # Initialize cmd_vel publisher self._cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=10) self._br = tf.TransformBroadcaster() def process_measurements(self, goal): """ YOUR CODE HERE Main loop of the robot - where all measurements, control, and estimation are done. """ self._apriltag_localization.broadcast_static_tf() odom_meas = self._base_controller.get_measurement() # current wheel angles (wheel_angle_left, wheel_angle_right) tag_meas = self._apriltag_localization.get_measurement() # calculated robot pose(s) based on Apriltag detections (x, y, theta) # Debug # print 'wheel_angle_left = ', self._base_controller._wheel_angle_left # print 'wheel_angle_right = ', self._base_controller._wheel_angle_right # print 'odom_meas = ', odom_meas # print 'tag_meas = ', tag_meas # odom_meas = None # tag_meas = None # Do KalmanFilter step pose_est = self._kalman_filter.step_filter(odom_meas, tag_meas) # print 'pose_est = ', pose_est # print 'goal = ', goal poselist_map2base_ekf = [pose_est[0], pose_est[1], 0, 0, 0, pose_est[2]] pubFrame(self._br, pose = poselist_map2base_ekf, frame_id = '/base_link_ekf', parent_frame_id = '/map') at_goal = False v, omega, at_goal = self._diff_drive_controller.compute_vel(pose_est, goal) cmd_vel_msg = Twist() cmd_vel_msg.linear.x = v cmd_vel_msg.angular.z = omega # self._cmd_vel_pub.publish(cmd_vel_msg) # self._base_controller.command_velocity(v, omega) return at_goal def shutdown(self): rospy.loginfo(rospy.get_caller_id() + "Navigation shutting down.") self._base_controller.shutdown()
class ArduinoROS(): def __init__(self): rospy.init_node('arduino', log_level=rospy.INFO) # Get the actual node name in case it is set in the launch file self.name = rospy.get_name() # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) self.port = rospy.get_param("~port", "/dev/ttyUSB0") self.baud = int(rospy.get_param("~baud", 57600)) self.timeout = rospy.get_param("~timeout", 0.5) self.base_frame = rospy.get_param("~base_frame", 'base_link') self.motors_reversed = rospy.get_param("~motors_reversed", False) # Overall loop rate: should be faster than fastest sensor rate self.rate = int(rospy.get_param("~rate", 50)) r = rospy.Rate(self.rate) # Rate at which summary SensorState message is published. Individual sensors publish # at their own rates. self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) self.use_base_controller = rospy.get_param("~use_base_controller", False) # Set up the time for publishing the next SensorState message now = rospy.Time.now() self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) self.t_next_sensors = now + self.t_delta_sensors # Initialize a Twist message self.cmd_vel = Twist() # A cmd_vel publisher so we can stop the robot when shutting down self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # The SensorState publisher periodically publishes the values of all sensors on # a single topic. self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)## # A service to position a PWM servo rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)## # A service to read the position of a PWM servo rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) # A service to turn set the direction of a digital pin (0 = input, 1 = output) rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) # A service to turn a digital sensor on or off rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler) # A service to read the value of a digital sensor rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) # A service to set pwm values for the pins rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler) # A service to read the value of an analog sensor rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler) # Initialize the controlller self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed) # Make the connection self.controller.connect() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # Reserve a thread lock mutex = thread.allocate_lock() # Initialize any sensors self.mySensors = list() sensor_params = rospy.get_param("~sensors", dict({})) for name, params in sensor_params.iteritems(): # Set the direction to input if not specified try: params['direction'] except: params['direction'] = 'input' if params['type'] == "Ping": sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == "GP2D12": sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame) # elif params['type'] == 'Digital': #sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) elif params['type'] == 'Analog': sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) elif params['type'] == 'PololuMotorCurrent': sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'PhidgetsVoltage': sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'PhidgetsCurrent': sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) # if params['type'] == "MaxEZ1": # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] try: self.mySensors.append(sensor) rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) except: rospy.logerr("Sensor type " + str(params['type']) + " not recognized.") # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller") # Start polling the sensors and base controller while not rospy.is_shutdown(): for sensor in self.mySensors: mutex.acquire() sensor.poll() mutex.release() if self.use_base_controller: mutex.acquire() self.myBaseController.poll() mutex.release() # Publish all sensor values on a single topic for convenience now = rospy.Time.now() if now > self.t_next_sensors: msg = SensorState() msg.header.frame_id = self.base_frame msg.header.stamp = now for i in range(len(self.mySensors)): msg.name.append(self.mySensors[i].name) msg.value.append(self.mySensors[i].value) try: self.sensorStatePub.publish(msg) except: pass self.t_next_sensors = now + self.t_delta_sensors r.sleep() # Service callback functions def ServoWriteHandler(self, req): self.controller.servo_write(req.id, req.value) return ServoWriteResponse() def ServoReadHandler(self, req): pos = self.controller.servo_read(req.id) return ServoReadResponse(pos) def DigitalSetDirectionHandler(self, req): self.controller.pin_mode(req.pin, req.direction) return DigitalSetDirectionResponse() def DigitalWriteHandler(self, req): self.controller.digital_write(req.pin, req.value) return DigitalWriteResponse() def DigitalReadHandler(self, req): value = self.controller.digital_read(req.pin) return DigitalReadResponse(value) def AnalogWriteHandler(self, req): self.controller.analog_write(req.pin, req.value) return AnalogWriteResponse() def AnalogReadHandler(self, req): value = self.controller.analog_read(req.pin) return AnalogReadResponse(value) def shutdown(self): rospy.loginfo("Shutting down Arduino Node...") # Stop the robot try: rospy.loginfo("Stopping the robot...") self.cmd_vel_pub.Publish(Twist()) rospy.sleep(2) except: pass # Close the serial port try: self.controller.close() except: pass finally: rospy.loginfo("Serial port closed.") os._exit(0)