Autonomous UV Robot with SLAM
2247 9
Robin Kanattu Thomas

Autonomous UV Robot with SLAM

The affordable autonomous and open-source robot provides localization and mapping facilities and...

Description:-

The affordable autonomous and open-source robot provides localization and mapping facilities and safely navigates the robot through the environment. We are looking for open innovation so that designers, Engineers, and hardware enthusiasts can come together to take this project to a new level and use this platform to build something different for humanity.

Project Used Hardware

UV-Robot-Hardware

Raspberry Pi 3 Model B , Arduino UNO , Ydlidar X4 , UV lights , Din 45 Ah battery , Inverter 650 watts , Seeed Wio Terminal,ULTRAVIOLET UV DETECTION SENSOR , 4-CHANNEL RELAY CONTROLLER FOR I2C 12V to 5V buck converter IB2 BTS7960 Monstor motor driver DHT11 Temperature & Humidity Sensor (4 pins), Arduino Mega 2560 Arduino Mega 2560 TFT Touchscreen,

Project Used Software

Arduino IDE, ROS, autodesk fusion 360, Mit app inventor

Project Hardware Software Selection

DESCRIPTION As we face a worldwide healthcare crisis caused by COVID-19, there's a large need for disinfection. This tutorial can be followed by anyone who is new to ROS all the instructions are mentioned so that you too can build an autonomous UV bot. UV-C in many parts of the globe for fighting the COVID-19. Currently, such units are either stationary or moved by humans. We improved upon that concept and built an autonomous, mobile unit that may move around lobbies and hallways to get into rooms for disinfection. Our aim is to provide a simple tutorial to help people to make an affordable autonomous solution. DETAILS Three Lamps mounted on the robot would emit intense UV light at 240 nm wavelength, as per current medical device standards. Our key innovation is in making this mobile with a state-of-the-art LIDAR-based SLAM technology that allows operation without exposing the workers to either the harmful UV light or infected rooms of patients. The mobility allows the disinfection of various corners and narrow alleys. We will be using a mobile app to communicate with the robot and the user can see the real-time status of the robot. The bottom side can be used for other autonomous applications for surveillance, for delivery, etc.

Circuit Diagram

UV-Robot Circuit Diagram

Wio terminal We are using the Wio terminal which has many inbuilt sensors. The Wio Terminal is a SAMD51-based microcontroller with Wireless Connectivity supported by Realtek RTL8720DN that’s compatible with Arduino and MicroPython. It runs at 120MHz (Boost up to 200MHz), 4MB External Flash, and 192KB RAM. It supports both Bluetooth and Wi-Fi providing the backbone for IoT projects. The Wio Terminal itself is equipped with a 2.4” LCD Screen, onboard IMU(LIS3DHTR), Microphone, Buzzer, microSD card slot, Light sensor, and Infrared Emitter(IR 940nm). Really cool features in one single device right. In our case, it acts as a wifi access point and we can connect the raspberry pi and our Virtual machine to a single access point. The inbuilt display will show the time and other messages. We are using the IMU sensor, inbuilt temperature sensor, and buzzer in the WIO terminal to detect a collision, overheating and it alerts through buzzer as well as our mobile app. We are adding the UV light sensor to detect and get the feedback if UV light is ON or OFF. The WIO terminal is also connected to the relay to control the UV lights. The WIO terminal acts as an access point as well as a server. So that we can control the relay from UV SENSOR It uses a UV photodiode, which can detect the 240-370nm range of light. The signal from the photodiode is a very small level, in the nano-ampere level, hence an opamp to amplify the signal to a more Manageable voltage-level. DHT sensor There is a thermistor. There is also a very basic chip inside that does some analog to digital conversion and shows a digital signal with the temperature and humidity. The digital signal is fairly easy to read using wio terminal. Here we are using it as a safety feature. The internal temperature of the robot is being measured here. If it is getting really high the complete system will shut down. UV light We are using 20w unit 2ft Sanyo Japan UVC lamp with a full assembly which provides an area coverage of 70 sqft which is sufficient for our application. You can refer to these articles to find out more regarding the disinfection properties of UVC light. PIR sensor PIR sensors allow you to sense motion, almost always used to detect whether a human has moved in or out of the sensors range. They are small, inexpensive, low-power, easy to use, and don't wear out But it is a little inaccurate in our case we have to use Contact-less Infrared Thermopile Sensor with OpenCV to get more accurate data for human detection. We are working on the Thermopile sensor we will update in this tutorial really fast. MOTORS The initial plan was to use two NEMA 34 motor with 85KGCM Torque and use two DM860A2 motor drivers. But one of them was not working properly and so we changed our motors and drivers.So in this project, we will be using four wiper motors and two BTS7960 IBT2 motor divers instead. These motors can carry a sufficient load of our robot and control and coding is simple too. Charging circuit The robot identifies the charging port using path planning and comes and charges once every 5 hours. There are two rods on the robot and there are electrodes in the charging port which aligns properly. There is a magnetic reed switch which gets closed when the robot arrives.

