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)
