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()