Code

Acclerometer Temperature Sensor Test

#include"LIS3DHTR.h"
LIS3DHTR<TwoWire> lis;

void setup() {
Serial1.begin(115200);
lis.begin(Wire1);
if (!lis) {
    Serial.println("ERROR");
    while(1);
  }
  lis.setOutputDataRate(LIS3DHTR_DATARATE_25HZ); //Data output rate
  lis.setFullScaleRange(LIS3DHTR_RANGE_2G); //Scale range set to 2g
}

void loop() {
 float x_values, y_values, z_values;
  x_values = lis.getAccelerationX();
  y_values = lis.getAccelerationY();
  z_values = lis.getAccelerationZ();
  Serial.print("X: "); Serial.print(x_values);
  Serial.print(" Y: "); Serial.print(y_values);
 Serial.print(" Z: "); Serial.print(z_values);
 Serial.print(" Sytem temp:"); Serial.println(lis.getTemperature());
}

Charging Circuit

#include <UTFT.h>

// Declare which fonts we will be using
extern uint8_t BigFont[];
UTFT myGLCD(ILI9481,38,39,40,41,42);// ElecFreaks TFT2.2SP Shield

// constants won't change. They're used here to set pin numbers:
const int Reedswitch = 11;     // the number of the pushbutton pin
const int ledPin =  10;
const int relay = 9; 
const int statusled = 12; 

// variables will change:
int buttonState = 0;         // variable for reading the pushbutton status

void setup() {
   randomSeed(analogRead(0)); 
// Setup the LCD
  myGLCD.InitLCD();
  myGLCD.setFont(BigFont);
  digitalWrite(statusled, HIGH);
  // initialize the LED pin as an output: 
  pinMode(ledPin, OUTPUT);
  pinMode(relay, OUTPUT);
  // initialize the pushbutton pin as an input:
  pinMode(Reedswitch, INPUT);
}

void loop() {
// Clear the screen and draw the frame
  //myGLCD.clrScr();

  // read the state of the pushbutton value:
  buttonState = digitalRead(Reedswitch);

  // check if the pushbutton is pressed. If it is, the buttonState is HIGH:
  if (buttonState == HIGH) {
    // turn LED on:
    digitalWrite(ledPin, HIGH);
    digitalWrite(relay, HIGH);
   myGLCD.clrScr();
  myGLCD.setColor(255, 255, 255);
  myGLCD.setBackColor(255, 0, 0);
  myGLCD.print("UVBOT is Charging!", CENTER, 93);
  } else {
    digitalWrite(ledPin, LOW);
    digitalWrite(relay, LOW);
     myGLCD.setColor(255, 255, 255);
  myGLCD.setBackColor(0, 0, 255);
  myGLCD.print("Charging circuit is ON!", CENTER, 93);
  delay(20);
  }
}

Pir test Wio Terminal

int led = D6;                // the pin that the LED is atteched to
int sensor = D4;              // the pin that the sensor is atteched to
int state = LOW;             // by default, no motion detected
int val = 0;                 // variable to store the sensor status (value)

void setup() {
  pinMode(led, OUTPUT);      // initalize LED as an output
  pinMode(sensor, INPUT);    // initialize sensor as an input
  Serial.begin(115200);        // initialize serial
}

void loop(){
  val = digitalRead(sensor);   // read sensor value
  if (val == HIGH) {           // check if the sensor is HIGH
    digitalWrite(led, HIGH);   // turn LED ON
    delay(100);                // delay 100 milliseconds  
    if (state == LOW) {
      Serial.println("Motion detected!"); 
      state = HIGH;       // update variable state to HIGH
    }
  } 
  else {
      digitalWrite(led, LOW); // turn LED OFF
      delay(200);             // delay 200 milliseconds  
      if (state == HIGH){
        Serial.println("Motion stopped!");
        state = LOW;       // update variable state to LOW
    }
  }
}

Ros Serial Motor Control 

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif

#include <ros.h>
#include <geometry_msgs/Twist.h>
// Pin variables for motors.
const int right_back = 5;
const int right_front = 6;
const int left_back = 9;
const int left_front = 10;

ros::NodeHandle  nh;

void MoveFwd() {
  digitalWrite(right_back, HIGH);
  digitalWrite(left_back, HIGH);
  analogWrite(right_front,LOW);
  analogWrite(left_front, LOW);
}

void MoveStop() {
  digitalWrite(right_back, LOW);
  digitalWrite(left_back, LOW);
  analogWrite(right_front,LOW);
  analogWrite(left_front, LOW);
}
void MoveLeft() {
  digitalWrite(right_back, HIGH);
  digitalWrite(left_back, LOW);
  analogWrite(right_front,LOW);
  analogWrite(left_front, HIGH);
}
void MoveRight() {
  digitalWrite(right_back, LOW);
  digitalWrite(left_back, HIGH);
  analogWrite(right_front,HIGH);
  analogWrite(left_front, LOW);
}
void MoveBack() {
  digitalWrite(right_back, LOW);
  digitalWrite(left_back, LOW);
  analogWrite(right_front,HIGH);
  analogWrite(left_front, HIGH);
}

void cmd_vel_cb(const geometry_msgs::Twist & msg) {
  // Read the message. Act accordingly.
  // We only care about the linear x, and the rotational z.
  const float x = msg.linear.x;
  const float z_rotation = msg.angular.z;

  // Decide on the morot state we need, according to command.
  if (x > 0 && z_rotation == 0) {
    MoveFwd();
  }
  else if (x == 0 && z_rotation == 1) {
    MoveRight();
  }
else if (x == 0 && z_rotation < 0) {
    MoveLeft();
  }
else if (x < 0 && z_rotation == 0) {
    MoveBack();
  }
else{
    MoveStop();
  }
}
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", cmd_vel_cb);
void setup() {
  pinMode(right_back, OUTPUT);    
  pinMode(left_back, OUTPUT);
  pinMode(right_front, OUTPUT);
  pinMode(left_front, OUTPUT); 
  nh.initNode();
  nh.subscribe(sub);
}

void loop() {
  nh.spinOnce();
  delay(1);
}

Sensor UV Test

//input this code to test UV sensor
void setup() {
    Serial.begin(115200);
    pinMode(A8, INPUT);
}
void loop() {
    int uvsensor = analogRead(A8);
    Serial.print("UV intensity: ");
    Serial.println(uvsensor);
    delay(50);
}

Serial Test

void setup() {
RTL8720D.begin(115200);
Serial.begin(115200);

}

void loop() {
 if (RTL8720D.available())
            {

                int inbyte = RTL8720D.read();

                Serial.write(inbyte)               

}
}

Wifi Realtek Development Board 

#include <WiFi.h>
// Current time
unsigned long currentTime = millis();
// Previous time
unsigned long previousTime = 0; 
// Define timeout time in milliseconds (example: 2000ms = 2s)
const long timeoutTime = 2000;
String header;

// Auxiliar variables to store the current output state
String output5State = "off";
String output4State = "off";
const int output5 =5;
const int output4 = 4;
char ssid[] = "UV bot";  //Set the AP's SSID
char pass[] = "12345678";     //Set the AP's password
char channel[] = "1";         //Set the AP's channel
int status = WL_IDLE_STATUS;  // the Wifi radio's status
WiFiServer server(80);
void printWifiStatus() {
    // print the SSID of the network you're attached to:
   // Serial.println();
   // Serial.print("SSID: ");
   // Serial.println(WiFi.SSID());

    // print your WiFi shield's IP address:
    IPAddress ip = WiFi.localIP();
    //Serial.print("IP Address: ");
   // Serial.println(ip);
}
void setup() {
    //Initialize serial and wait for port to open:
    Serial.begin(115200);
    Serial1.begin(115200);
    pinMode(output5,OUTPUT);
    pinMode(output4,OUTPUT);
    while (!Serial) {
        ; // wait for serial port to connect. Needed for native USB port only
    }

    // check for the presence of the shield:
    if (WiFi.status() == WL_NO_SHIELD) {
        //Serial.println("WiFi shield not present");
        while (true);
    }
    String fv = WiFi.firmwareVersion();
    if (fv != "1.0.0") {
       // Serial.println("Please upgrade the firmware");
    }

    // attempt to start AP:
    while (status != WL_CONNECTED) {
        //Serial.print("Attempting to start AP with SSID: ");
        //Serial.println(ssid);
        status = WiFi.apbegin(ssid, pass, channel);
        delay(10000);
    }

    //AP MODE already started:
   // Serial.println("AP mode already started");
    //Serial.println();
   server.begin();
   printWifiStatus();
}

