Last active
November 19, 2020 01:06
-
-
Save Fabien-B/cfc7f713ddd26c7025a1ce327a5a705f to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/python3 | |
| from ivy.std_api import * | |
| import time | |
| from math import cos, sin, atan2, sqrt | |
| from enum import Enum | |
| BUS = "127.255.255.255:2010" | |
| #SPEED_REG = "SpeedCmd (.+) (.+) (.+)" | |
| SPEED_REG = "Direction (.+),(.+),(.+)" | |
| POS_REG = "Go to linear (.+),(.+)" | |
| POS_ORIENT_REG = "Go to orient (.+),(.+),(.+)" | |
| #POS_REG = "PosCmd (.+) (.+) (.+) (.+)" | |
| POS_REPORT = "Update robot pose {};{};{}" | |
| class Navigation: | |
| class NavMode(Enum): | |
| SPEED = 0 | |
| POSITION = 1 | |
| class PosControlState(Enum): | |
| INITIAL_TURN = 0 | |
| CRUISE = 1 | |
| FINAL_TURN = 2 | |
| def __init__(self, pos_init): | |
| self.pos = pos_init | |
| self.pos_obj = (0, 0) | |
| self.speed = (0, 0, 0) | |
| self.update_time = time.time() | |
| self.mode = Navigation.NavMode.SPEED | |
| self.pos_control_state = Navigation.PosControlState.INITIAL_TURN | |
| self.last_distance_to_obj = 0 | |
| def set_speed(self, speed): | |
| self.speed = speed | |
| self.mode = Navigation.NavMode.SPEED | |
| def set_pos_objective(self, pos): | |
| self.pos_obj = pos | |
| self.mode = Navigation.NavMode.POSITION | |
| self.pos_control_state = Navigation.PosControlState.INITIAL_TURN | |
| def update_pos_control(self): | |
| x, y, theta = self.pos | |
| x_obj, y_obj, theta_obj = self.pos_obj | |
| route = atan2(y_obj - y, x_obj - x) | |
| if self.pos_control_state == Navigation.PosControlState.FINAL_TURN and theta_obj is not None: | |
| theta_diff = theta_obj - theta | |
| else: | |
| theta_diff = route - theta | |
| theta_diff = (theta_diff + 180) % 360 - 180 | |
| distance = sqrt((x_obj - x)**2 + (y_obj - y)**2) | |
| vtheta = 0 | |
| if theta_diff > 0.1: | |
| vtheta = 0.8 | |
| elif theta_diff < -0.1: | |
| vtheta = -0.8 | |
| if(self.pos_control_state == Navigation.PosControlState.INITIAL_TURN): | |
| self.speed = (0, 0, vtheta) | |
| if abs(theta_diff) < 0.1: | |
| self.pos_control_state = Navigation.PosControlState.CRUISE | |
| self.last_distance_to_obj = distance | |
| if(self.pos_control_state == Navigation.PosControlState.CRUISE): | |
| if(distance > self.last_distance_to_obj): | |
| self.speed = (0, 0, 0) | |
| self.pos_control_state = Navigation.PosControlState.FINAL_TURN | |
| else: | |
| self.speed = (300, 0, vtheta) | |
| self.last_distance_to_obj = distance | |
| if self.pos_control_state == Navigation.PosControlState.FINAL_TURN: | |
| if theta_obj is None or abs(theta_diff) < 0.1: | |
| self.speed = (0, 0, 0) | |
| self.pos_control_state = Navigation.PosControlState.INITIAL_TURN | |
| self.mode = Navigation.NavMode.SPEED | |
| else: | |
| self.speed = (0, 0, vtheta) | |
| def update_speed_control(self, dt): | |
| # destructuring position ans speed tuples | |
| x, y, theta = self.pos | |
| vxr, vyr, vtheta = self.speed | |
| # take the average angle of the robot during the last period | |
| theta_avr = theta + vtheta*dt/2 | |
| # convert speed from robot reference system to table reference system | |
| vx0 = cos(theta_avr) * vxr - sin(theta_avr) * vyr | |
| vy0 = sin(theta_avr) * vxr + cos(theta_avr) * vyr | |
| # update position | |
| x += vx0*dt | |
| y += vy0*dt | |
| theta += vtheta*dt | |
| self.pos = (x, y, theta) | |
| def update(self, dt): | |
| if self.mode == Navigation.NavMode.POSITION: | |
| self.update_pos_control() | |
| self.update_speed_control(dt) | |
| class Robot: | |
| NAV_PERIOD = 0.05 | |
| ODOM_REPORT_PERIOD = 0.1 | |
| def __init__(self, bus=BUS, pos_init=(1500, 1000, 0)): | |
| IvyInit("Robot", "TestIvy OK!") | |
| IvyStart(bus) | |
| IvyBindMsg(self.on_speed_cmd, SPEED_REG) | |
| IvyBindMsg(self.on_pos_cmd, POS_REG) | |
| IvyBindMsg(self.on_pos_orient_cmd, POS_ORIENT_REG) | |
| #IvyBindMsg(self.on_any, "(.*)") | |
| self.nav = Navigation(pos_init) | |
| self.update_time_nav = time.time() | |
| self.update_time_odom_report = time.time() | |
| def __enter__(self): | |
| return self | |
| def __exit__(self, type, value, traceback): | |
| IvyStop() | |
| def on_speed_cmd(self, sender, vx, vy, vtheta): | |
| print("sender", sender) | |
| speed = (float(vx) * 300, float(vy) * 300, float(vtheta) * 1.0) | |
| self.nav.set_speed(speed) | |
| def on_pos_cmd(self, sender, x, y): | |
| pos = (float(x), float(y), None) | |
| self.nav.set_pos_objective(pos) | |
| def on_pos_orient_cmd(self, sender, x, y, theta): | |
| pos = (float(x), float(y), float(theta)) | |
| self.nav.set_pos_objective(pos) | |
| def on_any(self, sender, *args): | |
| print(sender, args) | |
| def update(self): | |
| now = time.time() | |
| dt_nav = now - self.update_time_nav | |
| if(dt_nav >= Robot.NAV_PERIOD): | |
| self.nav.update(dt_nav) | |
| self.update_time_nav = now | |
| dt_odom_report = now - self.update_time_odom_report | |
| if(dt_odom_report >= Robot.ODOM_REPORT_PERIOD): | |
| x, y, theta = self.nav.pos | |
| IvySendMsg(POS_REPORT.format(x, y, theta)) | |
| self.update_time_odom_report = now | |
| def run(self): | |
| while True: | |
| self.update() | |
| time.sleep(0.01) | |
| if __name__ == '__main__': | |
| with Robot() as robot: | |
| robot.run() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment