class OdometryPublisher: def __init__(self): rospy.init_node('wheel_odom_publisher') self.chassis = Chassis() self.wheel_shifts_sub = rospy.Subscriber('/wheel_shifts', WheelShifts, self.publishOdom) self.odometry_pub = rospy.Publisher('/wheel_odom', Odometry, queue_size=1) self.odometry_calc = OdometryCalculator(self.chassis) self.rate = rospy.Rate(15) self.reset_odom_service = rospy.Service( 'reset_wheel_odom', SetBool, self.resetWheelOdom) #resets odometry to 0,0,0 def publishOdom(self, wheel_shifts): self.chassis.updateState(wheel_shifts) odom = self.odometry_calc.calcOdom() self.odometry_pub.publish(odom) self.rate.sleep() def resetWheelOdom(self, req): if req.data == True: self.odometry_calc.resetPose() return True, "ok"
def execute(self): leftInput = OI.getInstance().getLeftY() rightInput = OI.getInstance().getRightY() forwardSpeed = self.__desensitize((leftInput + rightInput) / 2) rotationalSpeed = self.__desensitize((leftInput - rightInput) / 2, 3) Chassis.getInstance().setPower(forwardSpeed, rotationalSpeed)
class Server: """A server is managed over IPMI""" def __init__(self, bmc): """ Arguments: bmc -- the BMC that's managing this server """ self.bmc = bmc self._chassis = Chassis(self.bmc.handle) def power_off(self): """Power off server if it's powered on""" if self._chassis.is_powered: self._chassis.power_off() def power_on(self): """Power on server if it's powered off""" if not self._chassis.is_powered: self._chassis.power_on() def power_cycle(self): """Power cycle server""" self._chassis.power_cycle() def hard_reset(self): """Warm reset server""" self._chassis.hard_reset() @property def is_powered(self): """True if server is powered, otherwise false""" return self._chassis.is_powered
def __init__(self, rospy): print 'Differential drive node initialization...' self.left_speed = 0 self.right_speed = 0 self.left_length = 0 self.right_length = 0 print 'Initialize Components...' self.chassis = Chassis() self.speed_measures = SpeedSensors(rospy) print 'Register control topic...' rospy.Subscriber("robot_mg", DiffSpeed, self.move) print 'Done !'
def __init__(self): rospy.init_node('wheel_odom_publisher') self.chassis = Chassis() self.wheel_shifts_sub = rospy.Subscriber('/wheel_shifts', WheelShifts, self.publishOdom) self.odometry_pub = rospy.Publisher('/wheel_odom', Odometry, queue_size=1) self.odometry_calc = OdometryCalculator(self.chassis) self.rate = rospy.Rate(15) self.reset_odom_service = rospy.Service( 'reset_wheel_odom', SetBool, self.resetWheelOdom) #resets odometry to 0,0,0
def __init__(self, host_and_port, adapter, log, device_id): self.log = log self.headers = {'Accept-Encoding': None} self.device_id = device_id """self.adapter_agent = adapter.adapter_agent self.adapter_name = adapter.name """ self.host_and_port = host_and_port.split( ':')[0] + Defaults.redfish_port requests.packages.urllib3.disable_warnings(InsecureRequestWarning) self.chassis_obj = Chassis(self) self.systems_obj = Systems(self) self.ethernet_obj = Ethernet(self) self.sys_info = {}
def test_creer_chassis(): result = True try: nom = "monChassis" poids = 10 aire_frontale = 2.4 coefficient_trainee = 0.6 chassis = Chassis(nom=nom, poids=poids, aire_frontale=aire_frontale, coefficient_trainee=coefficient_trainee) except: return False result &= chassis.nom == nom result &= chassis.poids == poids result &= chassis.aire_frontale == aire_frontale result &= chassis.coefficient_trainee == coefficient_trainee expected_dict = { '_Chassis__aire_frontale': 2.4, '_Chassis__coefficient_trainee': 0.6, '_Composante__nom': 'monChassis', '_Composante__poids': 10 } result &= chassis.__dict__ == expected_dict return result
def __init__(self, bmc): """ Arguments: bmc -- the BMC that's managing this server """ self.bmc = bmc self._chassis = Chassis(self.bmc.handle)
def __init__(self): self.lock = Lock() self.is_stopped = True self.pub = rospy.Publisher('odom', Odometry, queue_size=10) self.chassis = Chassis() self.target_wheel_speed = [0, 0, 0, 0] self.now_wheel_speed = [0, 0, 0, 0] self.last_moves = self.chassis.get_feedback() self.now_x = 0 self.now_y = 0 self.now_theta = 0 self.in_action_service = False self._simple_move_feedback = SimpleMoveFeedback() self._simple_move_result = SimpleMoveResult() self._as = actionlib.SimpleActionServer("simple_move", SimpleMoveAction, execute_cb = self.execut_cb, auto_start = False) self._as.start()
def main(): parser = argparse.ArgumentParser(description='Starts NetBot client.') parser.add_argument('host', action="store") parser.add_argument('--port', action="store", dest="port", default=2345, type=int, required=False) args = parser.parse_args() host = args.host port = args.port if (host == None) | (port == None): print(parser.usage) exit(0) else: chassis = Chassis() chassis.activate() client = Client(lambda msg: process_message(msg, chassis), get_msg_obj) client.init(host, port) capture_thread = threading.Thread(target=image_capture_thread_func, args=(client, )) capture_thread.start() done = False while not done: done = not client.process_recv_message() chassis.dectivate() stop_event.set() capture_thread.join() for _, cam in cameras.items(): cam.release() client.close()
def get_chassis(self): from chassis import Chassis chassis = Chassis() return chassis
class ChassisControlNode: def __init__(self): self.lock = Lock() self.is_stopped = True self.pub = rospy.Publisher('odom', Odometry, queue_size=10) self.chassis = Chassis() self.target_wheel_speed = [0, 0, 0, 0] self.now_wheel_speed = [0, 0, 0, 0] self.last_moves = self.chassis.get_feedback() self.now_x = 0 self.now_y = 0 self.now_theta = 0 self.in_action_service = False self._simple_move_feedback = SimpleMoveFeedback() self._simple_move_result = SimpleMoveResult() self._as = actionlib.SimpleActionServer("simple_move", SimpleMoveAction, execute_cb = self.execut_cb, auto_start = False) self._as.start() def execut_cb(self, goal): rate = rospy.Rate(RATE) self._simple_move_feedback.moved_distance.x = 0 self._simple_move_feedback.moved_distance.y = 0 self._simple_move_feedback.moved_distance.theta = 0 with self.lock: self.in_action_service = True original_moves = array(self.chassis.get_feedback(), dtype=np.float64) self.chassis.stop() target = goal.target distance = array([target.x, target.y, target.theta]) wheel_moves = MACANUM_MAT.dot(distance.T) total_distance = norm(wheel_moves) if total_distance < MAX_DELTA_VELOCITY: self._simple_move_result.success = True self._as.set_succeeded(self._simple_move_result) return moved_distance = 0 vels = wheel_moves / total_distance self.chassis.set_wheels_speed(vels * MAX_DELTA_VELOCITY * 4) real_moves = array([abs(v) for v in wheel_moves]) self.chassis.set_wheels_count(real_moves) success = True while True: if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._simple_move_result.success = False self._simple_move_result.moved_distance = self._simple_move_feedback.moved_distance self._as.set_preempted(self._simple_move_result) with self.lock: self.chassis.stop() self.in_action_service = False return with self.lock: now_moves = array(self.chassis.get_feedback(), dtype=np.float64) moved_distance = norm(now_moves - original_moves) now_speed = self.get_speed(moved_distance, total_distance) wheel_vels = now_speed * vels x, y, theta = tuple(lstsq(MACANUM_MAT, now_moves - original_moves)[0]) self._simple_move_feedback.moved_distance.x = x self._simple_move_feedback.moved_distance.y = y self._simple_move_feedback.moved_distance.theta = theta self._as.publish_feedback(self._simple_move_feedback) with self.lock: self.chassis.set_wheels_speed(wheel_vels * 4) if (fabs(moved_distance - total_distance) < 10): break rate.sleep() self._simple_move_result.success = success self._simple_move_result.moved_distance = self._simple_move_feedback.moved_distance if success: self._as.set_succeeded(self._simple_move_result) else: self._as.set_aborted(self._simple_move_result) with self.lock: self.in_action_service = False def get_speed(self, moved_distance, total_distance): if moved_distance > total_distance: rospy.logwarn("Moved too much") remain_distance = total_distance - moved_distance if remain_distance < moved_distance: moved_distance = remain_distance factor = (moved_distance / total_distance) / 0.2 if factor > 1: factor = 1 speed = factor * DEFAULT_VELOCITY if speed < MAX_DELTA_VELOCITY: speed = MAX_DELTA_VELOCITY return speed def vel_callback(self, vel): if abs(vel.linear.x) > MAX_VELOCITY or abs(vel.linear.y) > MAX_VELOCITY or abs(vel.angular.z) > MAX_A_VELOCITY: rospy.logwarn('vel too big!') return vel = array([vel.linear.x, vel.linear.y, vel.angular.z]) self.is_stopped = False with self.lock: self.target_wheel_speed = MACANUM_MAT.dot(vel.T) * PV_RATE def run(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): with self.lock: if not self.in_action_service: self.set_speed() self.feedback() rate.sleep() def set_speed(self): delta_wheel_speed = array(self.target_wheel_speed) - array(self.now_wheel_speed) if self.is_stopped: self.chassis.stop() else: if norm(delta_wheel_speed) > MAX_DELTA_VELOCITY: delta_wheel_speed /= norm(delta_wheel_speed) delta_wheel_speed *= MAX_DELTA_VELOCITY self.now_wheel_speed += delta_wheel_speed if norm(self.now_wheel_speed) < 1: self.now_wheel_speed = [0] * 4 self.is_stopped = True self.chassis.stop() else: self.chassis.set_wheels_speed(self.now_wheel_speed) self.chassis.set_wheels_count([10000] * 4) def feedback(self): new_moves = self.chassis.get_feedback() delta_moves = array(new_moves) - array(self.last_moves) self.last_moves = new_moves x, y, theta = tuple(lstsq(MACANUM_MAT, delta_moves)[0]) x_odom = cos(self.now_theta) * x - sin(self.now_theta) * y y_odom = sin(self.now_theta) * x + cos(self.now_theta) * y self.now_x += x_odom self.now_y += y_odom self.now_theta += theta self.odom_post(self.now_x, self.now_y, self.now_theta) def odom_post(self, now_x, now_y, now_theta): odom_broadcaster = tf.TransformBroadcaster() current_time = rospy.Time.now() odom_quat = tf.transformations.quaternion_from_euler(0,0,now_theta) odom_broadcaster.sendTransform((now_x, now_y, 0), odom_quat, current_time, "base_link", "odom") odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.pose.pose.position.x = now_x odom.pose.pose.position.y = now_y odom.pose.pose.orientation.x = odom_quat[0] odom.pose.pose.orientation.y = odom_quat[1] odom.pose.pose.orientation.z = odom_quat[2] odom.pose.pose.orientation.w = odom_quat[3] self.pub.publish(odom)
class RestClient(object): def __init__(self, host_and_port, adapter, log, device_id): self.log = log self.headers = {'Accept-Encoding': None} self.device_id = device_id """self.adapter_agent = adapter.adapter_agent self.adapter_name = adapter.name """ self.host_and_port = host_and_port.split( ':')[0] + Defaults.redfish_port requests.packages.urllib3.disable_warnings(InsecureRequestWarning) self.chassis_obj = Chassis(self) self.systems_obj = Systems(self) self.ethernet_obj = Ethernet(self) self.sys_info = {} def get_system_details(self): """ Method to fetch OLT info. This method is supposed to get invoked from NBI API get_device_details. Currently the get_device_details() is in NotImplemented stated """ self.systems_obj.system_get(self.sys_info) return self.sys_info def start_health_monitoring(self): """ Method to fetch health status of asfvolt16 olt hardware modules """ self.chassis_obj.get_chassis_health() self.ethernet_obj.get_ethernet_health() def stop_health_monitoring(self): """ Method to stop health monitoring of asfvolt16 olt modules are not controlled by bal """ self.chassis_obj.stop_chassis_monitoring() self.ethernet_obj.stop_ether_monitoring() def reboot_device(self): """ Reboot asfvolt16 olt through Redfish service """ self.systems_obj.reboot_olt() """def generate_alarm(self, status, alarm, alarm_severity): Method to create and submit alarms. if(alarm_severity == "Warning"): al_severity = AlarmEventSeverity.WARNING elif (alarm_severity == "Critical"): al_severity = AlarmEventSeverity.CRITICAL try: ts = arrow.utcnow().timestamp alarm_event = self.adapter_agent.create_alarm( id='voltha.{}.{}.olt'.format(self.adapter_name, self.device_id), resource_id='olt', type=AlarmEventType.EQUIPMENT, severity=al_severity, category=AlarmEventCategory.OLT, state=AlarmEventState.RAISED if status else AlarmEventState.CLEARED, description='OLT Alarm - Health Monitoring asfvolt16 olt - {}' .format('Raised' if status else 'Cleared'), context=alarm, raised_ts=ts) self.adapter_agent.submit_alarm(self.device_id, alarm_event) except Exception as e: self.log.exception('failed-to-submit-alarm', e=e) """ def http_get(self, uri=UriConst.base_uri): """ Method to send http GET request. """ url = UriConst.HTTPS + self.host_and_port + uri #print ("http_get url[%s]"%url) try: ret = requests.request(RestConst.GET, url=url, headers=self.headers, auth=None, verify=False) return ret except requests.exceptions.ConnectTimeout: print("Connection-timed-out.") except Exception as e: print("Exception1-occurred-:", str(e)) def http_put(self, uri=UriConst.base_uri, data=None): """ Method to send http PUT request. """ url = UriConst.HTTPS + self.host_and_port + uri try: ret = requests.request(RestConst.PUT, url=url, headers=self.headers, auth=None, verify=False, data=data) return ret except requests.exceptions.ConnectTimeout: print("Connection-timed-out.") except Exception as e: print("Exception1-occurred-:", str(e)) def http_post(self, uri=UriConst.base_uri, data=None): """ Method to send http POST request. """ url = UriConst.HTTPS + self.host_and_port + uri try: ret = requests.request(RestConst.POST, url=url, headers=self.headers, auth=None, verify=False, data=data) return ret except requests.exceptions.ConnectTimeout: print("Connection-timed-out.") except Exception as e: print("Exception1-occurred-:", str(e)) def http_patch(self, uri=UriConst.base_uri, data=None): """ Method to send http Patch request. """ url = UriConst.HTTPS + self.host_and_port + uri try: ret = requests.request(RestConst.PATCH, url=url, headers=self.headers, auth=None, verify=False, data=data) return ret except requests.exceptions.ConnectTimeout: print("Connection-timed-out.") except Exception as e: print("Exception1-occurred-:", str(e))
#from chassis import Chassis from telemetry import Telemetry from chassis import Chassis #192.168.1.107 IP_ADDRESS = '10.0.0.22' PORT = 65432 SSID = 'utmresak' telemetry = Telemetry(IP_ADDRESS, PORT, 'wlan0', SSID) chassis = Chassis() while True: connected = telemetry.ping() if not connected: #failsafe condition chassis.stop_all( ) #for extra safety but technically redundant as telemetry.speed is set to 0 failsafe condition telemetry.get_connection(IP_ADDRESS, PORT) chassis.drive(int(telemetry.speed * 100))
def __init__(self): super(self) self.chassis = Chassis.getInstance()
class DiffDriveControlHandler: servo_min = 800 # Min pulse length out of 4096 servo_max = 4095 # Max pulse length out of 4096 servo_scale = servo_max - servo_min def __init__(self, rospy): print 'Differential drive node initialization...' self.left_speed = 0 self.right_speed = 0 self.left_length = 0 self.right_length = 0 print 'Initialize Components...' self.chassis = Chassis() self.speed_measures = SpeedSensors(rospy) print 'Register control topic...' rospy.Subscriber("robot_mg", DiffSpeed, self.move) print 'Done !' def setSpeed(self): self.chassis.setLeftSpeed(int(self.left_length)) self.chassis.setRightSpeed(int(self.right_length)) def move(self, msg): self.left_speed = msg.left_speed self.right_speed = msg.right_speed #print self.left_speed, self.right_speed if self.left_speed == 0 and self.right_speed == 0: self.chassis.stop() self.speed_measures.stop_left_speed() self.speed_measures.stop_right_speed() #print 'stop' else: self.left_length = (self.servo_scale * (abs(self.left_speed) / 100)) + self.servo_min self.right_length = ( self.servo_scale * (abs(self.right_speed) / 100)) + self.servo_min if self.left_speed > 0 and self.right_speed > 0: self.speed_measures.set_left_direction(1) self.speed_measures.set_right_direction(1) #print 'forward' self.chassis.forward() self.setSpeed() elif self.left_speed < 0 and self.right_speed > 0: self.speed_measures.set_left_direction(0) self.speed_measures.set_right_direction(1) #print 'left' self.chassis.turnLeft() self.setSpeed() elif self.left_speed > 0 and self.right_speed < 0: self.speed_measures.set_left_direction(1) self.speed_measures.set_right_direction(0) #print 'right' self.chassis.turnRight() self.setSpeed() elif self.left_speed < 0 and self.right_speed < 0: self.speed_measures.set_left_direction(0) self.speed_measures.set_right_direction(0) #print 'reverse' self.chassis.reverse() self.setSpeed()
def test_creer_vehicule(): result = True try: nom_vehicule = "monVehicule" position_dep = [45, -2, 57] nom_roue = "maRoue" poids_roue = 10 coefficient_friction = 0.4 poids_supporte = 10 roues = [ Roue(nom=nom_roue, poids=poids_roue, coefficient_friction=coefficient_friction, poids_supporte=poids_supporte), Roue(nom=nom_roue, poids=2 * poids_roue, coefficient_friction=2 * coefficient_friction, poids_supporte=2 * poids_supporte) ] nom_moteur = "monMoteur" poids_moteur = 10 acceleration = 2.5 moteur = Moteur(nom=nom_moteur, poids=poids_moteur, acceleration=acceleration) nom_chassis = "monChassis" poids_chassis = 10 aire_frontale = 2.4 coefficient_trainee = 0.6 chassis = Chassis(nom=nom_chassis, poids=poids_chassis, aire_frontale=aire_frontale, coefficient_trainee=coefficient_trainee) vehicule = Vehicule(nom=nom_vehicule, position_dep=position_dep, roues=roues, moteur=moteur, chassis=chassis) result &= vehicule.celebrer() == None result &= vehicule.position == position_dep result &= compare_arrays(vehicule.vitesse, [0, 0, 0]) print(result) result &= vehicule.trainee == 0.0 result &= vehicule.friction == 0.0 dt = 1 vehicule.accelerer(temps_ecoule=dt) result &= compare_arrays(vehicule.vitesse, [0, 0, 2.5]) result &= compare_arrays(vehicule.position, [45, -2, 59.5]) result &= vehicule.trainee == 5.4 result &= vehicule.friction == 3.0 except: return False result &= vehicule.nom == nom_vehicule result &= vehicule.poids == 50 result &= vehicule.traction == 125.0 result &= vehicule.roues == roues result &= vehicule.moteur == moteur result &= vehicule.chassis == chassis result &= vehicule.acceleration == 2.332 expected_dict_keys = [ '_Vehicule__nom', '_Vehicule__vitesse', '_Vehicule__position', '_Vehicule__roues', '_Vehicule__moteur', '_Vehicule__chassis' ] result &= list(vehicule.__dict__.keys()) == expected_dict_keys return result
class ControlHandler: servo_min = 1000 # Min pulse length out of 4096 servo_max = 4095 # Max pulse length out of 4096 servo_scale = servo_max - servo_min def __init__(self): self.chassis = Chassis() def setDirection(self): x = math.cos(math.radians(self.angle)) # print int(self.totalFreq), int(self.totalFreq - self.totalFreq * math.fabs(x)) if x > 0: self.chassis.setLeftSpeed(int(self.totalFreq)) self.chassis.setRightSpeed(int(self.totalFreq - self.totalFreq * x)) elif x < 0: self.chassis.setRightSpeed(int(self.totalFreq)) self.chassis.setLeftSpeed( int(self.totalFreq - self.totalFreq * math.fabs(x))) def move(self, msg): self.angle = msg.angle self.strength = msg.strength if self.strength == 0: self.chassis.stop() else: factor = self.strength / 100 self.totalFreq = (self.servo_scale * factor) + self.servo_min if 350 <= self.angle <= 360 or 0 <= self.angle <= 10: self.chassis.turnRight() self.chassis.setTotalSpeed(int(self.totalFreq)) elif 11 <= self.angle <= 169: self.chassis.forward() self.setDirection() elif 170 <= self.angle <= 190: self.chassis.turnLeft() self.chassis.setTotalSpeed(int(self.totalFreq)) elif 191 <= self.angle <= 349: self.chassis.reverse() self.setDirection()
def __init__(self): self.requires(Chassis.getInstance())
def __init__(self): self.chassis = Chassis()