void loop(){
  WiFiClient client = server.available();   // Listen for incoming clients

if (client) { // If a new client connects,
 Serial.println("New Client."); // print a message out in the serial port
 String currentLine = ""; // make a String to hold incoming data from the client
 while (client.connected()) { // loop while the client's connected
 if (client.available()) { // if there's bytes to read from the client,
  char c = client.read(); // read a byte, then
  Serial.write(c); // print it out the serial monitor
  header += c;
  if (c == '\n') { // if the byte is a newline character
   // if the current line is blank, you got two newline characters in a row.
   // that's the end of the client HTTP request, so send a response:
   if (currentLine.length() == 0) {
    // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK)
    // and a content-type so the client knows what's coming, then a blank line:
    client.println("HTTP/1.1 200 OK");
    client.println("Content-type:text/html");
    client.println("Connection: close");
    client.println();
    // turns the GPIOs on and off
if (header.indexOf("GET /5/on") >= 0) {
  //Serial.println("GPIO 5 on");
  output5State = "on";
  digitalWrite(output5, HIGH);
  //Serial.print("5");
} else if (header.indexOf("GET /5/off") >= 0) {
 // Serial.println("GPIO 5 off");
  output5State = "off";
  Serial.print("7");
  digitalWrite(output5, LOW);
} else if (header.indexOf("GET /4/on") >= 0) {
  //Serial.println("GPIO 4 on");
  output4State = "on";
  digitalWrite(output4, HIGH);
} else if (header.indexOf("GET /4/off") >= 0) {
  //Serial.println("GPIO 4 off");
  output4State = "off";
  digitalWrite(output4, LOW);
}

 client.println("<!DOCTYPE html><html>");
            client.println("<head><meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">");
            client.println("<link rel=\"icon\" href=\"data:,\">");
            // CSS to style the on/off buttons 
            // Feel free to change the background-color and font-size attributes to fit your preferences
            client.println("<style>html { font-family: Helvetica; display: inline-block; margin: 0px auto; text-align: center;}");
            client.println(".button { background-color: #195B6A; border: none; color: white; padding: 16px 40px;");
            client.println("text-decoration: none; font-size: 30px; margin: 2px; cursor: pointer;}");
            client.println(".button2 {background-color: #77878A;}</style></head>");
client.println("<h1>UV BoT</h1>");
 client.println("<p>UV Light status " + output5State + "</p>");
            // If the output5State is off, it displays the ON button       
            if (output5State=="off") {
              client.println("<p><a href=\"/5/on\"><button class=\"button\">ON</button></a></p>");
            } 
            else {
              client.println("<p><a href=\"/5/off\"><button class=\"button button2\">OFF</button></a></p>");
            }
             client.println();
             break;
   }
     else { // if you got a newline, then clear currentLine
            currentLine = "";
          }
        } else if (c != '\r') {  // if you got anything else but a carriage return character,
          currentLine += c;      // add it to the end of the currentLine
        }
      }
    }
    // Clear the header variable
    header = "";
    // Close the connection
    client.stop();
   // Serial.println("Client disconnected.");
   // Serial.println("");
  }
}

Wio Terminal Code

#include <SPI.h>

#define TFT_GREY 0x5AEB
#include"LIS3DHTR.h"
LIS3DHTR<TwoWire> lis;
#include"TFT_eSPI.h"
#include"Free_Fonts.h" //include the header file
TFT_eSPI tft;
uint32_t targetTime = 0;                    // for next 1 second timeout

static uint8_t conv2d(const char* p); // Forward declaration needed for IDE 1.6.x

uint8_t hh = conv2d(__TIME__), mm = conv2d(__TIME__ + 3), ss = conv2d(__TIME__ + 6); // Get H, M, S from compile time

byte omm = 99, oss = 99;
byte xcolon = 0, xsecs = 0;
unsigned int colour = 0;

void setup(void) {
  pinMode(WIO_BUZZER, OUTPUT);
    pinMode(A8, INPUT);
    pinMode(D4,OUTPUT);
  tft.begin();
  tft.setRotation(3);
  tft.fillScreen(TFT_BLACK);
   tft.setTextSize(1);
    tft.setTextColor(TFT_YELLOW, TFT_BLACK);

    targetTime = millis() + 1000;
  Serial1.begin(115200);
  RTL8720D.begin(115200);
  lis.begin(Wire1); 
  if (!lis) {
    Serial.println("ERROR");
    while(1);
  }
  lis.setOutputDataRate(LIS3DHTR_DATARATE_25HZ); //Data output rate
  lis.setFullScaleRange(LIS3DHTR_RANGE_2G); //Scale range set to 2g
}
void loop() {
  float x_values, y_values, z_values;
  x_values = lis.getAccelerationX();
  y_values = lis.getAccelerationY();
  z_values = lis.getAccelerationZ();
   int uvsensor = analogRead(A8)
 if (RTL8720D.available())
            {

                int inbyte = RTL8720D.read();

                if (inbyte>4) 
                digitalWrite(D4,HIGH);
                Serial.print("inbyte:"); Serial.write(inbyte);
                tft.drawString("   Caution! UV lamp is On      ",30,180 );
                }
                else if(inbyte<4)
                {
                  digitalWrite(D4,LOW);
              Serial.print("inbyte:");Serial.print(inbyte);
              tft.drawString("         UV lamp is Off                        ",30,180 ); 
              }
                else {
                }
                }
 // Serial.print("X: "); Serial.print(x_values);
 // Serial.print(" Y: "); Serial.print(y_values);
 // Serial.print(" Z: "); Serial.print(z_values);
  //Serial.println();
  if (x_values<0.5)
  {
     Serial.print("The Bot is down...stopping systems! ");
     Serial.print("temp:"); Serial.println(lis.getTemperature());
     RTL8720D.write("1");
     analogWrite(WIO_BUZZER, 128);
      tft.setFreeFont(FS12);
     tft.drawString("The Bot is down",70,140 );
     delay(50);
     Serial.print("uv value"); Serial.println(uvsensor);
     if (uvsensor> 400){
    Serial.print("UV Lamp is working: "); 
    tft.drawString("   Caution! UV lamp is On      ",30,180 );
   }
   else{
    tft.drawString("         UV lamp is Off                        ",30,180 ); 
   }   
  }
  else
  {Serial.print("temp:"); Serial.println(lis.getTemperature());
   tft.drawString("     UV Robot      ",70,140 ); 
   RTL8720D.write("0");
   analogWrite(WIO_BUZZER, 0);
   Serial.print("uv value"); Serial.println(uvsensor);
   if (uvsensor> 400){
    Serial.print("UV Lamp is working: ");
    tft.drawString("   Caution! UV lamp is On     ",30,180 );
   }
   else{
    tft.drawString("         UV lamp is Off                        ",30,180 ); 
   }
  }
  delay(50);
   if (targetTime < millis()) {
        // Set next update for 1 second later
        targetTime = millis() + 1000; 

        // Adjust the time values by adding 1 second
        ss++;              // Advance second
        if (ss == 60) {    // Check for roll-over
            ss = 0;          // Reset seconds to zero
            omm = mm;        // Save last minute time for display update
            mm++;            // Advance minute
            if (mm > 59) {   // Check for roll-over
                mm = 0;
                hh++;          // Advance hour
                if (hh > 23) { // Check for 24hr roll-over (could roll-over on 13)
                    hh = 0;      // 0 for 24 hour clock, set to 1 for 12 hour clock
                }
            }
        }
        // Update digital time
        int xpos = 0;
        int ypos = 20; // Top left corner ot clock text, about half way down
        int ysecs = ypos + 24;

        if (omm != mm) { // Redraw hours and minutes time every minute
            omm = mm;
            // Draw hours and minutes
            if (hh < 10) {
                xpos += tft.drawChar('0', xpos, ypos, 8);    // Add hours leading zero for 24 hr clock
            }
            xpos += tft.drawNumber(hh, xpos, ypos, 8);             // Draw hours
            xcolon = xpos; // Save colon coord for later to flash on/off later
            xpos += tft.drawChar(':', xpos, ypos - 8, 8);
            if (mm < 10) {
                xpos += tft.drawChar('0', xpos, ypos, 8);    // Add minutes leading zero
            }
            xpos += tft.drawNumber(mm, xpos, ypos, 8);             // Draw minutes
            xsecs = xpos; // Sae seconds 'x' position for later display updates
        }
        if (oss != ss) { // Redraw seconds time every second
            oss = ss;
            xpos = xsecs;

            if (ss % 2) { // Flash the colons on/off
                tft.setTextColor(0x39C4, TFT_BLACK);        // Set colour to grey to dim colon
                tft.drawChar(':', xcolon, ypos - 8, 8);     // Hour:minute colon
                xpos += tft.drawChar(':', xsecs, ysecs, 6); // Seconds colon
                tft.setTextColor(TFT_YELLOW, TFT_BLACK);    // Set colour back to yellow
            } else {
                tft.drawChar(':', xcolon, ypos - 8, 8);     // Hour:minute colon
                xpos += tft.drawChar(':', xsecs, ysecs, 6); // Seconds colon
            }

            //Draw seconds
            if (ss < 10) {
                xpos += tft.drawChar('0', xpos, ysecs, 6);    // Add leading zero
            }
            tft.drawNumber(ss, xpos, ysecs, 6);                     // Draw seconds
        }
    }
}
static uint8_t conv2d(const char* p) {
    uint8_t v = 0;
    if ('0' <= *p && *p <= '9') {
        v = *p - '0';
    }
    return 10 * v + *++p - '0';
}

