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 __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", 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)