ros Autonomous Setup:

import numpy as np
import math
import matplotlib.pyplot as plt
import rospy
from rospy.numpy_msg import numpy_msg
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from controller import PID
class WallFollower:
# Import ROS parameters from the "params.yaml" file.
# Access these variables in class functions with self:
# i.e. self.CONSTANT
SCAN_TOPIC = "/scan"
DRIVE_TOPIC = "cmd_vel"
SIDE = -1 # -1 right is and +1 is left
VELOCITY = 1.6
DESIRED_DISTANCE = 0.5
def __init__(self):
# Create a node that
#   Subscribes to the laser scan topic,
#   Publishes to  drive topic - to move the vehicle.
# Initialize subscriber to laser scan.
rospy.Subscriber(self.SCAN_TOPIC, LaserScan, self.LaserCb)
# Initialize a publisher of drive commands.
self.drive_pub = rospy.Publisher(self.DRIVE_TOPIC, Twist, queue_size = 10)
# Variables to keep track of drive commands being sent to robot.
self.seq_id = 0
# Class variables for following.
self.side_angle_window_fwd_ = math.pi*0.1
self.side_angle_window_bwd_ = math.pi - math.pi*0.3
self.point_buffer_x_ = np.array([])
self.point_buffer_y_ = np.array([])
self.num_readings_in_buffer_ = 0
self.num_readings_for_fit_ = 2
self.reject_dist = 0.7
self.steer_cmd = 0
self.vel_cmd = self.VELOCITY
self.pid = PID()
def GetLocalSideWallCoords(self, ranges, angle_min, angle_max, angle_step):
# Slice out the interesting samples from our scan. pi/2 radians from pi/4 to (pi - pi/4) radians for the right side.
positive_start_angle = self.side_angle_window_fwd_
positive_end_angle   = self.side_angle_window_bwd_
if self.SIDE == -1: #"right":
start_angle = -positive_start_angle
end_angle   = -positive_end_angle
elif self.SIDE == 1: #"left":
start_angle = positive_start_angle
end_angle   = positive_end_angle
start_ix_ = int((-angle_min +start_angle)/angle_step)
end_ix_ = int((-angle_min +end_angle)/angle_step)
start_ix = min(start_ix_,end_ix_)
end_ix = max(start_ix_,end_ix_)
side_ranges = ranges[min(start_ix,end_ix):max(start_ix, end_ix)]
x_values = np.array([ranges[i]*math.cos(angle_min+i*angle_step) if i < len(ranges) else  ranges[(i - len(ranges))]*math.cos(angle_min+(i - len(ranges))*angle_step) for i in range(start_ix, end_ix) ])
y_values = np.array([ranges[i]*math.sin(angle_min+i*angle_step) if i < len(ranges) else ranges[i - len(ranges)]*math.sin(angle_min+(i - len(ranges))*angle_step) for i in range(start_ix, end_ix)])
# Check that the values for the points are within 1 meter from each other. Discard any point that is not within one meter form the one before it.
out_x = []
out_y = []
for ix in range(0, len(x_values)):
new_point = (x_values[ix],y_values[ix])
# This conditional handles points with infinite value.
if  side_ranges[ix] < 2.5 and side_ranges[ix] > 0 and abs(new_point[1]) < 7000 and abs(new_point[0]) < 7000 :
out_x.append(new_point[0])
out_y.append(new_point[1])
return np.array(out_x), np.array(out_y)
def LaserCb(self, scan_data):
# This function is called every time we get a laser scan.
# This is the plan:
# * Get scan data.
# * Convert it to x,y coordinates in the local frame of the robot.
# * Find a least squares - best fit line through those points with numpy.
#   Consider using data from multiple scans in one least-squares fit cycle.
#   This is a line equation, with respect to the car at (0,0), with the x axis being the heading.
#   Get vector theta for the line, and theta_0 as the y intersection.
# * Find the distance from the line to the origin with ( theta_T dot [[0],[0]] + theta_0 ) / (norm theta)
# TLDR, We have a vector theta for the line we have found, and a distance to that wall.
# TODO(yorai): Handle erroneous scan values. If one is too big, or too small, use past value.
# Do not do this for too many in a row, maybe just throw scan away if too many corrections.
angle_step = scan_data.angle_increment
angle_min = scan_data.angle_min
angle_max = scan_data.angle_max
ranges = scan_data.ranges
# Get data for side ranges. Add to buffer.
wall_coords_local = self.GetLocalSideWallCoords(ranges, angle_min, angle_max, angle_step)
#######
#Find mean and throw out everything that is 1 meter away from mean distance, no outliers.
# If one differs by more than a meter from the previous one, gets thrown out from both x and y. Distnaces as we go along vector of points.
# but print the things first.
#######
self.point_buffer_x_ = np.append(self.point_buffer_x_, wall_coords_local[0])
self.point_buffer_y_ = np.append(self.point_buffer_y_, wall_coords_local[1])
self.num_readings_in_buffer_ +=1
# If we have enough data, then find line of best fit.
if self.num_readings_in_buffer_ >= self.num_readings_for_fit_:
# Find line of best fit.
# self.point_buffer_x_ = np.array([0, 1, 2, 3])
# self.point_buffer_y_ = np.array([-1, 0.2, 0.9, 2.1])
A = np.vstack([self.point_buffer_x_, np.ones(len(self.point_buffer_x_))]).T
m, c = np.linalg.lstsq(A, self.point_buffer_y_, rcond=0.001)[0]
# Find angle from heading to wall.
# Vector of wall. Call wall direction vector theta.
th = np.array([[m],[1]])
th /= np.linalg.norm(th)
# Scalar to define the (hyper) plane
th_0 = c
# Distance to wall is (th.T dot x_0 + th_0)/(norm(th))
dist_to_wall = abs(c/np.linalg.norm(th))
# Angle between heading and wall.
angle_to_wall = math.atan2(m, 1)
# Clear scan buffers.
self.point_buffer_x_=np.array([])
self.point_buffer_y_=np.array([])
self.num_readings_in_buffer_ = 0
# Simple Proportional controller.
# Feeding the current angle ERROR(with target 0), and the distance ERROR to wall. Desired error to be 0.
print("ANGLE", angle_to_wall, "DIST", dist_to_wall)
steer = self.pid.GetControl(0.0 - angle_to_wall, self.DESIRED_DISTANCE - dist_to_wall, self.SIDE)
# Publish control to /drive topic.
drive_msg = Twist()
# drive_msg.header.seq = self.seq_id
# self.seq_id += 1
# Populate the command itself.
drive_msg.linear.x = self.VELOCITY
drive_msg.angular.z = steer
# drive_msg.drive.steering_angle = steer
# drive_msg.drive.steering_angle_velocity = 0.1
# drive_msg.drive.speed = self.VELOCITY
# drive_msg.drive.acceleration = 1
# drive_msg.drive.acceleration = 0.5
self.drive_pub.publish(drive_msg)
if __name__ == "__main__":
rospy.init_node('wall_follower')
wall_follower = WallFollower()
rospy.spin()