diff --git a/.gitignore b/.gitignore index f8bcd11..95c8cf7 100755 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,10 @@ msg/*Goal.msg msg/*Result.msg msg/_*.py +*.AppleDouble* +___* +.vscode + # Generated by dynamic reconfigure *.cfgc /cfg/cpp/ diff --git a/README.md b/README.md index 91685e7..f9d2048 100755 --- a/README.md +++ b/README.md @@ -1,225 +1,438 @@ -# intro_to_ros -Repository of packages and info for the SV-ROS Intro To ROS training series +# neato -This repo will contain the software and documents for the SV-ROS 2015 Intro to Ros training series. +Turn your neato vacuum into a ROS robot. This project contains the code and instructions required to run ROS on a neato robotic vacuum. -As of January, 2018, many updates have occurred, so we will be updating the documentation. +This project is currently under development but is expected to work. Please reach out with an issue here: -The series involves a set of talks presenting a general introduction to ROS and building ROS Robots. +## Sources -Reference Robot designs are provided as a guide to getting started. +This project is derived from the repository located at . I leveraged the project previously and found it to be very useful. This project leverages much of the code included in that project and originally created by created by Michael Ferguson. It interacts with most of the Neato API and has some clever logic that allows it to interact with a serial port without compromising the main thread for ROS. -The first of these is a robot built on the Neato BV80 base and using a Raspberry PI III. The PI III has built in WiFi and Blue Tooth, but because of this the default names of ttyUSB0 needs to be changed to ttyACM0 in order to connect to the robot. +I learned of the above package from a [Servo Magazine article](https://www.servomagazine.com/magazine/article/neato-ros-robot-navigation). They include instructions to expand on the readme in the above repository which helped me. I included those instructions where applicable in the below instructions. -The ROS packages and drivers for this robot can be found in the bv80bot folder(s). +This makes use of the ubiquity robotics image. This saved an enormous amount of time and makes the process far easier. Their image is excellent. I don't have one if their physical products but I would imagine they are equally great. Thank you ubiquity robotics. If you are looking for a robot base you can check out their products at -The driver is based on a modified Neato Driver first created by Michael Ferguson. +Several of the tasks below were researched and found on various sites. I sourced those references. Thank you all for your contributions. +## Setup -## Background info -The following link provides the basic command set for the BV80 over the USB serial port. +This setup makes use of two computers; A raspberry pi and another computer such as a laptop. In my case I leveraged a Raspberry Pi 3 B+ (and a Pi 4 B on another) and a laptop running Ubuntu 18.04. The Raspberry Pi is the ROS master and interacts with the Neato Vac via USB. The second computer is used to control navigation of the robot through rviz. I suppose this secondary computer would not be necessary if you don't wish to create a map and have the robot navigate to locations. You could just drive it around with a controller or add additional ROS packages (or your own) to do other things. -Also type HELP in a terminal connected to the port to get the built in command help info. +### Raspberry Pi Setup -USB Serial API doc - https://tinyurl.com/Neato-Programmers-Manual +1. Acquire a Raspberry Pi. I used a Model 3 B+ or 4 B however I believe these instructions will apply to other models as well. +1. Install the Ubiquity Robotics image 2020-11-07-ubiquity-xenial-lxde from https://downloads.ubiquityrobotics.com/pi.html +1. Install on SD card using balenaEtcher following the instructions at the link above +1. Plug into usb power and boot +1. On another computer, join `ubiquityrobotXXXX` wi-fi, the password is `robotseverywhere` +1. ssh into the raspberry pi `ssh ubuntu@10.42.0.1` password: `ubuntu` +1. update password: `sudo passwd ubuntu` +1. update hostname: `sudo pifi set-hostname HOSTNAME` and replace HOSTNAME with whatever you would like your pi's hostname to be. +1. Add wi-fi connection: `sudo pifi add SSID PASSWORD`. Replace SSID and PASSWORD with your wi-fi's ssid and password. -Other useful info on the Lidar +1. Restart raspberry pi: `sudo shutdown -r now` -https://github.com/rohbotics/xv11hacking/tree/master/mainSpace +1. Update Ubuntu: `sudo apt-get update && sudo apt-get upgrade` --- -## Setingup the Robot/Rasperry PI -------- +1. Disable magni-base (the base of the ubiquityrobotics robot): `sudo systemctl disable magni-base` -### Initial SD Card Image +1. Install VNC Server (Optional) - The robot is built from a Raspberry PI III SBC and a Neato BV80 or later robot vacuum base. - - To start use a blank 16Gb or 32Gb SDcard and install the Ubuntu Image from Ubiquity robotics: - - Follow the instructions from Ubiquity Robotics for building a ROS image on the PI. - https://downloads.ubiquityrobotics.com This will allow you to place a ROS image on a 16Gb or 32Gb SD card which can - then directly boot the PI with ubuntu 16.04 and ROS Kinetic pre installed. Since this image is for another robot, - additional modifications to the PI's OS will be required to make a working robot as detailed below. - - ### Setup the Intro to ROS Packages - -After creating the SDCard insert it into the Rasperry PI and boot it - at this point its best if you have an HDMI monitor, mouse and keyboard attached or you can work via an SSH terminal session. - -Initial login using: + This will allow you to connect to the Pi via VNC if you want. - user - ubuntu - password- ubuntu - -First disable the pre loaded ubiquity robotics ROS stacks that are auto started with the following command: + source: -```sudo systemctl disable magni-base``` + source for copy paste support: -If you don't always want to login as the ubuntu user you may want to create a new user - just follow standard Ubuntu admin steps to do this. + sudo apt-get install nano xorg lxde-core tightvncserver autocutsel -Login as your prefered user. + vncserver -Use the normal Ubuntu networking steps to connect to a suitable WiFi network. + vncserver -kill :1 -Now update the installed image: + nano ~/.vnc/xstartup -``` - sudo apt-get update - sudo apt-get upgrade -``` + Change and update the file by adding the lines below (cmd+o, cmd+x). -Make sure ROS files are up to date: + autocutsel -fork + lxterminal & + /usr/bin/lxsession -s LXDE & - ``` - rosdep update - ``` + Full contents below. -Create a new ROS catkin workspace under your home directory, then git clone the intro_to_ros repo to the catkin_ws/src folder. - - Note: do this on both your PC/Laptop and the Raspberry PI., - - you need copies of the files on both computers. - - ``` - mkdir -p ~/catkin_ws/src - cd ~/catkin_ws/src - git clone https://github.com/SV-ROS/intro_to_ros.git - ``` - Optional, but highly recommended: - - ``` - git clone https://github.com/pirobot/rbx1 - git clone https://github.com/vanadiumlabs/arbotix_ros - ``` - - -Also install the depedancies using the following command: - - -``` -cd ~/catkin_ws - -rosdep install --from-paths src --ignore-src -r -y -``` - -If you receive an error: -```'Cannot locate rosdep definition for [yocs-velocity-smoother]'``` -you will need to install it manualy - -```sudo apt-get install ros--yocs-velocity-smoother``` - - - do a catkin_make on the workspace (on both computers) - - ``` - cd ~/catkin_ws - catkin_make - ``` - - Now you can update the .bashrc file in your home directory. - - open ~/.bashrc in vi or whatever text editor you prefer. - - Near the end find the line: - - ``` - source /opt/ros//setup.bash - ``` - - comment out this line and replace it as shown: - -``` - # source /opt/ros//setup.bash - source ~/catkin_ws/devel/setup.bash -``` - -Note: in the above change <distro> to kinetic or melodic depending on what platform you are using - - Save and close the editor - - now close and reopen a terminal or just source ~/.bashrc - - You should now be able to launch the ROS code as discussed below. - - - - - ## Running the Robot - - There are a number of different way to launch the robot depending on where you want to run the packages. - - The robot can be run in two modes - mapping and navigation, you must first run the robot in mapping mode to create a map to use later for navigation. - - - Packages for mapping an navigation can be run either on the robot or the PC/Laptop, the following gives the launch commands for each configuration: - - Mapping Mode: - - Launch the ros packages for mapping on the robot: - - on the Raspberry PI -- roslaunch bv80bot_node bv80bot_base_map.launch - on the PC/Laptop -- roslaunch bv80bot_node bv80bot_gui_only.launch - - - Launch the ros packages for mapping on the PC/Laptop: - - on the Raspberry PI -- roslaunch bv80bot_node bv80bot_base_only.launch - on the PC/Laptop -- roslaunch bv80bot_node bv80bot_map_gui.launch - -Drive around with the joystick/keyboard until you have a good enough map (see below for telop configuration). - -Once you have crated a map you like you must save it before you stop running the nodes launched above. -You can save the map to the PI or the Laptop/PC, you should save it to the computer you intend to run the nav nodes on later. - -So on the appropriate computer, (PC/Laptop or the PI) save the map as follows: -``` -roscd neato_2dnav/maps -rosrun map_server map_saver -``` -The map will be saved as two files in the .../neato_2dnav/maps folder, map.yaml, map.pgm - - - Navigation Mode: - - Launch the ros packages for navigation on the robot (if you saved your map to the PI): - - on the Raspberry PI -- roslaunch bv80bot_node bv80bot_base_nav.launch - on the PC/Laptop -- roslaunch bv80bot_node bv80bot_gui_only.launch - - - Launch the ros packages for navigation on the PC/Laptop (if you saved your map to the PC/Laptop): - - on the Raspberry PI -- roslaunch bv80bot_node bv80bot_base_only.launch - on the PC/Laptop -- roslaunch bv80bot_node bv80bot_nav_gui.launch - - You should now be able to set the robots pose in rviz and set nav goals for the robot to goto with the 2d nav goal button in rviz. - - - Teleop Configuration: - - The launch files mentioned can be configured to use 1 of 4 teleop controllers: - - ps3 joystick - - xbox360 joystick - - logitech joypad - - keyboard - - There is an argument at the top of the bv80bot_base.launch file as shown below: - - ~/catkin_ws/src/intro_to_ros/bv80bot/bv80bot_node/launch/include/bv80bot_base.launch - ``` - - - - ``` - - Change the value of the default argument shown above to one of the indicated choices to change your controler settings/configuration. - - - - - + #!/bin/sh + + xrdb $HOME/.Xresources + xsetroot -solid grey + autocutsel -fork + #x-terminal-emulator -geometry 80x24+10+10 -ls -title "$VNCDESKTOP Desktop" & + #x-window-manager & + # Fix to make GNOME work + export XKL_XMODMAP_DISABLE=1 + /etc/X11/Xsession + lxterminal & + /usr/bin/lxsession -s LXDE & + + save start the server from your ssh terminal + + vncserver + + Setup a service. I used these instructions as reference: + + sudo nano /etc/init.d/tightvncserver + + add this to the new file and save. Note the user in this case is `ubuntu` + + #!/bin/sh + # /etc/init.d/tightvncserver + # Set the VNCUSER variable to the name of the user to start tightvncserver under + VNCUSER='ubuntu' + case "$1" in + start) + su $VNCUSER -c '/usr/bin/tightvncserver :1 -geometry 1440x900' + echo "Starting TightVNC server for $VNCUSER" + ;; + stop) + pkill Xtightvnc + echo "Tightvncserver stopped" + ;; + *) + echo "Usage: /etc/init.d/tightvncserver {start|stop}" + exit 1 + ;; + esac + exit 0 + + Edit file permissions: + + sudo chmod 755 /etc/init.d/tightvncserver + sudo update-rc.d tightvncserver defaults + + For reference only, start, stop, and restart vnc service using the following commands: + + service tightvncserver start + service tightvncserver stop + service tightvncserver restart + + Restart the raspberry pi for it to take effect: + + sudo shutdown -r now + + It should now start up automatically at when the Raspberry Pi is started. + + Access the server from vnc client on another computer using IP_ADDRESS:1 or HOST_NAME:1 (replace IP_ADDRESS or HOST_NAME) with the raspberry pi's using the password you setup above. + +1. Make sure ROS files are up to date: `rosdep update` + +1. On both computers, download the neato files. + + Note: do this on both your PC/Laptop and the Raspberry PI., + + + + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws/src + git clone https://github.com/brannonvann/neato.git + +1. Install the dependencies using the following command: + + cd ~/catkin_ws + rosdep install --from-paths src --ignore-src -r -y + +1. Run catkin_make on the catkin workspace + + cd ~/catkin_ws + catkin_make + +1. On Raspberry Pi, update .bashrc to include the setup for neato. Make the following changes and save the file. (cmd+o, cmd+x) + + nano ~/.bashrc + + At the end of the file paste the below command aliases. This will allow you to use the command before the equal sign to execute the command after the equal sign. + + alias startnav='roslaunch neato base_nav.launch' + alias startmap='roslaunch neato base_map.launch' + alias startui='roslaunch neato map_gui.launch' + alias savemap='roscd neato_nav/maps && rosrun map_server map_saver' + alias stopneato='python ~/catkin_ws/src/neato/scripts/stop_neato.py' + alias offneato='python ~/catkin_ws/src/neato/scripts/power_off_neato.py' + +1. Reopen terminal or run `source ~/.bashrc` + +1. Check date on the computer to make sure it is correct. + + date + + To update it install ntppdate and update using ubuntu. + + sudo apt-get install ntpdate + sudo ntpdate ntp.ubuntu.com + +1. Plug in your Neato to the usb port. If you only have the Neato plugged in via USB this should be the correct port. If you have an older pi, I believe you may give yourself rights to access the neato robot via usb `sudo chmod 666 /dev/ttyUSB0` but I did not verify this. + + sudo chmod 666 /dev/ttyACM0 + + If the command fails then check if the usb is reading/writing to a different file. + + ls /dev/ttyACM* + + or for an older raspberry pi + + ls /dev/USB* + + If it is going to another file, like ttyACM1 when there was no ttyACM0 then I found unplugging the USB from the neato waiting a minute and plugging back in resolved the issue and it started using ttyACM0 again. + +1. Setup A file share. Choose one of the options below. + +I tried all three. So far each have their advantages and disadvantages. I prefer the Netatalk or SSH option but both have trouble with git from VS Code. Samba doesn't have a problem with git but the connection drops frequently and it changed my line endings. + +1. File Share Option: SSH via VS Code + +You can access your file system using SSH which of course is already setup. Using a text editor such as VS Code has that functionality and can be setup using these instructions: Furthermore, setup keys to simplify the access (and make it a bit more secure): + +1. File Share Option: Netatalk + + Source: + + Build and install netatalk. + + cd ~ + nano install_netatalk3.sh + chmod u+x install_netatalk3.sh + + Paste the contents below into the file: + + #!/bin/bash + + # Enable extended attributes on filesystem + # http://netatalk.sourceforge.net/wiki/index.php/Install_Netatalk_3.1.11_on_Ubuntu_16.04_Xenial#Setting_Up + + # Get system to updated state and install required packages + sudo apt update + sudo apt full-upgrade -y + sudo apt install -y clang make libdb-dev libgcrypt20-dev libavahi-client-dev libpam0g-dev + + # Get code + cd ~ + wget https://iweb.dl.sourceforge.net/project/netatalk/netatalk/3.1.11/netatalk-3.1.11.tar.bz2 + tar xf netatalk-3.1.11.tar.bz2 && rm netatalk-3.1.11.tar.bz2 + cd netatalk-3.1.11 + + # Build and install + ./configure --with-init-style=systemd --disable-static + make -j $(grep -c ^processor /proc/cpuinfo) + sudo make install-strip + + # Add to /usr/local/etc/afp.conf (uncomment) + # [Homes] + # basedir regex = /home + + # Enable and start + sudo systemctl enable netatalk + sudo systemctl start netatalk + + Execute the script: + + install_netatalk3.sh + + Setup to share the home folder. + + sudo nano /etc/netatalk/afp.conf + + Uncommented the `[Homes]` section and configure like: + + [Homes] + basedir regex = /home + + Save and exit nano(cmd+o, cmd+x) and restart netatalk + + sudo /etc/init.d/netatalk restart + +1. File Share Option: Samba + + When I first configured my pi, I used Netatalk but there were problems with accessing the git files so I switched to samba. Below is the setup for Samba. + + sudo apt-get install samba samba-common-bin' + + Edit the config file + + sudo nano /etc/samba/smb.conf + + find the section called Share Definitions uncomment/set the following settings. Comments removed for brevity. + + #======================= Share Definitions ======================= + [homes] + comment = Home Directories + browseable = yes + read only = no + create mask = 0775 + directory mask = 0775 + valid users = ubuntu + + For reference only, start, stop, and restart samba service using the following commands: + + sudo /etc/init.d/samba start + sudo /etc/init.d/samba stop + sudo /etc/init.d/samba restart + +1. Setup shutdown button + + Setup a shutdown button and script so they pi can be shutdown in the event of a lost connection without unplugging. Unplugging will likely result in a corrupt microSD and require complete repeat of above instructions. These instructions have the full script with the license. They also have alternate implementations that allow for restarting. I modified the script for the purposes of this raspberry pi. The modified script is below. + + Full script (unmodified) and description and license: + + cd ~ + nano shutdown.py + + Paste the below in the file and use the two pins closest to the ethernet port (GPIO 26 and Ground) to shutdown the pi with a jumper wire or add a button if you wish. + + # LICENSE: This code is released under the MIT License (http://opensource.org/licenses/MIT) + # + # Distributed as-is; no warranty is given + # + # ----------------------------------------------------------------------------- + + import time + import RPi.GPIO as GPIO + + # Pin definition + shutdown_pin = 26 # BV: Default was 17 + + # Suppress warnings + GPIO.setwarnings(False) + + # Use "GPIO" pin numbering + GPIO.setmode(GPIO.BCM) + + # Use built-in internal pullup resistor so the pin is not floating + # if using a momentary push button without a resistor. + GPIO.setup(shutdown_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP) + + # Use Qwiic pHAT's pullup resistor so that the pin is not floating + #GPIO.setup(shutdown_pin, GPIO.IN) + + # modular function to shutdown Pi + def shut_down(): + print "shutting down" + command = "/usr/bin/sudo /sbin/shutdown -h now" + import subprocess + process = subprocess.Popen(command.split(), stdout=subprocess.PIPE) + output = process.communicate()[0] + print output + + + # Check button if we want to shutdown the Pi safely + while True: + # For troubleshooting, uncomment this line to output button status on command line + # print GPIO.input(shutdown_pin) + if GPIO.input(shutdown_pin)== False: + shut_down() + + Make the script run at startup by adding a line to the rc.local file. Open the file for editing: + + sudo nano /etc/rc.local + + Add this above `exit 0` in the file and save and exit (cmd+o, cmd+x). + + python /home/pi/shutdown.py & + +1. If you have a logitech controller, plug the usb dongle into the raspberry pi. I use the Logitech F710. If you do not have a logitech controller, change the + +### Secondary computer setup + +Note: These instructions were added in retrospect and may be missing some steps. Reference sites in the [References Section](#references) if problems are encountered. + +1. Make sure ROS files are up to date: `rosdep update` + +1. On both computers, download the neato files. + + Note: do this on both your PC/Laptop and the Raspberry PI., + + Repo: + + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws/src + git clone https://github.com/brannonvann/neato.git + +1. Install the dependencies using the following command: + + cd ~/catkin_ws + rosdep install --from-paths src --ignore-src -r -y + +1. do a catkin_make on the workspace + + cd ~/catkin_ws + catkin_make + +1. Update bashrc to include the setup for neato and set the ros master to the neato computer. I believe you can use the ip address or the hostname. I used the hostname. + + nano ~/.bashrc + +Near the end find the line comment out this line with a #: + + source /opt/ros//setup.bash + +Add the ros master environment variable. it should look like this after the changes. The HOSTNAME should be the one for the Raspberry Pi (Can also be the ip address). + + export ROS_MASTER_URI=http://HOSTNAME:11311 + +1. Reopen terminal or run `source ~/.bashrc` + +1. Check date on both computer. + + date + + To update it install ntppdate and update using ubuntu. + + sudo apt-get install ntpdate + sudo ntpdate ntp.ubuntu.com + +## Running the robot + +### Make a Map + +You can run this package in two modes, Map Making and navigation. You must first make a map if you want to use the navigation. + +On the Raspberry Pi run (startmap): + + roslaunch neato base_map.launch + +On the secondary computer run (startui): + + roslaunch neato map_gui.launch + +The lidar should start spinning on the Neato and Rviz should load on the secondary computer. I found that if the lidar doesn't show on the secondary computer when rviz loads, just close it down and restart the above command from the terminal. + +If you are using a logitech controller as described above you can start driving by holding down the 'A' button and driving with the left stick or left D-pad depending on how your controller is configured (switch on top of F710). The 'A' button on the controller is called the enable_button and is configured in the logitech_teleop.launch file. The 'A' button maps to button 1 but this can be changed if you would like. + +If you are not using the logitech controller and didn't change the config, the easiest thing to do is open a new terminal on either computer and run `rosrun teleop_twist_keyboard teleop_twist_keyboard.py` and the keys indicated to drive around (i,m,j,l). + +### Save Your Map + +This project takes advantage of static maps (rather than SLAM) so you will need to save your map after you have driven around. The map will be used to run the navigation. + +On a new terminal on the Raspberry Pi run (savemap): + + roscd neato_nav/maps and rosrun map_server map_saver + +### Navigate + +This will load the map created previously and allow you to click on the map in RVIZ and have your robot navigate to that location. Close your previous roslaunch tasks mentioned above then: + +On the Raspberry Pi run (startnav): + + roslaunch neato base_nav.launch + +On the secondary computer run (startui): + + roslaunch neato map_gui.launch + +You can still drive around manually if you wish using the logitech controller or using the keyboard as described above. + +## Edit your map + +If you would like, you may edit your map using a image editing program like Gimp. Open the `map.pgm` file saved previously. Use the grey, black, and white colors from your map to edit it. Black is a solid object, white is open space, and grey is unknown space. To save using Gimp, use the "Export as" function and save in raw form. + +## Troubleshooting + +I have followed this setup a few times and received different results. The one error i received about 50% of the time was `"left_wheel_joint" was received but not found in URDF`. I wasn't able to figure out what the source of this was or how to resolve the issue. The only thing I found to fix this was to follow the instructions again starting at the top with re-imaging the Raspberry Pi. If you know the cause of this or the solution, please let me know. diff --git a/README_Melodic.md b/README_Melodic.md deleted file mode 100644 index c2d8b0c..0000000 --- a/README_Melodic.md +++ /dev/null @@ -1,20 +0,0 @@ -Here are some quick notes on running under Ubuntu 18.04 and ROS-Melodic. -I was able to get it working with some minor headaches. - -After a full install of Ubuntu 18.04 and ros-melodic-ddesktop-full - - add the default user to the dialout group - apt install or check that openssh-server is installed - apt install ros-melodic-turtlebot* (may be overkill) and ros-melodic-yocs* - so far I havent been able to locate the turtlebot-teleop-joy node in melodic, - so I don't have a joystick working. - - Assuming you've built a catkin_ws, git clone this repository and catkin_make - - I was using an older Acer small laptop originally supplied with a Turtlebot2. - Make sure your dev/tty is USB0 or ACM0 and that it is properly noted in the bv80bot drivers, - by default it is ttyACM0, but some computers point to ttyUSB0. - - If you are attached to the Botvac USB port, launching the bv80bot_node base_only.launch should start the Lidar spinning. - - There will be some error messages about not finding the joystick. diff --git a/ROS_README.md b/ROS_README.md new file mode 100644 index 0000000..b19a9e2 --- /dev/null +++ b/ROS_README.md @@ -0,0 +1,9 @@ +# Notes on using ROS with this package + +rostopic echo --filter "m.name=='Wall_Distance'" /sensorAnalog + +rostopic /move_base_simple/goal + +roslaunch neato base_map.launch + +roslaunch neato base_nav.launch \ No newline at end of file diff --git a/bv80bot/bv80bot_node/CMakeLists.txt b/bv80bot/bv80bot_node/CMakeLists.txt deleted file mode 100755 index 5fb5bd1..0000000 --- a/bv80bot/bv80bot_node/CMakeLists.txt +++ /dev/null @@ -1,156 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(bv80bot_node) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES bv80bot -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) - -## Declare a cpp library -# add_library(bv80bot -# src/${PROJECT_NAME}/bv80bot.cpp -# ) - -## Declare a cpp executable -# add_executable(bv80bot_node src/bv80bot_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(bv80bot_node bv80bot_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(bv80bot_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS bv80bot bv80bot_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_bv80bot.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/bv80bot/bv80bot_node/launch/bv80bot_base_map.launch b/bv80bot/bv80bot_node/launch/bv80bot_base_map.launch deleted file mode 100755 index 00bcd96..0000000 --- a/bv80bot/bv80bot_node/launch/bv80bot_base_map.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/bv80bot_base_nav.launch b/bv80bot/bv80bot_node/launch/bv80bot_base_nav.launch deleted file mode 100755 index 8c0c6e1..0000000 --- a/bv80bot/bv80bot_node/launch/bv80bot_base_nav.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/bv80bot_base_only.launch b/bv80bot/bv80bot_node/launch/bv80bot_base_only.launch deleted file mode 100755 index 82c045e..0000000 --- a/bv80bot/bv80bot_node/launch/bv80bot_base_only.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/bv80bot/bv80bot_node/launch/bv80bot_gui_only.launch b/bv80bot/bv80bot_node/launch/bv80bot_gui_only.launch deleted file mode 100755 index f0fe9f8..0000000 --- a/bv80bot/bv80bot_node/launch/bv80bot_gui_only.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/bv80bot/bv80bot_node/launch/bv80bot_map_gui.launch b/bv80bot/bv80bot_node/launch/bv80bot_map_gui.launch deleted file mode 100755 index 29a296c..0000000 --- a/bv80bot/bv80bot_node/launch/bv80bot_map_gui.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/bv80bot_nav_gui.launch b/bv80bot/bv80bot_node/launch/bv80bot_nav_gui.launch deleted file mode 100755 index 5886e7f..0000000 --- a/bv80bot/bv80bot_node/launch/bv80bot_nav_gui.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/bv80bot_base.launch b/bv80bot/bv80bot_node/launch/include/bv80bot_base.launch deleted file mode 100755 index 05efe66..0000000 --- a/bv80bot/bv80bot_node/launch/include/bv80bot_base.launch +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/bv80bot_base_cam.launch b/bv80bot/bv80bot_node/launch/include/bv80bot_base_cam.launch deleted file mode 100644 index abc2445..0000000 --- a/bv80bot/bv80bot_node/launch/include/bv80bot_base_cam.launch +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/bv80bot_gui.launch b/bv80bot/bv80bot_node/launch/include/bv80bot_gui.launch deleted file mode 100755 index deb2649..0000000 --- a/bv80bot/bv80bot_node/launch/include/bv80bot_gui.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/bv80bot/bv80bot_node/launch/include/bv80bot_map.launch b/bv80bot/bv80bot_node/launch/include/bv80bot_map.launch deleted file mode 100755 index 05b7104..0000000 --- a/bv80bot/bv80bot_node/launch/include/bv80bot_map.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/bv80bot_nav.launch b/bv80bot/bv80bot_node/launch/include/bv80bot_nav.launch deleted file mode 100755 index e83b77f..0000000 --- a/bv80bot/bv80bot_node/launch/include/bv80bot_nav.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/keyboard_teleop.launch b/bv80bot/bv80bot_node/launch/include/keyboard_teleop.launch deleted file mode 100755 index 9d6175f..0000000 --- a/bv80bot/bv80bot_node/launch/include/keyboard_teleop.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/logitech_teleop.launch b/bv80bot/bv80bot_node/launch/include/logitech_teleop.launch deleted file mode 100755 index fb9f842..0000000 --- a/bv80bot/bv80bot_node/launch/include/logitech_teleop.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/ps3_teleop.launch b/bv80bot/bv80bot_node/launch/include/ps3_teleop.launch deleted file mode 100755 index a91e988..0000000 --- a/bv80bot/bv80bot_node/launch/include/ps3_teleop.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/launch/include/xbox360_teleop.launch b/bv80bot/bv80bot_node/launch/include/xbox360_teleop.launch deleted file mode 100755 index e2dea1b..0000000 --- a/bv80bot/bv80bot_node/launch/include/xbox360_teleop.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/bv80bot/bv80bot_node/package.xml b/bv80bot/bv80bot_node/package.xml deleted file mode 100755 index 7a42f47..0000000 --- a/bv80bot/bv80bot_node/package.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - bv80bot_node - 0.0.0 - The bv80bot_node package provides the drivers and configuration for using a Neato BV80 with a raspberry PI II SBC - - - - - ralph - - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - - - - - yocs_cmd_vel_mux - xacro - yocs_velocity_smoother - teleop_twist_joy - teleop_twist_keyboard - joy - roscpp - sensor_msgs - move_base - map_server - amcl - gmapping - dwa_local_planner - - catkin - - - - - - - - - diff --git a/bv80bot/bv80bot_node/scripts/BumperToLaser.py b/bv80bot/bv80bot_node/scripts/BumperToLaser.py deleted file mode 100755 index 8ed881a..0000000 --- a/bv80bot/bv80bot_node/scripts/BumperToLaser.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python -# license removed for brevity -import roslib; roslib.load_manifest("neato_node") -import rospy -from neato_node.msg import Button, Sensor -from sensor_msgs.msg import LaserScan - -class NeatoNode: - - def __init__(self): - self.default_frame_id = '/base_laser_link' - self.sequence = 1 - self.pub = rospy.Publisher('scan', LaserScan, queue_size=1) - self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10) - sensor = Sensor() - - def callback(self, sensor): - #rospy.loginfo(rospy.get_caller_id() + ' I heard %s',sensor.name) - - laserscan = LaserScan() - laserscan.header.stamp = rospy.Time.now() - laserscan.header.seq = self.sequence - laserscan.header.frame_id = self.default_frame_id - self.sequence = self.sequence + 1 - if (sensor.name == "Left_Side_Bumper"): - laserscan.angle_min = 0.6 - laserscan.angle_max = 1 - laserscan.range_min = 0.0 - laserscan.range_max = 0.3 - laserscan.ranges = [0.29, 0.29] - elif (sensor.name == "Left_Bumper"): - laserscan.angle_min = 0.6 - laserscan.angle_max = 1 - laserscan.range_min = 0.0 - laserscan.range_max = 0.3 - laserscan.ranges = [0.29, 0.29] - elif (sensor.name == "Right_Bumper"): - laserscan.angle_min = -0.6 - laserscan.angle_max = 0 - laserscan.range_min = 0.0 - laserscan.range_max = 0.3 - laserscan.ranges = [0.29, 0.29] - elif (sensor.name == "Right_Side_Bumper"): - laserscan.angle_min = -0.6 - laserscan.angle_max = 0 - laserscan.range_min = 0.0 - laserscan.range_max = 0.3 - laserscan.ranges = [0.29, 0.29] - - #rospy.loginfo(laserscan) - self.pub.publish(laserscan) - - def listener(self): - rospy.init_node('bumperToLaser', anonymous=True) - rospy.loginfo(rospy.get_caller_id() + 'listener start') - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can - # run simultaneously. - rospy.loginfo(rospy.get_caller_id() + 'listener init_node') - - rospy.Subscriber('sensor', Sensor, self.callback) - rospy.loginfo(rospy.get_caller_id() + 'listener set subscriber') - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() - -if __name__ == '__main__': - print("main start") - node = NeatoNode() - node.listener() -#### - - diff --git a/bv80bot/bv80bot_node/scripts/bumper.py b/bv80bot/bv80bot_node/scripts/bumper.py deleted file mode 100755 index 2009502..0000000 --- a/bv80bot/bv80bot_node/scripts/bumper.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -## Simple talker demo that listens to std_msgs/Strings published -## to the 'chatter' topic -# -# 2017 Alan Federman SV-ROS -# -# A simple listener for the SV_ROS (from neato_node) the listens to see if -# the bumper has been pressed on a Botvac. -# -# This code can be incuded in navigation scripts to halt or turn the robot if -# the bumper has been pressed -# -# the values are Left_Bumper Right_Bumper Left_Side_Bumper Right_Side_Bumper -# -# to use install in catkin_ws in a node subdirectory on your laptop for example -# /catkin_ws/src/test/src and rosrun test bumper.py -- of course set the master -# and launch the base on the Botvac. -# -import roslib; roslib.load_manifest("neato_node") -import rospy -from math import sin,cos,pi - -from neato_node.msg import Button, Sensor - -class NeatoNode: - - def __init__(self): - - self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10) - sensor = Sensor() - -def callback(sensor): - rospy.loginfo(rospy.get_caller_id() + ' I heard %s',sensor.name) - -def listener(): - - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can - # run simultaneously. - rospy.init_node('listener', anonymous=True) - - rospy.Subscriber('sensor', Sensor, callback) - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() - -if __name__ == '__main__': - listener() - diff --git a/bv80bot/bv80bot_node/scripts/test_odom.sh b/bv80bot/bv80bot_node/scripts/test_odom.sh deleted file mode 100755 index efe57c9..0000000 --- a/bv80bot/bv80bot_node/scripts/test_odom.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -rostopic pub --once /cmd_vel geometry_msgs/Twist "[0.0,0,0]" "[0,0,3]" -rostopic pub --once /cmd_vel geometry_msgs/Twist "[0.0,0,0]" "[0,0,0]" - diff --git a/bv80bot/neato_robot/README.md b/bv80bot/neato_robot/README.md deleted file mode 100755 index 339d97c..0000000 --- a/bv80bot/neato_robot/README.md +++ /dev/null @@ -1,19 +0,0 @@ -Copy of the neato_robot package containing the Neato robot Drivers and nodes. - -Remove other copies before uing this repo. - -Changes to Origional - - -updated coms software in the driver (multi thread reads) - - -updated URDF - - -Added base_footprint frame - - -AMCL and Move base tuning updates - - -Added arg to switch from mapping and nav modes - - See Top level README.md for more details. - - diff --git a/bv80bot/neato_robot/neato_2dnav/.gitignore b/bv80bot/neato_robot/neato_2dnav/.gitignore deleted file mode 100644 index 914675e..0000000 --- a/bv80bot/neato_robot/neato_2dnav/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -maps - diff --git a/bv80bot/neato_robot/neato_2dnav/launch/amcl.launch b/bv80bot/neato_robot/neato_2dnav/launch/amcl.launch deleted file mode 100755 index 8cdfa91..0000000 --- a/bv80bot/neato_robot/neato_2dnav/launch/amcl.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bv80bot/neato_robot/neato_2dnav/launch/move_base.launch b/bv80bot/neato_robot/neato_2dnav/launch/move_base.launch deleted file mode 100755 index 71393d6..0000000 --- a/bv80bot/neato_robot/neato_2dnav/launch/move_base.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/bv80bot/neato_robot/neato_2dnav/package.xml b/bv80bot/neato_robot/neato_2dnav/package.xml deleted file mode 100755 index 8bc17b3..0000000 --- a/bv80bot/neato_robot/neato_2dnav/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - neato_2dnav - 0.2.0 - - This package contains configuration and launch files for using the navigation stack with Neato robots. - - Michael Ferguson - Michael Ferguson - BSD - http://ros.org/wiki/neato_2dnav - - catkin - - neato_node - \ No newline at end of file diff --git a/bv80bot/neato_robot/neato_2dnav/param/costmap_common_params.yaml b/bv80bot/neato_robot/neato_2dnav/param/costmap_common_params.yaml deleted file mode 100755 index 9beb39d..0000000 --- a/bv80bot/neato_robot/neato_2dnav/param/costmap_common_params.yaml +++ /dev/null @@ -1,7 +0,0 @@ -obstacle_range: 1.75 -raytrace_range: 5.0 - -footprint: [[0.18, 0.18], [0.18, -0.18], [-0.08, -0.18], [-0.2, -0.060],[-0.2,0.060],[-0.08, 0.18]] - -observation_sources: scan -scan: {sensor_frame: base_laser_link, data_type: LaserScan, topic: /scan, marking: true, clearing: true} diff --git a/bv80bot/neato_robot/neato_driver/CMakeLists.txt b/bv80bot/neato_robot/neato_driver/CMakeLists.txt deleted file mode 100755 index b15e8a8..0000000 --- a/bv80bot/neato_robot/neato_driver/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(neato_driver) - -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() diff --git a/bv80bot/neato_robot/neato_driver/package.xml b/bv80bot/neato_robot/neato_driver/package.xml deleted file mode 100755 index e27b5ed..0000000 --- a/bv80bot/neato_robot/neato_driver/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - neato_driver - 0.2.0 - - This is a generic driver for the Neato's robotic vacuums. - - Michael Ferguson - Michael Ferguson - BSD - http://ros.org/wiki/neato_driver - - catkin - - python-serial - - - - - diff --git a/bv80bot/neato_robot/neato_driver/rosdoc.yaml b/bv80bot/neato_robot/neato_driver/rosdoc.yaml deleted file mode 100755 index 0bdea97..0000000 --- a/bv80bot/neato_robot/neato_driver/rosdoc.yaml +++ /dev/null @@ -1 +0,0 @@ - - builder: epydoc diff --git a/bv80bot/neato_robot/neato_driver/src/neato_driver/neato_driver.py b/bv80bot/neato_robot/neato_driver/src/neato_driver/neato_driver.py deleted file mode 100755 index de5e65e..0000000 --- a/bv80bot/neato_robot/neato_driver/src/neato_driver/neato_driver.py +++ /dev/null @@ -1,491 +0,0 @@ -#!/usr/bin/env python - -# Generic driver for the Neato XV-11 Robot Vacuum -# Copyright (c) 2010 University at Albany. All right reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the University at Albany nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, -# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE -# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF -# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -""" -neato_driver.py is a generic driver for the Neato XV-11 Robotic Vacuum. -ROS Bindings can be found in the neato_node package. -""" - -__author__ = "ferguson@cs.albany.edu (Michael Ferguson)" - -import serial -import rospy -import time -import threading - -BASE_WIDTH = 248 # millimeters -MAX_SPEED = 300 # millimeters/second - -xv11_analog_sensors = [ "WallSensorInMM", - "BatteryVoltageInmV", - "LeftDropInMM", - "RightDropInMM", - "RightMagSensor", - "LeftMagSensor", - "XTemp0InC", - "XTemp1InC", - "VacuumCurrentInmA", - "ChargeVoltInmV", - "NotConnected1", - "BatteryTemp1InC", - "NotConnected2", - "CurrentInmA", - "NotConnected3", - "BatteryTemp0InC" ] - -xv11_digital_sensors = [ "SNSR_DC_JACK_CONNECT", - "SNSR_DUSTBIN_IS_IN", - "SNSR_LEFT_WHEEL_EXTENDED", - "SNSR_RIGHT_WHEEL_EXTENDED", - "LSIDEBIT", - "LFRONTBIT", - "RSIDEBIT", - "RFRONTBIT" ] - -xv11_motor_info = [ "Brush_MaxPWM", - "Brush_PWM", - "Brush_mVolts", - "Brush_Encoder", - "Brush_RPM", - "Vacuum_MaxPWM", - "Vacuum_PWM", - "Vacuum_CurrentInMA", - "Vacuum_Encoder", - "Vacuum_RPM", - "LeftWheel_MaxPWM", - "LeftWheel_PWM", - "LeftWheel_mVolts", - "LeftWheel_Encoder", - "LeftWheel_PositionInMM", - "LeftWheel_RPM", - "RightWheel_MaxPWM", - "RightWheel_PWM", - "RightWheel_mVolts", - "RightWheel_Encoder", - "RightWheel_PositionInMM", - "RightWheel_RPM", - "Laser_MaxPWM", - "Laser_PWM", - "Laser_mVolts", - "Laser_Encoder", - "Laser_RPM", - "Charger_MaxPWM", - "Charger_PWM", - "Charger_mAH" ] - -xv11_charger_info = [ "FuelPercent", - "BatteryOverTemp", - "ChargingActive", - "ChargingEnabled", - "ConfidentOnFuel", - "OnReservedFuel", - "EmptyFuel", - "BatteryFailure", - "ExtPwrPresent", - "ThermistorPresent[0]", - "ThermistorPresent[1]", - "BattTempCAvg[0]", - "BattTempCAvg[1]", - "VBattV", - "VExtV", - "Charger_mAH", - "MaxPWM" ] - - -#class xv11(): -class Botvac(): - def __init__(self, port="/dev/ttyUSB0"): - - self.port = serial.Serial(port,115200,timeout=0.1) - - if not self.port.isOpen(): - rospy.logerror("Failed To Open Serial Port") - return - - rospy.loginfo("Open Serial Port %s ok" % port) - - - - # Storage for motor and sensor information - self.state = {"LeftWheel_PositionInMM": 0, "RightWheel_PositionInMM": 0,"LSIDEBIT":0,"RSIDEBIT":0,"LFRONTBIT":0,"RFRONTBIT":0} - - self.stop_state = True - # turn things on - - - self.comsData = [] - self.responseData= [] - self.currentResponse=[] - - self.reading = False - - self.readLock = threading.RLock() - self.readThread = threading.Thread(None,self.read) - self.readThread.start() - - self.port.flushInput() - self.sendCmd("\n\n\n") - self.port.flushInput() - - - self.setTestMode("on") - self.setLDS("on") - - time.sleep(0.5) - self.setLed("ledgreen") - - self.base_width = BASE_WIDTH - self.max_speed = MAX_SPEED - - self.flush() - - rospy.loginfo("Init Done") - - - def exit(self): - self.setLDS("off") - self.setLed("buttonoff") - - time.sleep(1) - - self.setTestMode("off") - self.port.flush() - - self.reading=False - self.readThread.join() - - self.port.close() - - - def setTestMode(self, value): - """ Turn test mode on/off. """ - self.sendCmd("testmode " + value) - - def setLDS(self, value): - self.sendCmd("setldsrotation " + value ) - - def requestScan(self): - """ Ask neato for an array of scan reads. """ - self.sendCmd("getldsscan") - - def getScanRanges(self): - - """ Read values of a scan -- call requestScan first! """ - ranges = list() - intensities = list() - - angle = 0 - - if not self.readTo("AngleInDegrees"): - self.flush() - return [] - - last=False - while not last: #angle < 360: - try: - vals,last = self.getResponse() - except Exception as ex: - rospy.logerr("Exception Reading Neato lidar: " + str(ex)) - last=True - vals=[] - - vals = vals.split(",") - - if ((not last) and ord(vals[0][0])>=48 and ord(vals[0][0])<=57 ): - #print angle, vals - try: - a = int(vals[0]) - r = int(vals[1]) - i = int(vals[2]) - e = int(vals[3]) - - while (angle < a): - ranges.append(0) - intensities.append(0) - angle +=1 - - if(e==0): - ranges.append(r/1000.0) - intensities.append(i) - else: - ranges.append(0) - intensities.append(0) - except: - ranges.append(0) - intensities.append(0) - - angle += 1 - - if len(ranges) != 360: - rospy.loginfo( "Missing laser scans: got %d points" %len(ranges)) - - return ranges, intensities - - def setMotors(self, l, r, s): - """ Set motors, distance left & right + speed """ - #This is a work-around for a bug in the Neato API. The bug is that the - #robot won't stop instantly if a 0-velocity command is sent - the robot - #could continue moving for up to a second. To work around this bug, the - #first time a 0-velocity is sent in, a velocity of 1,1,1 is sent. Then, - #the zero is sent. This effectively causes the robot to stop instantly. - if (int(l) == 0 and int(r) == 0 and int(s) == 0): - if (not self.stop_state): - self.stop_state = True - l = 1 - r = 1 - s = 1 - else: - self.stop_state = False - - self.sendCmd("setmotor" + - " lwheeldist " + str(int(l)) + - " rwheeldist " + str(int(r)) + - " speed " + str(int(s))) - - def getMotors(self): - """ Update values for motors in the self.state dictionary. - Returns current left, right encoder values. """ - - self.sendCmd("getmotors") - - if not self.readTo("Parameter"): - self.flush() - return [0,0] - - last=False - while not last: - #for i in range(len(xv11_motor_info)): - try: - vals,last = self.getResponse() - #print vals,last - values = vals.split(",") - self.state[values[0]] = float(values[1]) - except Exception as ex: - rospy.logerr("Exception Reading Neato motors: " + str(ex)) - - return [self.state["LeftWheel_PositionInMM"],self.state["RightWheel_PositionInMM"]] - - def getAnalogSensors(self): - """ Update values for analog sensors in the self.state dictionary. """ - - self.sendCmd("getanalogsensors") - - if not self.readTo("SensorName"): - self.flush() - return - - last =False - while not last: #for i in range(len(xv11_analog_sensors)): - try: - vals,last = self.getResponse() - values = vals.split(",") - self.state[values[0]] = int(values[1]) - except Exception as ex: - rospy.logerr("Exception Reading Neato Analog sensors: " + str(ex)) - - def getDigitalSensors(self): - """ Update values for digital sensors in the self.state dictionary. """ - - - self.sendCmd("getdigitalsensors") - - if not self.readTo("Digital Sensor Name"): - self.flush() - return [0, 0, 0, 0] - - last =False - while not last: #for i in range(len(xv11_digital_sensors)): - try: - vals,last = self.getResponse() - #print vals - values = vals.split(",") - self.state[values[0]] = int(values[1]) - #print "Got Sensor: %s=%s" %(values[0],values[1]) - except Exception as ex: - rospy.logerr("Exception Reading Neato Digital sensors: " + str(ex)) - return [self.state["LSIDEBIT"], self.state["RSIDEBIT"], self.state["LFRONTBIT"], self.state["RFRONTBIT"]] - - def getButtons(self): - return [0,0,0,0,0] - - def getCharger(self): - """ Update values for charger/battery related info in self.state dictionary. """ - - self.sendCmd("getcharger") - - if not self.readTo("Label"): - self.flush() - return - - last =False - while not last: #for i in range(len(xv11_charger_info)): - - vals,last = self.getResponse() - values=vals.split(",") - try: - self.state[values[0]] = int(values[1]) - - except Exception as ex: - rospy.logerr("Exception Reading Neato charger info: " + str(ex)) - - def setBacklight(self, value): - if value > 0: - self.sendCmd("setled backlighton") - else: - self.sendCmd("setled backlightoff") - - def setLed(self,cmd): - self.sendCmd("setled %s" % cmd) - - def setLED(self,cmd): - self.setLed(cmd) - - def sendCmd(self,cmd): - #rospy.loginfo("Sent command: %s"%cmd) - self.port.write("%s\n" % cmd) - - def readTo(self,tag,timeout=1): - try: - line,last = self.getResponse(timeout) - except: - return False - - if line=="": - return False - - while line.split(",")[0] != tag: - try: - line,last = self.getResponse(timeout) - if line=="": - return False - except: - return False - - return True - - # thread to read data from the serial port - # buffers each line in a list (self.comsData) - # when an end of response (^Z) is read, adds the complete list of response lines to self.responseData and resets the comsData list for the next command response. - def read(self): - self.reading = True - line="" - - while(self.reading and not rospy.is_shutdown()): - try: - val = self.port.read(1) # read from serial 1 char at a time so we can parse each character - except Exception as ex: - rospy.logerr("Exception Reading Neato Serial: " + str(ex)) - val=[] - - if len(val) > 0: - - ''' - if ord(val[0]) < 32: - print("'%s'"% hex(ord(val[0]))) - else: - print ("'%s'"%str(val)) - ''' - - if ord(val[0]) ==13: # ignore the CRs - pass - - elif ord(val[0]) == 26: # ^Z (end of response) - if len(line) > 0: - self.comsData.append(line) # add last line to response set if it is not empty - #print("Got Last Line: %s" % line) - line="" # clear the line buffer for the next line - - #print ("Got Last") - with self.readLock: # got the end of the command response so add the full set of response data as a new item in self.responseData - self.responseData.append(list(self.comsData)) - - self.comsData = [] # clear the bucket for the lines of the next command response - - elif ord(val[0]) == 10: # NL, terminate the current line and add it to the response data list (comsData) (if it is not a blank line) - if len(line) > 0: - self.comsData.append(line) - #print("Got Line: %s" % line) - line = "" # clear the bufer for the next line - else: - line=line+val # add the character to the current line buffer - - # read response data for a command - # returns tuple (line,last) - # line is one complete line of text from the command response - # last = true if the line was the last line of the response data (indicated by a ^Z from the neato) - # returns the next line of data from the buffer. - # if the line was the last line last = true - # if no data is avaialable and we timeout returns line="" - def getResponse(self,timeout=1): - - # if we don't have any data in currentResponse, wait for more data to come in (or timeout) - while (len(self.currentResponse)==0) and (not rospy.is_shutdown()) and timeout > 0: - - with self.readLock: # pop a new response data list out of self.responseData (should contain all data lines returned for the last sent command) - if len(self.responseData) > 0: - self.currentResponse = self.responseData.pop(0) - #print "New Response Set" - else: - self.currentResponse=[] # no data to get - - if len(self.currentResponse)==0: # nothing in the buffer so wait (or until timeout) - time.sleep(0.010) - timeout=timeout-0.010 - - # default to nothing to return - line = "" - last=False - - # if currentResponse has data pop the next line - if not len(self.currentResponse)==0: - line = self.currentResponse.pop(0) - #print line,len(self.currentResponse) - if len(self.currentResponse)==0: - last=True # if this was the last line in the response set the last flag - else: - print("Time Out") # no data so must have timedout - - #rospy.loginfo("Got Response: %s, Last: %d" %(line,last)) - return (line,last) - - def flush(self): - while(1): - l,last= self.getResponse(1) - if l=="": - return - -#SetLED - Sets the specified LED to on,off,blink, or dim. (TestMode Only) -#BacklightOn - LCD Backlight On (mutually exclusive of BacklightOff) -#BacklightOff - LCD Backlight Off (mutually exclusive of BacklightOn) -#ButtonAmber - Start Button Amber (mutually exclusive of other Button options) -#ButtonGreen - Start Button Green (mutually exclusive of other Button options) -#LEDRed - Start Red LED (mutually exclusive of other Button options) -#LEDGreen - Start Green LED (mutually exclusive of other Button options) -#ButtonAmberDim - Start Button Amber Dim (mutually exclusive of other Button options) -#ButtonGreenDim - Start Button Green Dim (mutually exclusive of other Button options) -#ButtonOff - Start Button Off - diff --git a/bv80bot/neato_robot/neato_driver/src/neato_driver/neato_driver.py_bk b/bv80bot/neato_robot/neato_driver/src/neato_driver/neato_driver.py_bk deleted file mode 100755 index 99dd0e7..0000000 --- a/bv80bot/neato_robot/neato_driver/src/neato_driver/neato_driver.py_bk +++ /dev/null @@ -1,205 +0,0 @@ -#!/usr/bin/env python - -# Generic driver for the Neato XV-11 Robot Vacuum -# Copyright (c) 2010 University at Albany. All right reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the University at Albany nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, -# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE -# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF -# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -""" -neato_driver.py is a generic driver for the Neato XV-11 Robotic Vacuum. -ROS Bindings can be found in the neato_node package. -""" - -__author__ = "ferguson@cs.albany.edu (Michael Ferguson)" - -import serial - -""" -This driver has been changed from the original version in order to support -a wider range of neato models and firmware versions. - -The expected responses are not hardcoded in this driver anymore. - -This driver reads responses until it receives a control-z. Neato Robotics has -documented that all responses have a control-Z (^Z) at the end of the -response string: http://www.neatorobotics.com.au/programmer-s-manual -""" - - -class Botvac(): - - def __init__(self, port="/dev/ttyUSB0"): - self.port = serial.Serial(port,115200) - # Storage for motor and sensor information - self.state = {"FuelPercent": 0, "LeftWheel_PositionInMM": 0, "RightWheel_PositionInMM": 0, "LSIDEBIT": 0, - "RSIDEBIT": 0, "LFRONTBIT": 0, "RFRONTBIT": 0, "BTN_SOFT_KEY": 0, "BTN_SCROLL_UP": 0, - "BTN_START": 0, "BTN_BACK": 0, "BTN_SCROLL_DOWN": 0} - self.stop_state = True - - self.base_width = 248 # millimeters - self.max_speed = 300 # millimeters/second - self.crtl_z = chr(26) - # turn things on - self.port.flushInput() - self.port.write("\n") - self.setTestMode("on") - self.setLDS("on") - - def exit(self): - self.port.flushInput() - self.port.write("\n") - self.setLDS("off") - self.setTestMode("off") - - def setTestMode(self, value): - """ Turn test mode on/off. """ - self.port.write("testmode " + value + "\n") - self.readResponseString() - - def setLDS(self, value): - self.port.write("setldsrotation " + value + "\n") - self.readResponseString() - - def requestScan(self): - """ Ask neato for an array of scan reads. """ - self.port.flushInput() - self.port.flushOutput() - self.port.write("getldsscan\n") - response = self.readResponseString() - return response - - def readResponseString(self): - """ Returns the entire response from neato in one string. """ - response = str() - self.port.timeout = 0.001 - while True: - try: - buf = self.port.read(1024) - except: - return "" - if len(buf) == 0: - self.port.timeout *= 2 - else: - response += buf - if buf[len(buf)-1] == self.crtl_z: - break - self.port.timeout = None - return response - - def getScanRanges(self): - """ Read values of a scan -- call requestScan first! """ - ranges = list() - response = self.requestScan() - for line in response.splitlines(): - #print line - vals = line.split(",") - # vals[[0] angle, vals[1] range, vals[2] intensity, vals[3] error code - if len(vals) >= 2 and vals[0].isdigit() and vals[1].isdigit(): - ranges.append(int(vals[1])/1000.0) - # sanity check - if len(ranges) != 360: - return [] - return ranges - - def setMotors(self, l, r, s): - """ Set motors, distance left & right + speed """ - #This is a work-around for a bug in the Neato API. The bug is that the - #robot won't stop instantly if a 0-velocity command is sent - the robot - #could continue moving for up to a second. To work around this bug, the - #first time a 0-velocity is sent in, a velocity of 1,1,1 is sent. Then, - #the zero is sent. This effectively causes the robot to stop instantly. - if (int(l) == 0 and int(r) == 0 and int(s) == 0): - if (not self.stop_state): - self.stop_state = True - l = 1 - r = 1 - s = 1 - else: - self.stop_state = False - self.port.write("setmotor "+str(int(l))+" "+str(int(r))+" "+str(int(s))+"\n") - - def readResponseAndUpdateState(self): - """ Read neato's response and update self.state dictionary. - Call this function only after sending a command. """ - response = self.readResponseString() - for line in response.splitlines(): - vals = line.split(",") - if len(vals) >= 2 and vals[0].replace('_', '').isalpha() and vals[1].isdigit(): - self.state[vals[0]] = int(vals[1]) - - def getMotors(self): - """ Update values for motors in the self.state dictionary. - Returns current left, right encoder values. """ - self.port.flushInput() - self.port.write("getmotors\n") - self.readResponseAndUpdateState() - return [self.state["LeftWheel_PositionInMM"], self.state["RightWheel_PositionInMM"]] - - def getAnalogSensors(self): - """ Update values for analog sensors in the self.state dictionary. """ - self.port.write("getanalogsensors\n") - self.readResponseAndUpdateState() - - def getDigitalSensors(self): - """ Update values for digital sensors in the self.state dictionary. """ - self.port.write("getdigitalsensors\n") - self.readResponseAndUpdateState() - return [self.state["LSIDEBIT"], self.state["RSIDEBIT"], self.state["LFRONTBIT"], self.state["RFRONTBIT"]] - - def getButtons(self): - """ Update values for digital buttons in the self.state dictionary. """ - self.port.write("getbuttons\n") - self.readResponseAndUpdateState() - return [self.state["BTN_SOFT_KEY"], self.state["BTN_SCROLL_UP"], self.state["BTN_START"], self.state["BTN_BACK"], self.state["BTN_SCROLL_DOWN"]] - - def getCharger(self): - """ Update values for charger/battery related info in self.state dictionary. """ - self.port.write("getcharger\n") - self.readResponseAndUpdateState() - return self.state["FuelPercent"] - - def setBacklight(self, value): - - if value > 0: - self.port.write("setled backlighton\n") - else: - self.port.write("setled backlightoff\n") - #self.readResponseString() - - def setLED(self, value): - - if value == "Green": - self.port.write("setled ButtonGreen\n") - if value == "Amber": - self.port.write("setled ButtonAmber\n") - if value == "Red": - self.port.write("setled LEDRed\n") - if value == "Off": - self.port.write("setled ButtonOff\n") - if value == "DimGreen": - self.port.write("setled ButtonGreenDim\n") - if value == "DimAmber": - self.port.write("setled ButtonAmberDim\n") - - - diff --git a/bv80bot/neato_robot/neato_node/launch/bringup.launch b/bv80bot/neato_robot/neato_node/launch/bringup.launch deleted file mode 100755 index ae8820c..0000000 --- a/bv80bot/neato_robot/neato_node/launch/bringup.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/bv80bot/neato_robot/neato_node/msg/Button.msg b/bv80bot/neato_robot/neato_node/msg/Button.msg deleted file mode 100755 index ce504fa..0000000 --- a/bv80bot/neato_robot/neato_node/msg/Button.msg +++ /dev/null @@ -1,2 +0,0 @@ -string name -bool value diff --git a/bv80bot/neato_robot/neato_node/msg/Sensor.msg b/bv80bot/neato_robot/neato_node/msg/Sensor.msg deleted file mode 100755 index ce504fa..0000000 --- a/bv80bot/neato_robot/neato_node/msg/Sensor.msg +++ /dev/null @@ -1,2 +0,0 @@ -string name -bool value diff --git a/bv80bot/neato_robot/neato_node/nodes/neato.py b/bv80bot/neato_robot/neato_node/nodes/neato.py deleted file mode 100755 index d0ca602..0000000 --- a/bv80bot/neato_robot/neato_node/nodes/neato.py +++ /dev/null @@ -1,210 +0,0 @@ -#!/usr/bin/env python - -# ROS node for the Neato Robot Vacuum -# Copyright (c) 2010 University at Albany. All right reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the University at Albany nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, -# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE -# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF -# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -""" -ROS node for Neato robot vacuums. -""" - -__author__ = "ferguson@cs.albany.edu (Michael Ferguson)" - -import roslib; roslib.load_manifest("neato_node") -import rospy -from math import sin,cos,pi - -from sensor_msgs.msg import LaserScan -from neato_node.msg import Button, Sensor -from geometry_msgs.msg import Quaternion -from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry -from tf.broadcaster import TransformBroadcaster - -from neato_driver.neato_driver import Botvac - -class NeatoNode: - - def __init__(self): - """ Start up connection to the Neato Robot. """ - rospy.init_node('neato') - - self.CMD_RATE =2 - - self.port = rospy.get_param('~port', "/dev/ttyUSB0") - rospy.loginfo("Using port: %s" % self.port) - - self.robot = Botvac(self.port) - - rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) - self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10) - self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10) - self.buttonPub = rospy.Publisher('button', Button, queue_size=10) - self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10) - self.odomBroadcaster = TransformBroadcaster() - self.cmd_vel = [0, 0] - self.old_vel = self.cmd_vel - - def spin(self): - encoders = [0, 0] - - self.x = 0 # position in xy plane - self.y = 0 - self.th = 0 - then = rospy.Time.now() - - # things that don't ever change - scan_link = rospy.get_param('~frame_id', 'base_laser_link') - scan = LaserScan(header=rospy.Header(frame_id=scan_link)) - - scan.angle_min =0.0 - scan.angle_max =359.0*pi/180.0 - scan.angle_increment =pi/180.0 - scan.range_min = 0.020 - scan.range_max = 5.0 - - odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint') - - button = Button() - sensor = Sensor() - self.robot.setBacklight(1) - self.robot.setLED("Green") - # main loop of driver - r = rospy.Rate(20) - cmd_rate= self.CMD_RATE - - while not rospy.is_shutdown(): - # notify if low batt - #if self.robot.getCharger() < 10: - # print "battery low " + str(self.robot.getCharger()) + "%" - # get motor encoder values - left, right = self.robot.getMotors() - - cmd_rate = cmd_rate-1 - if cmd_rate ==0: - # send updated movement commands - #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]: - # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) - #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2) - self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) - cmd_rate = self.CMD_RATE - - self.old_vel = self.cmd_vel - - # prepare laser scan - scan.header.stamp = rospy.Time.now() - - self.robot.requestScan() - scan.ranges, scan.intensities = self.robot.getScanRanges() - - # now update position information - dt = (scan.header.stamp - then).to_sec() - then = scan.header.stamp - - d_left = (left - encoders[0])/1000.0 - d_right = (right - encoders[1])/1000.0 - encoders = [left, right] - - #print d_left, d_right, encoders - - dx = (d_left+d_right)/2 - dth = (d_right-d_left)/(self.robot.base_width/1000.0) - - x = cos(dth)*dx - y = -sin(dth)*dx - self.x += cos(self.th)*x - sin(self.th)*y - self.y += sin(self.th)*x + cos(self.th)*y - self.th += dth - #print self.x,self.y,self.th - - # prepare tf from base_link to odom - quaternion = Quaternion() - quaternion.z = sin(self.th/2.0) - quaternion.w = cos(self.th/2.0) - - # prepare odometry - odom.header.stamp = rospy.Time.now() - odom.pose.pose.position.x = self.x - odom.pose.pose.position.y = self.y - odom.pose.pose.position.z = 0 - odom.pose.pose.orientation = quaternion - odom.twist.twist.linear.x = dx/dt - odom.twist.twist.angular.z = dth/dt - - - # sensors - lsb, rsb, lfb, rfb = self.robot.getDigitalSensors() - - # buttons - btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons() - - - # publish everything - self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, - quaternion.w), then, "base_footprint", "odom") - self.scanPub.publish(scan) - self.odomPub.publish(odom) - button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button") - sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper") - for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)): - if b == 1: - button.value = b - button.name = button_enum[idx] - self.buttonPub.publish(button) - - for idx, b in enumerate((lsb, rsb, lfb, rfb)): - if b == 1: - sensor.value = b - sensor.name = sensor_enum[idx] - self.sensorPub.publish(sensor) - # wait, then do it again - r.sleep() - - # shut down - self.robot.setBacklight(0) - self.robot.setLED("Off") - self.robot.setLDS("off") - self.robot.setTestMode("off") - - def sign(self,a): - if a>=0: - return 1 - else: - return-1 - - def cmdVelCb(self,req): - x = req.linear.x * 1000 - th = req.angular.z * (self.robot.base_width/2) - k = max(abs(x-th),abs(x+th)) - # sending commands higher than max speed will fail - - if k > self.robot.max_speed: - x = x*self.robot.max_speed/k; th = th*self.robot.max_speed/k - - self.cmd_vel = [int(x-th), int(x+th)] - -if __name__ == "__main__": - robot = NeatoNode() - robot.spin() - diff --git a/bv80bot/neato_robot/neato_node/package.xml b/bv80bot/neato_robot/neato_node/package.xml deleted file mode 100755 index 300239a..0000000 --- a/bv80bot/neato_robot/neato_node/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - neato_node - 0.2.0 - - A node wrapper for the neato_driver package - - Michael Ferguson - Michael Ferguson - BSD - http://ros.org/wiki/neato_node - - catkin - message_generation - message_runtime - neato_driver - rospy - sensor_msgs - geometry_msgs - nav_msgs - tf - diff --git a/bv80bot/neato_robot/neato_node/urdf/neato.urdf.xacro b/bv80bot/neato_robot/neato_node/urdf/neato.urdf.xacro deleted file mode 100755 index 252cdae..0000000 --- a/bv80bot/neato_robot/neato_node/urdf/neato.urdf.xacro +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/bv80bot/neato_robot/neato_robot/CMakeLists.txt b/bv80bot/neato_robot/neato_robot/CMakeLists.txt deleted file mode 100755 index 001df3a..0000000 --- a/bv80bot/neato_robot/neato_robot/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(neato_robot) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/bv80bot/neato_robot/neato_robot/package.xml b/bv80bot/neato_robot/neato_robot/package.xml deleted file mode 100755 index 5e89653..0000000 --- a/bv80bot/neato_robot/neato_robot/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - neato_robot - 0.2.0 - - Metapackage for drivers for the Neato XV-11 robot. - - Michael Ferguson - Michael Ferguson - BSD - http://ros.org/wiki/neato_robot - - catkin - - neato_driver - neato_node - neato_2dnav - - xacro - - yocs_cmd_vel_mux - yocs_velocity_smoother - - - - - diff --git a/documentation/XV-ProgrammersManual-3_1.pdf b/documentation/XV-ProgrammersManual-3_1.pdf new file mode 100644 index 0000000..1098f46 Binary files /dev/null and b/documentation/XV-ProgrammersManual-3_1.pdf differ diff --git a/documents/Roll Your Own Turtlebot Part II.odt b/documents/Roll Your Own Turtlebot Part II.odt deleted file mode 100755 index 75059a6..0000000 Binary files a/documents/Roll Your Own Turtlebot Part II.odt and /dev/null differ diff --git a/documents/SV ROS Class 2.odp b/documents/SV ROS Class 2.odp deleted file mode 100755 index ea7e06a..0000000 Binary files a/documents/SV ROS Class 2.odp and /dev/null differ diff --git a/documents/SV ROS Class1 ROS.odp b/documents/SV ROS Class1 ROS.odp deleted file mode 100755 index 5554f2c..0000000 Binary files a/documents/SV ROS Class1 ROS.odp and /dev/null differ diff --git a/documents/Simulation Notes b/documents/Simulation Notes deleted file mode 100644 index 40148c3..0000000 --- a/documents/Simulation Notes +++ /dev/null @@ -1,43 +0,0 @@ -Prerequsites: - -Abotix Simulation - -after ROS full desktop install - -sudo apt install ros-*-arbotix-python - -catkin install and make of - -github.com/pirobot/rbx1 - - -After installation try: - -roslaunch rbx1_bringup fake_turtlebot.launch & - -(optionally) - - -roslaunch rbx1_nav fake_move_base_map_with_obstacles.launch & - - -rosrun rviz rviz (rbx1/rbx1_nav/nav.rviz) - - -then rosrun teleop_twist keyboard or other rbx1_nav/nodes programs. - - - -Magni Gazebo Simulations - - -sudo apt install ros-*-magni-robot or catkin_make github ubiquityrobotics/magni_robot - -after - -roslaunch magni_gazebo empty_world.launch & - -pkill roslaunch - - -roslaunch magni_gazebo fiducial_world.launch diff --git a/documents/botvacusb.jpg b/documents/botvacusb.jpg deleted file mode 100644 index 1c6ed53..0000000 Binary files a/documents/botvacusb.jpg and /dev/null differ diff --git a/documents/readme.txt b/documents/readme.txt deleted file mode 100755 index 4535e15..0000000 --- a/documents/readme.txt +++ /dev/null @@ -1 +0,0 @@ -Add notes and Slides etc for the classes to this folder. diff --git a/documents/ros_messages_keynote.tar.gz b/documents/ros_messages_keynote.tar.gz deleted file mode 100755 index 11340a7..0000000 Binary files a/documents/ros_messages_keynote.tar.gz and /dev/null differ diff --git a/bv80bot/neato_robot/neato_node/CMakeLists.txt b/neato/CMakeLists.txt similarity index 73% rename from bv80bot/neato_robot/neato_node/CMakeLists.txt rename to neato/CMakeLists.txt index c4132fe..b726a05 100755 --- a/bv80bot/neato_robot/neato_node/CMakeLists.txt +++ b/neato/CMakeLists.txt @@ -1,20 +1,19 @@ cmake_minimum_required(VERSION 2.8.3) -project(neato_node) +project(neato) find_package(catkin REQUIRED message_generation) add_message_files( FILES - Button.msg - Sensor.msg + BumperEvent.msg + ButtonEvent.msg + Sensors.msg ) - + generate_messages( DEPENDENCIES ) -#catkin_package(DEPENDS message_runtime ) - install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) @@ -22,3 +21,10 @@ install(DIRECTORY launch install(DIRECTORY nodes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +#catkin_package() +#catkin_python_setup() + + + + diff --git a/bv80bot/neato_robot/neato_node/neato.rviz b/neato/config/neato.rviz similarity index 98% rename from bv80bot/neato_robot/neato_node/neato.rviz rename to neato/config/neato.rviz index c33e0ad..03c1e0d 100755 --- a/bv80bot/neato_robot/neato_node/neato.rviz +++ b/neato/config/neato.rviz @@ -38,7 +38,7 @@ Panels: SyncMode: 0 SyncSource: LaserScan Visualization Manager: - Class: "" + Class: '' Displays: - Alpha: 0.5 Cell Size: 1 @@ -112,7 +112,7 @@ Visualization Manager: Value: true Name: RobotModel Robot Description: robot_description - TF Prefix: "" + TF Prefix: '' Update Interval: 0 Value: true Visual Enabled: true @@ -197,8 +197,7 @@ Visualization Manager: map: odom: base_link: - base_laser_link: - {} + base_laser_link: {} Update Interval: 0 Value: true - Class: rviz/Image diff --git a/bv80bot/bv80bot_node/config/rviz.rviz b/neato/config/rviz.rviz similarity index 99% rename from bv80bot/bv80bot_node/config/rviz.rviz rename to neato/config/rviz.rviz index 240b9e7..1467605 100755 --- a/bv80bot/bv80bot_node/config/rviz.rviz +++ b/neato/config/rviz.rviz @@ -39,7 +39,7 @@ Preferences: Toolbars: toolButtonStyle: 2 Visualization Manager: - Class: "" + Class: '' Displays: - Alpha: 0.5 Cell Size: 1 @@ -90,7 +90,7 @@ Visualization Manager: Value: true Name: RobotModel Robot Description: robot_description - TF Prefix: "" + TF Prefix: '' Update Interval: 0 Value: true Visual Enabled: true @@ -169,8 +169,7 @@ Visualization Manager: Show Arrows: true Show Axes: true Show Names: true - Tree: - {} + Tree: {} Update Interval: 0 Value: false - Alpha: 1 diff --git a/neato/launch/base_map.launch b/neato/launch/base_map.launch new file mode 100755 index 0000000..2b0f9df --- /dev/null +++ b/neato/launch/base_map.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/neato/launch/base_nav.launch b/neato/launch/base_nav.launch new file mode 100755 index 0000000..07625e7 --- /dev/null +++ b/neato/launch/base_nav.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/neato/launch/base_only.launch b/neato/launch/base_only.launch new file mode 100755 index 0000000..6f4d6af --- /dev/null +++ b/neato/launch/base_only.launch @@ -0,0 +1,3 @@ + + + diff --git a/neato/launch/bringup.launch b/neato/launch/bringup.launch new file mode 100755 index 0000000..8a7e04a --- /dev/null +++ b/neato/launch/bringup.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/neato/launch/gui_only.launch b/neato/launch/gui_only.launch new file mode 100755 index 0000000..5aff52e --- /dev/null +++ b/neato/launch/gui_only.launch @@ -0,0 +1,3 @@ + + + diff --git a/neato/launch/include/base.launch b/neato/launch/include/base.launch new file mode 100755 index 0000000..0b1fb40 --- /dev/null +++ b/neato/launch/include/base.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/neato/launch/include/base_cam.launch b/neato/launch/include/base_cam.launch new file mode 100644 index 0000000..63d8cb6 --- /dev/null +++ b/neato/launch/include/base_cam.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/neato/launch/include/gui.launch b/neato/launch/include/gui.launch new file mode 100755 index 0000000..195fc71 --- /dev/null +++ b/neato/launch/include/gui.launch @@ -0,0 +1,3 @@ + + + diff --git a/neato/launch/include/keyboard_teleop.launch b/neato/launch/include/keyboard_teleop.launch new file mode 100755 index 0000000..efb5356 --- /dev/null +++ b/neato/launch/include/keyboard_teleop.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/neato/launch/include/logitech_teleop.launch b/neato/launch/include/logitech_teleop.launch new file mode 100755 index 0000000..57d935b --- /dev/null +++ b/neato/launch/include/logitech_teleop.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + diff --git a/neato/launch/include/map.launch b/neato/launch/include/map.launch new file mode 100755 index 0000000..5c12250 --- /dev/null +++ b/neato/launch/include/map.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/neato/launch/include/nav.launch b/neato/launch/include/nav.launch new file mode 100755 index 0000000..e87772f --- /dev/null +++ b/neato/launch/include/nav.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/neato/launch/include/ps3_teleop.launch b/neato/launch/include/ps3_teleop.launch new file mode 100755 index 0000000..6acc718 --- /dev/null +++ b/neato/launch/include/ps3_teleop.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/bv80bot/bv80bot_node/launch/include/velocity_smoother.launch b/neato/launch/include/velocity_smoother.launch similarity index 70% rename from bv80bot/bv80bot_node/launch/include/velocity_smoother.launch rename to neato/launch/include/velocity_smoother.launch index 70288a3..b9df4cf 100755 --- a/bv80bot/bv80bot_node/launch/include/velocity_smoother.launch +++ b/neato/launch/include/velocity_smoother.launch @@ -2,22 +2,21 @@ Velocity smoother for Teleop --> - + - - - - + + + + - - - + + + - + - + - - + diff --git a/neato/launch/include/xbox360_teleop.launch b/neato/launch/include/xbox360_teleop.launch new file mode 100755 index 0000000..206c49b --- /dev/null +++ b/neato/launch/include/xbox360_teleop.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + diff --git a/neato/launch/map_gui.launch b/neato/launch/map_gui.launch new file mode 100755 index 0000000..9287132 --- /dev/null +++ b/neato/launch/map_gui.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/neato/launch/nav_gui.launch b/neato/launch/nav_gui.launch new file mode 100755 index 0000000..9aa8489 --- /dev/null +++ b/neato/launch/nav_gui.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/bv80bot/neato_robot/neato_node/meshes/neato.dae b/neato/meshes/neato.dae similarity index 99% rename from bv80bot/neato_robot/neato_node/meshes/neato.dae rename to neato/meshes/neato.dae index 9aec5ff..971d264 100755 --- a/bv80bot/neato_robot/neato_node/meshes/neato.dae +++ b/neato/meshes/neato.dae @@ -1,4 +1,4 @@ - + diff --git a/bv80bot/neato_robot/neato_node/meshes/neato.skp b/neato/meshes/neato.skp similarity index 98% rename from bv80bot/neato_robot/neato_node/meshes/neato.skp rename to neato/meshes/neato.skp index d1adcb5..18afdc2 100755 Binary files a/bv80bot/neato_robot/neato_node/meshes/neato.skp and b/neato/meshes/neato.skp differ diff --git a/neato/msg/BumperEvent.msg b/neato/msg/BumperEvent.msg new file mode 100644 index 0000000..c8d5727 --- /dev/null +++ b/neato/msg/BumperEvent.msg @@ -0,0 +1,12 @@ +# Bumper values +uint8 LEFT_SIDE = 0 # left front side bumper switch +uint8 RIGHT_SIDE = 1 # right front side bumper switch +uint8 LEFT_FRONT = 2 # Left front bumper switch +uint8 RIGHT_FRONT = 3 # Right front bumper switch +uint8 LEFT_DROP = 4 # Left optical sensor indicating drop off +uint8 RIGHT_DROP = 5 # Right optical sensor indicating drop off +uint8 LEFT_MAG = 6 # Left magnet sensor +uint8 RIGHT_MAG = 7 # Right magnet sensor + +uint8 bumper # values defined above +bool engaged # whether or not the bumper is engaged (detecting something) or not \ No newline at end of file diff --git a/neato/msg/ButtonEvent.msg b/neato/msg/ButtonEvent.msg new file mode 100755 index 0000000..822184a --- /dev/null +++ b/neato/msg/ButtonEvent.msg @@ -0,0 +1,9 @@ +# Buttons +uint8 SOFT_BUTTON = 0 +uint8 UP_BUTTON = 1 +uint8 START_BUTTON = 2 +uint8 BACK_BUTTON = 3 +uint8 DOWN_BUTTON = 4 + +uint8 button #values defined above +bool engaged #whether or not the button is down or not \ No newline at end of file diff --git a/neato/msg/Sensors.msg b/neato/msg/Sensors.msg new file mode 100644 index 0000000..25abd64 --- /dev/null +++ b/neato/msg/Sensors.msg @@ -0,0 +1,36 @@ + +#Analog Sensors +uint16 WallSensorInMM #Example: 34585 +uint16 BatteryVoltageInmV #Example: 16348 +uint16 LeftDropInMM #Example: 0 +uint16 RightDropInMM #Example: 0 +int16 LeftMagSensor #Example: 32768 +int16 RightMagSensor #Example: 32768 +int16 UIButtonInmV #Example: 3330 +int16 VacuumCurrentInmA #Example: 0 +uint16 ChargeVoltInmV #Example: 24024 +int8 BatteryTemp0InC #Example: 30 +int8 BatteryTemp1InC #Example: 28 +int16 CurrentInmA #Example: 40 +int16 SideBrushCurrentInmA #Example: 0 +int16 VoltageReferenceInmV #Example: 1225 +int16 AccelXInmG #Example: 36 +int16 AccelYInmG #Example: 16 +int16 AccelZInmG #Example: 1008 +# the ones below are not supported due to lack of compatibility +# int8 XTemp0InC #Example: 28 +# int8 XTemp1InC #Example: 28 +# int8 NotConnected1 #Example: 0 - unknown +# int8 NotConnected2 #Example: 0 - unknown +# int8 NotConnected3 #Example: 0 - unknown + +#Digital sensors +bool SNSR_DC_JACK_CONNECT #Example: 0 +bool SNSR_DUSTBIN_IS_IN #Example: 1 +bool SNSR_LEFT_WHEEL_EXTENDED #Example: 0 +bool SNSR_RIGHT_WHEEL_EXTENDED #Example: 0 +bool LSIDEBIT #Example: 0 +bool LFRONTBIT #Example: 0 +bool RSIDEBIT #Example: 0 +bool RFRONTBIT #Example: 0 + diff --git a/neato/package.xml b/neato/package.xml new file mode 100755 index 0000000..c866275 --- /dev/null +++ b/neato/package.xml @@ -0,0 +1,42 @@ + + neato + 0.0.1 + + This is a generic driver for the Neato's robotic vacuums. + + This package is derived from code originally developed by Michael Ferguson - ferguson@cs.albany.edu with various contributors. The source of the original code is https://github.com/SV-ROS/intro_to_ros/. Original license disclosures left where applicable. + + Brannon Vann + Brannon Vann + BSD + catkin + message_generation + + + + + + rospy + sensor_msgs + geometry_msgs + nav_msgs + tf + + + yocs_cmd_vel_mux + xacro + yocs_velocity_smoother + teleop_twist_joy + teleop_twist_keyboard + move_base + map_server + amcl + gmapping + dwa_local_planner + + + + diff --git a/bv80bot/bv80bot_node/param/defaults/smoother.yaml b/neato/param/defaults/smoother.yaml similarity index 99% rename from bv80bot/bv80bot_node/param/defaults/smoother.yaml rename to neato/param/defaults/smoother.yaml index e1b61f8..a69a302 100755 --- a/bv80bot/bv80bot_node/param/defaults/smoother.yaml +++ b/neato/param/defaults/smoother.yaml @@ -18,4 +18,3 @@ decel_factor: 1.0 # 1 - odometry # 2 - end robot commands robot_feedback: 1 - diff --git a/bv80bot/bv80bot_node/param/mux.yaml b/neato/param/mux.yaml similarity index 67% rename from bv80bot/bv80bot_node/param/mux.yaml rename to neato/param/mux.yaml index 0deadac..1f8f99b 100755 --- a/bv80bot/bv80bot_node/param/mux.yaml +++ b/neato/param/mux.yaml @@ -6,16 +6,15 @@ # name: Source name # topic: The topic that provides cmd_vel messages # timeout: Time in seconds without incoming messages to consider this topic inactive -# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT +# priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT # short_desc: Short description (optional) subscribers: - - name: "Teleoperation" - topic: "input/teleop" - timeout: 0.2 - priority: 7 - - name: "Navigation" - topic: "input/navi" - timeout: 2.0 - priority: 5 - + - name: 'Teleoperation' + topic: 'input/teleop' + timeout: 0.2 + priority: 7 + - name: 'Navigation' + topic: 'input/navi' + timeout: 2.0 + priority: 5 diff --git a/bv80bot/neato_robot/neato_driver/setup.py b/neato/setup.py similarity index 83% rename from bv80bot/neato_robot/neato_driver/setup.py rename to neato/setup.py index d73b5e9..e3f5bab 100755 --- a/bv80bot/neato_robot/neato_driver/setup.py +++ b/neato/setup.py @@ -4,9 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['neato_driver'], + packages=['neato'], package_dir={'': 'src'}, - ) +) setup(**d) - diff --git a/bv80bot/neato_robot/neato_driver/src/neato_driver/__init__.py b/neato/src/__init__.py similarity index 100% rename from bv80bot/neato_robot/neato_driver/src/neato_driver/__init__.py rename to neato/src/__init__.py diff --git a/neato/src/driver.py b/neato/src/driver.py new file mode 100755 index 0000000..29a5bbc --- /dev/null +++ b/neato/src/driver.py @@ -0,0 +1,835 @@ +#!/usr/bin/env python + +# Generic driver for the Neato XV-11 Robot Vacuum +# Copyright (c) 2010 University at Albany. All right reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the University at Albany nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, +# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +""" +driver.py is a generic driver for the Neato XV-11 Robotic Vacuum. +""" + +__author__ = "ferguson@cs.albany.edu (Michael Ferguson)" + +import serial +import rospy +import roslib +import time +import threading + +from enum import Enum +from math import sin, cos, pi +from tf.broadcaster import TransformBroadcaster +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +from geometry_msgs.msg import Quaternion +from neato.msg import ButtonEvent, BumperEvent, Sensors +from sensor_msgs.msg import LaserScan, BatteryState + +BASE_WIDTH = 248 # millimeters +MAX_SPEED = 300 # millimeters/second +CMD_RATE = 2 + +START_LIDAR = rospy.get_param('START_LIDAR', True) + + +class LED(Enum): + BacklightOn = "BacklightOn" + BacklightOff = "BacklightOff" + ButtonAmber = "ButtonAmber" + ButtonGreen = "ButtonGreen" + LEDRed = "LEDRed" + LEDGreen = "LEDGreen" + ButtonAmberDim = "ButtonAmberDim" + ButtonGreenDim = "ButtonGreenDim" + ButtonOff = "ButtonOff" + + +class Neato: + + def __init__(self): + """ Start up connection to the Neato Robot. """ + rospy.init_node('neato') # ,anonymous = True + + port = rospy.get_param('~port', "/dev/ttyACM0") + rospy.loginfo("Using port: %s" % port) + + self.port = serial.Serial(port, 115200, timeout=0.1) + + if not self.port.isOpen(): + rospy.logerror("Failed To Open Serial Port") + return + + rospy.loginfo("Opened Serial Port %s" % port) + + # Storage for state tracking + self.state = {"LeftWheel_PositionInMM": 0, "RightWheel_PositionInMM": 0, + "LSIDEBIT": False, "RSIDEBIT": False, + "LFRONTBIT": False, "RFRONTBIT": False, + "LeftDropInMM": 0, "RightDropInMM": 0, + "LeftMagSensor": 0, "RightMagSensor": 0, + "BTN_SOFT_KEY": False, "BTN_SCROLL_UP": False, + "BTN_START": False, "BTN_BACK": False, + "BTN_SCROLL_DOWN": False + } + + self.stop_state = True + self.moving_forward = False + self.lifted = False + + # turn things on + self.comsData = [] + self.responseData = [] + self.currentResponse = [] + + self.reading = False + + self.readLock = threading.RLock() + self.readThread = threading.Thread(None, self.read) + self.readThread.start() + + self.port.flushInput() + self.sendCmd("\n\n\n") + self.port.flushInput() + + self.setTestMode("On") + self.setLed(LED.BacklightOn) + self.setLed(LED.LEDGreen) + + time.sleep(0.5) + + self.base_width = BASE_WIDTH + self.max_speed = MAX_SPEED + + # set initial read values from neato + self.getDigitalSensors() + time.sleep(0.5) + self.getAnalogSensors() + time.sleep(0.5) + self.getCharger() + time.sleep(0.5) + self.getButtons() + + self.flush() + + # initialize publishers and subscribers + rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) + self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10) + self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10) + self.batteryPub = rospy.Publisher( + 'sensor_msgs', BatteryState, queue_size=10) + self.buttonEventPub = rospy.Publisher( + 'neato/button_event', ButtonEvent, queue_size=10) + self.bumperEventPub = rospy.Publisher( + 'neato/bumper_event', BumperEvent, queue_size=10) + self.sensorsPub = rospy.Publisher( + 'neato/sensors', Sensors, queue_size=10) + + self.odomBroadcaster = TransformBroadcaster() + self.cmd_vel = [0, 0] + self.old_vel = self.cmd_vel + + def spin(self): + encoders = [0, 0] + + self.x = 0 # position in xy plane + self.y = 0 + self.th = 0 + then = rospy.Time.now() + + # things that don't ever change + scan_link = rospy.get_param('~frame_id', 'base_laser_link') + scan = LaserScan(header=rospy.Header(frame_id=scan_link)) + + scan.angle_min = 0.0 + scan.angle_max = 359.0 * pi / 180.0 + scan.angle_increment = pi / 180.0 + scan.range_min = 0.020 + scan.range_max = 5.0 + + odom = Odometry(header=rospy.Header(frame_id="odom"), + child_frame_id='base_footprint') + + # main loop of driver + r = rospy.Rate(20) + cycle_count = 0 + self.bumperEngaged = None + + while not rospy.is_shutdown(): + + # Emergency shutdown checks. + if int(self.chargerValues["FuelPercent"]) < 10: + rospy.logerr("Neato battery is less than 10%. Terminating Node") + rospy.signal_shutdown( + "Neato battery is less than 10%. Terminating Node") + break + if self.chargerValues["BatteryFailure"] == "1": + rospy.logerr("Neato battery failure. Terminating Node") + rospy.signal_shutdown( + "Neato battery failure. Terminating Node") + break + if self.chargerValues["EmptyFuel"] == "1": + rospy.logerr("Neato battery is empty. Terminating Node") + break + + # get motor encoder values + left, right = self.getMotors() + + if not self.lifted and cycle_count % 2 == 0: + + # bumper engaged procedure + # left or right bumpers + if self.moving_forward and self.bumperEngaged == 0: + # left bump + self.setMotors(-100, -110, MAX_SPEED/2) + + if self.moving_forward and self.bumperEngaged == 1: + # right bump + self.setMotors(-110, -100, MAX_SPEED/2) + + # all other bumpers + elif self.moving_forward and self.bumperEngaged > 1: + self.setMotors(-100, -100, MAX_SPEED/2) + + # undock proceedure + if self.cmd_vel[0] and self.chargerValues["ChargingActive"]: + self.setMotors(-400, -400, MAX_SPEED/2) + + else: + # send updated movement commands + self.setMotors(self.cmd_vel[0], self.cmd_vel[1], + max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1]))) + + self.old_vel = self.cmd_vel + + # prepare laser scan + scan.header.stamp = rospy.Time.now() + + self.getldsscan() + scan.ranges, scan.intensities = self.getScanRanges() + + # now update position information + dt = (scan.header.stamp - then).to_sec() + then = scan.header.stamp + + d_left = (left - encoders[0]) / 1000.0 + d_right = (right - encoders[1]) / 1000.0 + encoders = [left, right] + + dx = (d_left + d_right) / 2 + dth = (d_right - d_left) / (self.base_width / 1000.0) + + x = cos(dth) * dx + y = -sin(dth) * dx + self.x += cos(self.th) * x - sin(self.th) * y + self.y += sin(self.th) * x + cos(self.th) * y + self.th += dth + + # prepare tf from base_link to odom + quaternion = Quaternion() + quaternion.z = sin(self.th / 2.0) + quaternion.w = cos(self.th / 2.0) + + # prepare odometry + odom.header.stamp = rospy.Time.now() + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0 + odom.pose.pose.orientation = quaternion + odom.twist.twist.linear.x = dx / dt + odom.twist.twist.angular.z = dth / dt + + # read sensors and data + # Neato cannot handle reads of all sensors every cycle. + # use cycle_count to rate limit the reads or + # you will get errors like: + # navigation costmap2DROS transform timeout. + # Could not get robot pose. + + if cycle_count % 2 == 0: + self.getDigitalSensors() + + for i, b in enumerate(("LSIDEBIT", "RSIDEBIT", + "LFRONTBIT", "RFRONTBIT")): + + engaged = None + engaged = self.digitalSensors[b] # Bumper Switches + self.bumperHandler(b, engaged, i) + + if cycle_count == 2: + self.getAnalogSensors() + + for i, b in enumerate(("LeftDropInMM", "RightDropInMM", + "LeftMagSensor", "RightMagSensor")): + + engaged = None + if i < 2: + # Optical Sensors (no drop: ~0-60) + engaged = (self.analogSensors[b] > 100) + else: + # Mag Sensors (no mag: ~ +/-20) + engaged = (abs(self.analogSensors[b]) > 20) + + self.bumperHandler(b, engaged, i) + + if cycle_count == 1: + self.getButtons() + + # region Publish Button Events + + for i, b in enumerate(("BTN_SOFT_KEY", "BTN_SCROLL_UP", "BTN_START", + "BTN_BACK", "BTN_SCROLL_DOWN")): + engaged = (self.buttons[b] == 1) + if engaged != self.state[b]: + buttonEvent = ButtonEvent() + buttonEvent.button = i + buttonEvent.engaged = engaged + self.buttonEventPub.publish(buttonEvent) + + self.state[b] = engaged + + # endregion Publish Button Info + + if cycle_count == 3: + self.getCharger() + + # region Publish Battery Info + # pulls data from analogSensors and charger info to publish battery state + battery = BatteryState() + # http://docs.ros.org/en/api/sensor_msgs/html/msg/BatteryState.html + + power_supply_health = 1 # POWER_SUPPLY_HEALTH_GOOD + if self.chargerValues["BatteryOverTemp"]: + power_supply_health = 2 # POWER_SUPPLY_HEALTH_OVERHEAT + elif self.chargerValues["EmptyFuel"]: + power_supply_health = 3 # POWER_SUPPLY_HEALTH_DEAD + elif self.chargerValues["BatteryFailure"]: + power_supply_health = 5 # POWER_SUPPLY_HEALTH_UNSPEC_FAILURE + + power_supply_status = 3 # POWER_SUPPLY_STATUS_NOT_CHARGING + if self.chargerValues["ChargingActive"]: + power_supply_status = 1 # POWER_SUPPLY_STATUS_CHARGING + elif (self.chargerValues["FuelPercent"] == 100): + power_supply_status = 4 # POWER_SUPPLY_STATUS_FULL + + battery.voltage = self.analogSensors["BatteryVoltageInmV"] // 1000 + # battery.temperature = self.analogSensors["BatteryTemp0InC"] + battery.current = self.analogSensors["CurrentInmA"] // 1000 + # battery.charge + # battery.capacity + # battery.design_capacity + battery.percentage = self.chargerValues["FuelPercent"] + battery.power_supply_status = power_supply_status + battery.power_supply_health = power_supply_health + battery.power_supply_technology = 1 # POWER_SUPPLY_TECHNOLOGY_NIMH + battery.present = self.chargerValues['FuelPercent'] > 0 + # battery.cell_voltage + # battery.cell_temperature + # battery.location + # battery.serial_number + self.batteryPub.publish(battery) + + # endregion Publish Battery Info + + self.publishSensors() + # region publish lidar and odom + self.odomBroadcaster.sendTransform( + (self.x, self.y, 0), + (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then, + "base_footprint", "odom") + self.scanPub.publish(scan) + self.odomPub.publish(odom) + + # endregion publish lidar and odom + + # wait, then do it again + r.sleep() + + cycle_count = cycle_count + 1 + if cycle_count == 4: + cycle_count = 0 + + # shut down + self.setLed(LED.BacklightOff) + self.setLed(LED.ButtonOff) + self.setLdsRotation("Off") + self.testmode("Off") + + def publishSensors(): + + # region Publish Sensors + + self.sensors = Sensors() + + # for i, s in enumerate(self.analogSensors): + # self.sensors[s] = self.analogSensors[s] + + # for i, s in enumerate(self.digitalSensors): + # self.sensors[s] = self.digitalSensors + + # if you receive an error similar to "KeyError: 'XTemp0InC'" after startup + # just comment out the value. It means it's not supported by your neato. + + self.WallSensorInMM = self.analogSensors["WallSensorInMM"] + self.sensors.BatteryVoltageInmV = self.analogSensors["BatteryVoltageInmV"] + self.sensors.LeftDropInMM = self.analogSensors["LeftDropInMM"] + self.sensors.RightDropInMM = self.analogSensors["RightDropInMM"] + # self.sensors.XTemp0InC = self.analogSensors["XTemp0InC"] + # self.sensors.XTemp1InC = self.analogSensors["XTemp1InC"] + self.sensors.LeftMagSensor = self.analogSensors["LeftMagSensor"] + self.sensors.RightMagSensor = self.analogSensors["RightMagSensor"] + self.sensors.UIButtonInmV = self.analogSensors["UIButtonInmV"] + self.sensors.VacuumCurrentInmA = self.analogSensors["VacuumCurrentInmA"] + self.sensors.ChargeVoltInmV = self.analogSensors["ChargeVoltInmV"] + self.sensors.BatteryTemp0InC = self.analogSensors["BatteryTemp0InC"] + self.sensors.BatteryTemp1InC = self.analogSensors["BatteryTemp1InC"] + self.sensors.CurrentInmA = self.analogSensors["CurrentInmA"] + self.sensors.SideBrushCurrentInmA = self.analogSensors["SideBrushCurrentInmA"] + self.sensors.VoltageReferenceInmV = self.analogSensors["VoltageReferenceInmV"] + self.sensors.AccelXInmG = self.analogSensors["AccelXInmG"] + self.sensors.AccelYInmG = self.analogSensors["AccelYInmG"] + self.sensors.AccelZInmG = self.analogSensors["AccelZInmG"] + + self.sensors.SNSR_DC_JACK_CONNECT = self.digitalSensors["SNSR_DC_JACK_CONNECT"] + self.sensors.SNSR_DUSTBIN_IS_IN = self.digitalSensors["SNSR_DUSTBIN_IS_IN"] + self.sensors.SNSR_LEFT_WHEEL_EXTENDED = self.digitalSensors["SNSR_LEFT_WHEEL_EXTENDED"] + self.sensors.SNSR_RIGHT_WHEEL_EXTENDED = self.digitalSensors[ + "SNSR_RIGHT_WHEEL_EXTENDED"] + self.sensors.LSIDEBIT = self.digitalSensors["LSIDEBIT"] + self.sensors.LFRONTBIT = self.digitalSensors["LFRONTBIT"] + self.sensors.RSIDEBIT = self.digitalSensors["RSIDEBIT"] + self.sensors.RFRONTBIT = self.digitalSensors["RFRONTBIT"] + + self.lifted = ( + self.sensors.SNSR_LEFT_WHEEL_EXTENDED or self.sensors.SNSR_RIGHT_WHEEL_EXTENDED) + + self.sensorsPub.publish(self.sensors) + + # endregion Publish Sensors + + def bumperHandler(self, name, engaged, i): + if engaged != self.state[name]: + + # set bumper + if not this.bumperEngaged: + this.bumperEngaged = bumperIndex + + # clear bumper + elif this.bumperEngaged == bumperIndex and not engaged: + this.bumperEngaged = None + + bumperEvent = BumperEvent() + bumperEvent.bumper = bumperIndex + bumperEvent.engaged = engaged + # rospy.loginfobumperEvent) + self.bumperEventPub.publish(bumperEvent) + + self.state[name] = engaged + + def sign(self, a): + if a >= 0: + return 1 + else: + return -1 + + def cmdVelCb(self, req): + x = req.linear.x * 1000 + th = req.angular.z * (self.base_width / 2) + k = max(abs(x - th), abs(x + th)) + # sending commands higher than max speed will fail + + if k > self.max_speed: + x = x * self.max_speed / k + th = th * self.max_speed / k + + self.cmd_vel = [int(x - th), int(x + th)] + + def exit(self): + self.setLdsRotation("Off") + self.setLed(LED.ButtonOff) + + time.sleep(1) + + self.testmode("Off") + self.port.flush() + + self.reading = False + self.readThread.join() + + self.port.close() + + def testmode(self, value): + """ Turn test mode on/off. """ + self.sendCmd("testmode " + value) + + def setLdsRotation(self, value): + self.sendCmd("setldsrotation " + value) + + def getldsscan(self): + """ Ask neato for an array of scan reads. """ + self.sendCmd("getldsscan") + + def getScanRanges(self): + """ Read values of a scan -- call requestScan first! """ + ranges = list() + intensities = list() + + angle = 0 + + if not self.readTo("AngleInDegrees"): + self.flush() + return ranges, intensities + + last = False + while not last: # angle < 360: + try: + vals, last = self.getResponse() + except Exception as ex: + rospy.logerr("Exception Reading Neato lidar: " + str(ex)) + last = True + vals = [] + + vals = vals.split(",") + + if ((not last) and ord(vals[0][0]) >= 48 + and ord(vals[0][0]) <= 57): + # rospy.loginfo(angle, vals) + try: + a = int(vals[0]) + r = int(vals[1]) + i = int(vals[2]) + e = int(vals[3]) + + while (angle < a): + ranges.append(0) + intensities.append(0) + angle += 1 + + if (e == 0): + ranges.append(r / 1000.0) + intensities.append(i) + else: + ranges.append(0) + intensities.append(0) + except: + ranges.append(0) + intensities.append(0) + + angle += 1 + + if len(ranges) != 360: + rospy.loginfo("Missing laser scans: got %d points" % len(ranges)) + + return ranges, intensities + + def setMotors(self, l, r, s): + """ Set motors, distance left & right + speed """ + # This is a work-around for a bug in the Neato API. The bug is that the + # robot won't stop instantly if a 0-velocity command is sent - the robot + # could continue moving for up to a second. To work around this bug, the + # first time a 0-velocity is sent in, a velocity of 1,1,1 is sent. Then, + # the zero is sent. This effectively causes the robot to stop instantly. + if (int(l) == 0 and int(r) == 0 and int(s) == 0): + if (not self.stop_state): + self.stop_state = True + l = 1 + r = 1 + s = 1 + else: + self.stop_state = False + + self.moving_forward = (l > 0 or r > 0) + + self.sendCmd("setmotor" + " lwheeldist " + str(int(l)) + + " rwheeldist " + str(int(r)) + " speed " + str(int(s))) + + def getMotors(self): + """ Update values for motors in the self.state dictionary. + Returns current left, right encoder values. """ + + self.sendCmd("getmotors") + + if not self.readTo("Parameter"): + self.flush() + return [0, 0] + + last = False + while not last: + # for i in range(len(xv11_motor_info)): + try: + vals, last = self.getResponse() + # rospy.loginfo(vals,last) + values = vals.split(",") + self.state[values[0]] = float(values[1]) + except Exception as ex: + rospy.logerr("Exception Reading Neato motors: " + str(ex)) + + return [ + self.state["LeftWheel_PositionInMM"], + self.state["RightWheel_PositionInMM"] + ] + + def getAnalogSensors(self): + """ Update values for analog sensors in the self.state dictionary. """ + + self.sendCmd("getanalogsensors") + + if not self.readTo("SensorName"): + self.flush() + return + + last = False + analogSensors = {} + while not last: # for i in range(len(xv11_analog_sensors)): + try: + vals, last = self.getResponse() + values = vals.split(",") + # self.state[values[0]] = int(values[1]) + analogSensors[values[0]] = int(values[1]) + except Exception as ex: + rospy.logerr("Exception Reading Neato Analog sensors: " + + str(ex)) + + if analogSensors: + self.analogSensors = analogSensors + + return analogSensors + + def getDigitalSensors(self): + """ Update values for digital sensors in the self.state dictionary. """ + + self.sendCmd("getdigitalsensors") + + if not self.readTo("Digital Sensor Name"): + self.flush() + return {} + + last = False + digitalSensors = {} + while not last: # for i in range(len(xv11_digital_sensors)): + try: + vals, last = self.getResponse() + # rospy.loginfo(vals) + values = vals.split(",") + # rospy.loginfo("Got Sensor: %s=%s" %(values[0],values[1])) + # self.state[values[0]] = int(values[1]) + digitalSensors[values[0]] = int(values[1]) + except Exception as ex: + rospy.logerr("Exception Reading Neato Digital sensors: " + + str(ex)) + + if digitalSensors: + self.digitalSensors = digitalSensors + + return digitalSensors + + def getButtons(self): + """ Get button values from neato. """ + + self.sendCmd("GetButtons") + + if not self.readTo("Button Name"): + self.flush() + return {} + + last = False + buttons = {} + while not last: + vals, last = self.getResponse() + values = vals.split(",") + try: + # self.state[values[0]] = (values[1] == '1') + buttons[values[0]] = (values[1] == '1') + except Exception as ex: + rospy.logerr("Exception Reading Neato button info: " + + str(ex)) + + if buttons: + self.buttons = buttons + + return buttons + + def getCharger(self): + """ Update values for charger/battery related info in self.state dictionary. """ + + self.sendCmd("getcharger") + + if not self.readTo("Label"): + self.flush() + return + + last = False + chargerValues = {} + while not last: # for i in range(len(xv11_charger_info)): + + vals, last = self.getResponse() + values = vals.split(",") + try: + if values[0] in ["VBattV", "VExtV"]: + # convert to millivolt to maintain as int and become mVBattV & mVExtV. + # self.state['m' + values[0]] = int(float(values[1]) * 100) + chargerValues['m' + values[0]] = int( + float(values[1]) * 100) + elif values[0] in ["BatteryOverTemp", "ChargingActive", "ChargingEnabled", "ConfidentOnFuel", "OnReservedFuel", "EmptyFuel", "BatteryFailure", "ExtPwrPresent"]: + # boolean values + # self.state[values[0]] = (values[1] == '1') + chargerValues[values[0]] = (values[1] == '1') + elif values[0] in ["FuelPercent", "MaxPWM", "PWM"]: + # int values + # self.state[values[0]] = int(values[1]) + chargerValues[values[0]] = int(values[1]) + # other values not supported. + + except Exception as ex: + rospy.logerr("Exception Reading Neato charger info: " + + str(ex)) + + if chargerValues: + self.chargerValues = chargerValues + return chargerValues + + def setLed(self, command): + self.sendCmd("setled %s" % command) + + def sendCmd(self, cmd): + # rospy.loginfo("Sent command: %s"%cmd) + self.port.write("%s\n" % cmd) + + def readTo(self, tag, timeout=1): + try: + line, last = self.getResponse(timeout) + except: + return False + + if line == "": + return False + + while line.split(",")[0] != tag: + try: + line, last = self.getResponse(timeout) + if line == "": + return False + except: + return False + + return True + + # thread to read data from the serial port + # buffers each line in a list (self.comsData) + # when an end of response (^Z) is read, adds the complete list of response lines to self.responseData and resets the comsData list for the next command response. + def read(self): + self.reading = True + line = "" + + while (self.reading and not rospy.is_shutdown()): + try: + # read from serial 1 char at a time so we can parse each character + val = self.port.read(1) + except Exception as ex: + rospy.logerr("Exception Reading Neato Serial: " + str(ex)) + val = [] + + if len(val) > 0: + ''' + if ord(val[0]) < 32: + rospy.loginfo"'%s'"% hex(ord(val[0]))) + else: + rospy.loginfo"'%s'"%str(val)) + ''' + + if ord(val[0]) == 13: # ignore the CRs + pass + + elif ord(val[0]) == 26: # ^Z (end of response) + if len(line) > 0: + # add last line to response set if it is not empty + self.comsData.append(line) + # rospy.loginfo"Got Last Line: %s" % line) + line = "" # clear the line buffer for the next line + + # rospy.loginfo("Got Last") + with self.readLock: # got the end of the command response so add the full set of response data as a new item in self.responseData + self.responseData.append(list(self.comsData)) + + self.comsData = [ + ] # clear the bucket for the lines of the next command response + + # NL, terminate the current line and add it to the response data list (comsData) (if it is not a blank line) + elif ord(val[0]) == 10: + if len(line) > 0: + self.comsData.append(line) + # rospy.loginfo"Got Line: %s" % line) + line = "" # clear the bufer for the next line + else: + line = line + val # add the character to the current line buffer + + # read response data for a command + # returns tuple (line,last) + # line is one complete line of text from the command response + # last = true if the line was the last line of the response data (indicated by a ^Z from the neato) + # returns the next line of data from the buffer. + # if the line was the last line last = true + # if no data is avaialable and we timeout returns line="" + def getResponse(self, timeout=1): + + # if we don't have any data in currentResponse, wait for more data to come in (or timeout) + while (len(self.currentResponse) + == 0) and (not rospy.is_shutdown()) and timeout > 0: + + # pop a new response data list out of self.responseData (should contain all data lines returned for the last sent command) + with self.readLock: + if len(self.responseData) > 0: + self.currentResponse = self.responseData.pop(0) + # rospy.loginfo("New Response Set") + else: + self.currentResponse = [] # no data to get + + if len(self.currentResponse + ) == 0: # nothing in the buffer so wait (or until timeout) + time.sleep(0.010) + timeout = timeout - 0.010 + + # default to nothing to return + line = "" + last = False + + # if currentResponse has data pop the next line + if not len(self.currentResponse) == 0: + line = self.currentResponse.pop(0) + # rospy.loginfo(line,len(self.currentResponse)) + if len(self.currentResponse) == 0: + last = True # if this was the last line in the response set the last flag + else: + rospy.loginfo("Time Out") # no data so must have timedout + # rospy.loginfo("Got Response: %s, Last: %d" %(line,last)) + return (line, last) + + def flush(self): + while (1): + l, last = self.getResponse(1) + if l == "": + return + + +if __name__ == "__main__": + robot = Neato() + robot.spin() diff --git a/neato/urdf/neato.urdf.xacro b/neato/urdf/neato.urdf.xacro new file mode 100755 index 0000000..e2fb30b --- /dev/null +++ b/neato/urdf/neato.urdf.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/neato_nav/.gitignore b/neato_nav/.gitignore new file mode 100644 index 0000000..e52738a --- /dev/null +++ b/neato_nav/.gitignore @@ -0,0 +1,3 @@ +maps/*.pgm +maps/*.yaml + diff --git a/bv80bot/neato_robot/neato_2dnav/CMakeLists.txt b/neato_nav/CMakeLists.txt similarity index 93% rename from bv80bot/neato_robot/neato_2dnav/CMakeLists.txt rename to neato_nav/CMakeLists.txt index 7021675..63c8ded 100755 --- a/bv80bot/neato_robot/neato_2dnav/CMakeLists.txt +++ b/neato_nav/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(neato_2dnav) +project(neato_nav) find_package(catkin REQUIRED) catkin_package(DEPENDS) diff --git a/neato_nav/launch/amcl.launch b/neato_nav/launch/amcl.launch new file mode 100755 index 0000000..e71f847 --- /dev/null +++ b/neato_nav/launch/amcl.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/neato_nav/launch/move_base.launch b/neato_nav/launch/move_base.launch new file mode 100755 index 0000000..005335d --- /dev/null +++ b/neato_nav/launch/move_base.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/neato_nav/maps/.keep b/neato_nav/maps/.keep new file mode 100644 index 0000000..ca1c2fb --- /dev/null +++ b/neato_nav/maps/.keep @@ -0,0 +1 @@ +This file is here to maintain the data folder but files from the data directory are not included in the git source set. \ No newline at end of file diff --git a/neato_nav/package.xml b/neato_nav/package.xml new file mode 100755 index 0000000..5ce838d --- /dev/null +++ b/neato_nav/package.xml @@ -0,0 +1,17 @@ + + neato_nav + 0.0.1 + + This package contains configuration and launch files for using the navigation stack with Neato robots. + + This package is derived from code originally developed by Michael Ferguson - ferguson@cs.albany.edu with various contributors. The source of the original code is https://github.com/SV-ROS/intro_to_ros/. Original license disclosures left where applicable. + + Brannon Vann + Brannon Vann + BSD + http://ros.org/wiki/neato_nav + + catkin + + neato + \ No newline at end of file diff --git a/bv80bot/neato_robot/neato_2dnav/param/base_local_planner_params.yaml b/neato_nav/param/base_local_planner_params.yaml similarity index 91% rename from bv80bot/neato_robot/neato_2dnav/param/base_local_planner_params.yaml rename to neato_nav/param/base_local_planner_params.yaml index 9470a42..fc2bec1 100755 --- a/bv80bot/neato_robot/neato_2dnav/param/base_local_planner_params.yaml +++ b/neato_nav/param/base_local_planner_params.yaml @@ -1,7 +1,6 @@ base_global_planner: navfn/NavfnROS base_local_planner: dwa_local_planner/DWAPlannerROS - planner_frequency: 2.0 controller_frequency: 1.5 @@ -18,10 +17,7 @@ shutdown_cost_maps: true oscillation_timeout: 9.0 oscillation_distance: 0.25 - - DWAPlannerROS: - max_vel_trans: 0.15 min_vel_trans: 0.015 @@ -39,7 +35,7 @@ DWAPlannerROS: acc_lim_theta: 0.5 acc_lim_trans: 0.1 - xy_goal_tolerance: 0.075 + xy_goal_tolerance: 0.075 yaw_goal_tolerance: 0.25 latch_xy_goal_tolerance: true prune_plan: true @@ -48,10 +44,10 @@ DWAPlannerROS: theta_stopped_vel: 0.1 sim_time: 3.0 - sim_granularity: 0.025 + sim_granularity: 0.025 angular_sim_granularity: 0.015 - + path_distance_bias: 50.0 goal_distance_bias: 30.0 occdist_scale: 0.9 @@ -64,15 +60,10 @@ DWAPlannerROS: forward_point_distance: 0.433 scaling_speed: 0.5 - max_scaling_factor: 1.17 - + max_scaling_factor: 1.17 + vx_samples: 20 vy_samples: 10 vth_samples: 20 use_dwa: true - - - - - diff --git a/neato_nav/param/costmap_common_params.yaml b/neato_nav/param/costmap_common_params.yaml new file mode 100755 index 0000000..f6f3e36 --- /dev/null +++ b/neato_nav/param/costmap_common_params.yaml @@ -0,0 +1,7 @@ +obstacle_range: 1.75 +raytrace_range: 5.0 + +footprint: [[0.18, 0.18], [0.18, -0.18], [-0.08, -0.18], [-0.2, -0.060], [-0.2, 0.060], [-0.08, 0.18]] + +observation_sources: scan +scan: { sensor_frame: base_laser_link, data_type: LaserScan, topic: /scan, marking: true, clearing: true } diff --git a/bv80bot/neato_robot/neato_2dnav/param/global_costmap_params.yaml b/neato_nav/param/global_costmap_params.yaml similarity index 88% rename from bv80bot/neato_robot/neato_2dnav/param/global_costmap_params.yaml rename to neato_nav/param/global_costmap_params.yaml index d4f47aa..bac98e7 100755 --- a/bv80bot/neato_robot/neato_2dnav/param/global_costmap_params.yaml +++ b/neato_nav/param/global_costmap_params.yaml @@ -1,5 +1,5 @@ global_costmap: - transform_tolerance: 2.0 + transform_tolerance: 2.0 update_frequency: 1.0 publish_frequency: 2.0 @@ -10,7 +10,6 @@ global_costmap: footprint_padding: 0.0 - static_map: true global_frame: map @@ -19,4 +18,3 @@ global_costmap: inflation_layer: inflation_radius: 0.26 cost_scaling_factor: 9.0 - diff --git a/bv80bot/neato_robot/neato_2dnav/param/local_costmap_params.yaml b/neato_nav/param/local_costmap_params.yaml similarity index 91% rename from bv80bot/neato_robot/neato_2dnav/param/local_costmap_params.yaml rename to neato_nav/param/local_costmap_params.yaml index 32def42..372cb13 100755 --- a/bv80bot/neato_robot/neato_2dnav/param/local_costmap_params.yaml +++ b/neato_nav/param/local_costmap_params.yaml @@ -1,10 +1,9 @@ local_costmap: - transform_tolerance: 1.0 update_frequency: 2.0 publish_frequency: 2.0 - + width: 3.0 height: 3.0 @@ -15,12 +14,9 @@ local_costmap: footprint_padding: 0.01 - global_frame: odom + global_frame: odom robot_base_frame: base_footprint - + inflation_layer: inflation_radius: 0.26 cost_scaling_factor: 9.0 - - - diff --git a/sample_code/beginner_tutorials/CMakeLists.txt b/sample_code/beginner_tutorials/CMakeLists.txt deleted file mode 100755 index 662c192..0000000 --- a/sample_code/beginner_tutorials/CMakeLists.txt +++ /dev/null @@ -1,174 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(beginner_tutorials) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs -message_generation -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -add_message_files( - FILES - distance.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -add_service_files( - FILES - AdcToDistance.srv -) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here - generate_messages( - DEPENDENCIES - std_msgs - ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES beginner_tutorials -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(beginner_tutorials -# src/${PROJECT_NAME}/beginner_tutorials.cpp -# ) - -## Declare a cpp executable -# add_executable(beginner_tutorials_node src/beginner_tutorials_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(beginner_tutorials_node beginner_tutorials_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(beginner_tutorials_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS beginner_tutorials beginner_tutorials_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_beginner_tutorials.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/sample_code/beginner_tutorials/msg/distance.msg b/sample_code/beginner_tutorials/msg/distance.msg deleted file mode 100755 index 0dba053..0000000 --- a/sample_code/beginner_tutorials/msg/distance.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint8 raw -float32 inch -float32 cm diff --git a/sample_code/beginner_tutorials/package.xml b/sample_code/beginner_tutorials/package.xml deleted file mode 100755 index ab18dc1..0000000 --- a/sample_code/beginner_tutorials/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - beginner_tutorials - 0.0.0 - The beginner_tutorials package - - - - - ubuntu - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - message_generation - roscpp - rospy - std_msgs - message_runtime - - - - - - - diff --git a/sample_code/beginner_tutorials/scripts/AdcToDistance.py b/sample_code/beginner_tutorials/scripts/AdcToDistance.py deleted file mode 100755 index 4697ed2..0000000 --- a/sample_code/beginner_tutorials/scripts/AdcToDistance.py +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from beginner_tutorials.srv import * -import rospy - -def handle_AdcToDistance(req): - - print "Returning distance values for analog value %d " % (req.adc) - inches = (254.0/1024.0) *2.0* (req.adc/5) - cm = inches * 2.54 - - return AdcToDistanceResponse(inches,cm) - -def AdcToDistance_server(): - rospy.init_node('AdcToDistance_server') - s = rospy.Service('adc_to_distance', AdcToDistance, handle_AdcToDistance) - print "Ready to convert adc values to distance" - rospy.spin() - -if __name__ == "__main__": - AdcToDistance_server() - diff --git a/sample_code/beginner_tutorials/scripts/AdcToDistance_Client.py b/sample_code/beginner_tutorials/scripts/AdcToDistance_Client.py deleted file mode 100755 index 3689547..0000000 --- a/sample_code/beginner_tutorials/scripts/AdcToDistance_Client.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python -import roslib; roslib.load_manifest('beginner_tutorials') - -import sys - -import rospy -from beginner_tutorials.srv import * - -def adc_to_distance_client(adc): - rospy.wait_for_service('adc_to_distance') - try: - adc_dist = rospy.ServiceProxy('adc_to_distance', AdcToDistance) - resp1 = adc_dist(adc) - print resp1.inches - print resp1.cm - return resp1 - except rospy.ServiceException, e: - print "Service call failed: %s" % e - -def usage(): - return "%s [ x where 0 < x < 1023 ]" % sys.argv[0] - -if __name__ == "__main__": - if len(sys.argv) == 2: - x = int(sys.argv[1]) - else: - print usage() - sys.exit(1) - print "Requesting to convert %d to distance" % x - results=adc_to_distance_client(x) - print "%d = %3.3f in inches and %3.3f in cm " % (x, results.inches,results.cm) diff --git a/sample_code/beginner_tutorials/scripts/Sonar_Node.py b/sample_code/beginner_tutorials/scripts/Sonar_Node.py deleted file mode 100755 index af61e2d..0000000 --- a/sample_code/beginner_tutorials/scripts/Sonar_Node.py +++ /dev/null @@ -1,65 +0,0 @@ -import sys -import getopt - -import rospy -import roslib; roslib.load_manifest('beginner_tutorials') -from std_msgs.msg import UInt16 -from sensor_msgs.msg import Range - -from beginner_tutorials.srv import * - -class Sonar_Node(object): - - def __init__(self): - - rospy.init_node('sonar_node',anonymous=False) - self.inchpub = rospy.Publisher('/ultrasound/inch',UInt16) - self.cmpub = rospy.Publisher('/ultrasound/cm',UInt16) - rospy.on_shutdown(self.shutdown) - - robotrate=10 - r=rospy.Rate(robotrate) - - self.analog_range=0 - - def shutdown(self): - # Always stop the robot when shutting down the node. - rospy.loginfo("Stopping the Node...") - rospy.sleep(1) - - - def sonarCb(self,Range): - msg_str="SonarRange=%3.2f" % (Range.range) - rospy.loginfo(rospy.get_name()+msg_str) - self.analog_range = Range.range - - def listener(self): - rospy.Subscriber("ultrasound_fwd", Range, self.sonarCb) - - def calc_distance(self): - - rospy.wait_for_service('adc_to_distance') - try: - adc_dist = rospy.ServiceProxy('adc_to_distance', AdcToDistance) - distance = adc_dist(self.analog_range) - print distance.inches - print distance.cm - #self.inches=distance.inches - #self.cm=distance.cm - - self.inchpub.publish(distance.inches) - self.cmpub.publish(distance.cm) - - except rospy.ServiceException, e: - print "Service call failed: %s" % e - -if __name__ == '__main__': - - sn=Sonar_Node() - sn.listener() - - r=rospy.Rate(10) - - while not rospy.is_shutdown(): - sn.calc_distance() - r.sleep() diff --git a/sample_code/beginner_tutorials/scripts/bumper.py b/sample_code/beginner_tutorials/scripts/bumper.py deleted file mode 100755 index bbaff63..0000000 --- a/sample_code/beginner_tutorials/scripts/bumper.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -## Simple talker demo that listens to std_msgs/Strings published -## to the 'chatter' topic -# -# 2017 Alan Federman SV-ROS -# -# A simple listener for the SV_ROS (from neato_node) the listens to see if -# the bumper has been pressed on a Botvac. -# -# This code can be incuded in navigation scripts to halt or turn the robot if -# the bumper has been pressed -# -# the values are Left_Bumper Right_Bumper Left_Side_Bumper Right_Side_Bumper -# -# to use install in catkin_ws in a node subdirectory on your laptop for example -# /catkin_ws/src/test/src and rosrun test bumper.py -- of course set the master -# and launch the base on the Botvac. -# -import roslib; roslib.load_manifest("neato_node") -import rospy -from math import sin,cos,pi - -from neato_node.msg import Button, Sensor - -class NeatoNode: - - def __init__(self): - - self.sensorPub = rospy.Publisher('sensor', Sensor, queue_size=10) - sensor = Sensor() - -def callback(sensor): - rospy.loginfo(rospy.get_caller_id() + ' I heard %s',sensor.name) - -def listener(): - - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can - # run simultaneously. - rospy.init_node('listener', anonymous=True) - - rospy.Subscriber('sensor', Sensor, callback) - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() - -if __name__ == '__main__': - listener() diff --git a/sample_code/beginner_tutorials/srv/AdcToDistance.srv b/sample_code/beginner_tutorials/srv/AdcToDistance.srv deleted file mode 100755 index 5e37882..0000000 --- a/sample_code/beginner_tutorials/srv/AdcToDistance.srv +++ /dev/null @@ -1,4 +0,0 @@ -int16 adc ---- -float32 inches -float32 cm diff --git a/sample_code/beginner_tutorials/srv/AddTwoInts.srv b/sample_code/beginner_tutorials/srv/AddTwoInts.srv deleted file mode 100755 index 3a68808..0000000 --- a/sample_code/beginner_tutorials/srv/AddTwoInts.srv +++ /dev/null @@ -1,4 +0,0 @@ -int64 a -int64 b ---- -int64 sum diff --git a/sample_code/beginner_tutorials/srv/AnalogToDistance.srv b/sample_code/beginner_tutorials/srv/AnalogToDistance.srv deleted file mode 100755 index 3c551ed..0000000 --- a/sample_code/beginner_tutorials/srv/AnalogToDistance.srv +++ /dev/null @@ -1,4 +0,0 @@ -int16 range ---- -float32 inches -float32 cm diff --git a/sample_code/svros_single_sonar/svros_single_sonar.ino b/sample_code/svros_single_sonar/svros_single_sonar.ino deleted file mode 100755 index c929fce..0000000 --- a/sample_code/svros_single_sonar/svros_single_sonar.ino +++ /dev/null @@ -1,83 +0,0 @@ -/* - *Process rosserial messages for a Maxbotix Sonar - */ -#include -//#include -#include -#include -#include -#include - -const int fwd_sonar=0; - -char sonarframe_fwd[]="/ultrasound_fwd"; - -ros::NodeHandle nh; - -sensor_msgs::Range range_msg_fwd; - -//sensor_msgs::Range range_msg_arr[3]; - -void initSonar( sensor_msgs::Range range_msg, char* frameid) { - - range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; - range_msg.header.frame_id = frameid; - range_msg.field_of_view = 0.1; // fake - range_msg.min_range = 0.0; - range_msg.max_range = 6.47; -} - -ros::Publisher pub_sonar_fwd( "/ultrasound_fwd", &range_msg_fwd); - -void setup() -{ - Serial.begin(9600); - initSonar(range_msg_fwd,sonarframe_fwd); - - nh.initNode(); - nh.advertise(pub_sonar_fwd); - -} - -void loop() -{ - - static int i = 0; - int j; - uint16_t blocks; - char buf[32]; - float sonarval=0.0; - int num_sonars=1; - int num_samples=5; - - sonarval=ProcSonar(num_samples); - - initSonar(range_msg_fwd,sonarframe_fwd); - - range_msg_fwd.range = sonarval; - - range_msg_fwd.header.stamp = nh.now(); - - if ( range_msg_fwd.range < 100) { - - pub_sonar_fwd.publish(&range_msg_fwd); - } - - nh.spinOnce(); - - //Serial.println(sonarval); - - delay(100); -} - -int ProcSonar(int num_samples) { - int raw_val=0; - for(int j=0;j . + +## Help Command Output from Neato + +## Command help + + Help Strlen = 1840 + Help - Without any argument, this prints a list of all possible cmds. + With a command name, it prints the help for that particular command + Clean - Starts a cleaning by simulating press of start button. + DiagTest - Executes different test modes. Once set, press Start button to engage. (Test modes are mutually exclusive.) + GetAccel - Get the Accelerometer readings. + GetAnalogSensors - Get the A2D readings for the analog sensors. + GetButtons - Get the state of the UI Buttons. + GetCalInfo - Prints out the cal info from the System Control Block. + GetCharger - Get the diagnostic data for the charging system. + GetDigitalSensors - Get the state of the digital sensors. + GetErr - Get Error Message. + GetLDSScan - Get scan packet from LDS. + GetMotors - Get the diagnostic data for the motors. + GetSchedule - Get the Cleaning Schedule. (24 hour clock format) + GetTime - Get Current Scheduler Time. + GetVersion - Get the version information for the system software and hardware. + GetWarranty - Get the warranty validation codes. + PlaySound - Play the specified sound in the robot. + RestoreDefaults - Restore user settings to default. + SetFuelGauge - Set Fuel Gauge Level. + SetMotor - Sets the specified motor to run in a direction at a requested speed. (TestMode Only) + SetTime - Sets the current day, hour, and minute for the scheduler clock. + SetLED - Sets the specified LED to on,off,blink, or dim. (TestMode Only) + SetIEC - Sets the IEC Cleaning Test parameters + SetLCD - Sets the LCD to the specified display. (TestMode Only) + SetLDSRotation - Sets LDS rotation on or off. Can only be run in TestMode. + SetSchedule - Modify Cleaning Schedule. + SetSystemMode - Set the operation mode of the robot. (TestMode Only) + TestMode - Sets TestMode on or off. Some commands can only be run in TestMode. + Upload - Uploads new program to the robot. + +  +## Command help Help + + Help Strlen = 262 + Help - Without any argument, this prints a list of all possible cmds. + With a command name, it prints the help for that particular command + Cmd - (Optional) Next argument is command to show help for. + If Cmd option not used, help gives list of all commands. + +  +## Command help Clean + + Help Strlen = 420 + Clean - Starts a cleaning by simulating press of start button. + House - (Optional) Equivalent to pressing 'Start' button once. + Starts a house cleaning. + (House cleaning mode is the default cleaning mode.) + Spot - (Optional) Starts a spot clean. + + Width - (Optional) Spot Width in CM (100-500)(-1=use default). + Height - (Optional) Spot Height in CM (100-500)(-1=use default). + Stop - Stop Cleaning. + + +  +## Command help DiagTest + + Help Strlen = 2472 + DiagTest - Executes different test modes. Once set, press Start button to engage. (Test modes are mutually exclusive.) + TestsOff - Stop Diagnostic Test and clears all diagnostic test modes. + + DrivePath - Sets DrivePath TestMode. Press start button to start. Robot travels straight by commanded distance as path. Mutually exclusive with other diagtest modes. Use 'TestsOff' option to stop. + + DriveForever - Sets DriveForever TestMode. Press start button to start. Robot drives continuously. Mutually exclusive with other diagtest modes. Use 'TestsOff' option to stop. + + MoveAndBump - Sets Move and Bump TestMode. Press start button to start. Executes canned series of motions, but will react to bumps. Mutually exclusive with other diagtest modes. + + DropTest - Enables DropTest. Robot drives forward until a drop is detected. Mutually exclusive with other diagtest modes. + + AutoCycle - DropTest argument to enable automatic restart of the test. The robot will drive backwards and then forward until a drop is detected until the test is over. + + OneShot - Only executes test once. + + BrushOn - Turns on brush during test. May conflict with motor commands of test so use carefully! + + VacuumOn - Turns on vacuum during test. May conflict with motor commands of test so use carefully! + + LDSOn - Turns on LDS during test. May conflict with motor commands of test so use carefully! + + SideBrushOn - Turns on side brush during test. May conflict with motor commands of test so use carefully! + + AllMotorsOn - Turns on brush, vacuum, and lds during test. May conflict with motor commands of test so use carefully! + + DisablePickupDetect - Ignores pickup (wheel suspension). By default, pickup detect is enabled and stops the test. + + DrivePathDist - Distance in mm + + DriveForeverLeftDist - Use next arg to set left wheel dist for DriveForever test. Requires DriveForeverRightDist as well. The ratio of this value to DriveForeverRightDist determines turn radius. + + DriveForeverRightDist - Use next arg to set right wheel dist for DriveForever test. Requires DriveForeverLeftDist as well. The ratio of this value to DriveForeverLeftDist determines turn radius. + + DriveForeverSpeed - Use next arg to set turn speed of outer wheel for DriveForever test in mm/s. + + Speed - DropTest argument to set the robot speed in mm/s. + + BrushSpeed - DropTest argument to set the speed of the brush in rpm. + + +  +## Command help GetAccel + + Help Strlen = 188 + GetAccel - Get the Accelerometer readings. + brief - Returns a single-line summary. Data order: + Revision(0) + Pitch + Roll + X Axis + Y Axis + Z Axis + + +  +## Command help GetAnalogSensors + + Help Strlen = 790 + GetAnalogSensors - Get the A2D readings for the analog sensors. + raw - Return raw analog sensor values as milliVolts. + (Default is sensor values in native units of what they measure.) + mainboard - Return raw analog sensor values as milliVolts from the mainboard. + (Default is sensor values in native units of what they measure.) + stats - Return stats (avg,max,min,dev,cnt) of raw analog sensor values as milliVolts. + (Implies 'raw' option) + brief - Returns a single-line summary of default values. Data order: + Revision(0) + Wall Sensor MM + Battery Voltage mV + Left Drop MM + Right Drop MM + Left Mag + Right Mag + Vacuum Current mA + Charge Voltage mV + Battery Temp 0 C + Battery Temp 1 C + Current mA + + +  +## Command help GetButtons + + Help Strlen = 187 + GetButtons - Get the state of the UI Buttons. + brief - Returns a single-line summary. Data order: + Revision(0) + Soft key + Up + Start + Back + Down + + +  +## Command help GetCalInfo + + Help Strlen = 69 + GetCalInfo - Prints out the cal info from the System Control Block. + +  +## Command help GetCharger + + Help Strlen = 63 + GetCharger - Get the diagnostic data for the charging system. + +  +## Command help GetDigitalSensors + + Help Strlen = 303 + GetDigitalSensors - Get the state of the digital sensors. + brief - Returns a single-line summary. Data order: + Revision(0) + DC Jack + Dustbin + Left Wheel + Right Wheel + Left Side Bumper + Left Front Bumper + Right Side Bumper + Right Front Bumper + + +  +## Command help GetErr + + Help Strlen = 70 + GetErr - Get Error Message. + Clear - Dismiss the reported error. + +  +## Command help GetLDSScan + + Help Strlen = 157 + GetLDSScan - Get scan packet from LDS. + streamOn - Start streaming of raw (binary) LDS readings + + streamOff - Stop streaming of raw LDS readings + + +  +## Command help GetMotors + + Help Strlen = 596 + GetMotors - Get the diagnostic data for the motors. + Brush - Return Brush Motor stats. + Vacuum - Return Vacuum Motor stats. + LeftWheel - Return LeftWheel Motor stats. + RightWheel - Return RightWheel Motor stats. + Laser - Return LDS Motor stats. + Charger - Return Battery Charger stats. + SideBrush - Return Side Brush stats. + brief - Returns a single-line WHEEL ONLY summary. Data order: + Revision(0) + Left Wheel RPM + Left Wheel Position MM + Left Wheel Speed + Right Wheel RPM + Right Wheel Position MM + Right Wheel Speed + + +  +## Command help GetSchedule + + Help Strlen = 170 + GetSchedule - Get the Cleaning Schedule. (24 hour clock format) + Day - Day of the week to get schedule for. Sun=0,Sat=6. + If not specified, then all days are given. + +  +## Command help GetTime + + Help Strlen = 39 + GetTime - Get Current Scheduler Time. + +  +## Command help GetVersion + + Help Strlen = 80 + GetVersion - Get the version information for the system software and hardware. + +  +## Command help GetWarranty + + Help Strlen = 50 + GetWarranty - Get the warranty validation codes. + +  +## Command help PlaySound + + Help Strlen = 810 + PlaySound - Play the specified sound in the robot. + SoundID - Play the sound library entry specified by the number in the next argument. + + Legal values are: + + 0 - Waking Up + 1 - Starting Cleaning + 2 - Cleaning Completed + 3 - Attention Needed + 4 - Backing up into base station + 5 - Base Station Docking Completed + 6 - Test Sound 1 + 7 - Test Sound 2 + 8 - Test Sound 3 + 9 - Test Sound 4 + 10 - Test Sound 5 + 11 - Exploring + 12 - ShutDown + 13 - Picked Up + 14 - Going to sleep + 15 - Returning Home + 16 - User Canceled Cleaning + 17 - User Terminated Cleaning + 18 - Slipped Off Base While Charging + 19 - Alert + 20 - Thank You + + Stop - Stop playing sound. + +  +## Command help RestoreDefaults + + Help Strlen = 53 + RestoreDefaults - Restore user settings to default. + +  +## Command help SetFuelGauge + + Help Strlen = 86 + SetFuelGauge - Set Fuel Gauge Level. + Percent - Fuel Gauge percent from 0 to 100 + +  +## Command help SetMotor + + Help Strlen = 1396 + SetMotor - Sets the specified motor to run in a direction at a requested speed. (TestMode Only) + LWheelDist - Distance in millimeters to drive Left wheel + +/- 10000, pos = forward, neg = backward + RWheelDist - Distance in millimeters to drive Right wheel + +/- 10000, pos = forward, neg = backward + Speed - Speed in millimeters/second + 0-300, required for wheel movements + Accel - Acceleration in millimeters/second, + 0-300, used only for wheel movements, defaults to 'Speed' + RPM - The following argument is the RPM of the motor. + 0-10000, not used for wheels, but applied to all other motors + Brush - Brush motor (Mutually exclusive with wheels and vacuum.) + VacuumOn - Vacuum motor on (Mutually exclusive with VacuumOff) + VacuumOff - Vacuum motor off (Mutually exclusive with VacuumOn) + VacuumSpeed - Vacuum speed in percent (1-100) + RWheelDisable - Disable Right Wheel motor + LWheelDisable - Disable Left Wheel motor + BrushDisable - Disable Brush motor + RWheelEnable - Enable Right Wheel motor + LWheelEnable - Enable Left Wheel motor + BrushEnable - Enable Brush motor + SideBrushEnable - Enable Side Brush Motor motor + SideBrushDisable - Disable Side Brush Motor motor + SideBrushOn - Enable the Side Brush + SideBrushOff - Disable the Side Brush + SideBrushPower - Side Brush maximum power in milliwatts + +  +## Command help SetTime + + Help Strlen = 276 + SetTime - Sets the current day, hour, and minute for the scheduler clock. + Day - Day of week value Sunday=0,Monday=1,... (required) + Hour - Hour value 0..23 (required) + Min - Minutes value 0..59 (required) + Sec - Seconds value 0..59 (Optional, defaults to 0) + +  +## Command help SetLED + + Help Strlen = 822 + SetLED - Sets the specified LED to on,off,blink, or dim. (TestMode Only) + BacklightOn - LCD Backlight On (mutually exclusive of BacklightOff) + BacklightOff - LCD Backlight Off (mutually exclusive of BacklightOn) + ButtonAmber - Start Button Amber (mutually exclusive of other Button options) + ButtonGreen - Start Button Green (mutually exclusive of other Button options) + LEDRed - Start Red LED (mutually exclusive of other Button options) + LEDGreen - Start Green LED (mutually exclusive of other Button options) + ButtonAmberDim - Start Button Amber Dim (mutually exclusive of other Button options) + ButtonGreenDim - Start Button Green Dim (mutually exclusive of other Button options) + ButtonOff - Start Button Off + BlinkOn - Start the LED Blink + BlinkOff - Stop the LED Blink + +  +## Command help SetIEC + + Help Strlen = 318 + SetIEC - Sets the IEC Cleaning Test parameters + FloorSelection - Next Arg is the floor type < carpet | hard > + CarpetSpeed - Next Arg is test speed on carpet (10-300mm/s) + HardSpeed - Next Arg is test speed on hard floors (10-300mm/s) + Distance - Next Arg is test distance (200-4000 mm, default 1200) + +  +## Command help SetLCD + + Help Strlen = 687 + SetLCD - Sets the LCD to the specified display. (TestMode Only) + BGWhite - Fill LCD background with White + BGBlack - Fill LCD background with Black + HLine - Draw a horizontal line (in foreground color) at the following row. + VLine - Draw a vertical line (in foreground color) at the following column. + HBars - Draw alternating horizontal lines (FG,BG,FG,BG,...), + across the whole screen. + VBars - Draw alternating vertical lines (FG,BG,FG,BG,...), + across the whole screen. + FGWhite - Use White as Foreground (line) color + FGBlack - Use Black as Foreground (line) color + Contrast - Set the following value as the LCD Contrast value into NAND. 0..63 + +  +## Command help SetLDSRotation + + Help Strlen = 205 + SetLDSRotation - Sets LDS rotation on or off. Can only be run in TestMode. + On - Turns LDS rotation on. Mutually exclusive with Off. + + Off - Turns LDS rotation off. Mutually exclusive with On. + + +  +## Command help SetSchedule + + Help Strlen = 525 + SetSchedule - Modify Cleaning Schedule. + Day - Day of the week to schedule cleaning for. Sun=0,Sat=6. (required) + Hour - Hour value 0..23 (required) + Min - Minutes value 0..59 (required) + House - Schedule to Clean whole house (default) + (Mutually exclusive with None) + None - Remove Scheduled Cleaning for specified day. Time is ignored. + (Mutually exclusive with House) + ON - Enable Scheduled cleanings (Mutually exclusive with OFF) + OFF - Disable Scheduled cleanings (Mutually exclusive with ON) + +  +## Command help SetSystemMode + + Help Strlen = 392 + SetSystemMode - Set the operation mode of the robot. (TestMode Only) + Shutdown - Shut down the robot. (mutually exclusive of other options) + Hibernate - Start hibernate operation.(mutually exclusive of other options) + Standby - Start standby operation. (mutually exclusive of other options) + PowerCycle - Power cycles the entire system. (mutually exclusive of other options) + +  +## Command help TestMode + + Help Strlen = 201 + TestMode - Sets TestMode on or off. Some commands can only be run in TestMode. + On - Turns Testmode on. Mutually exclusive with Off. + + Off - Turns Testmode off. Mutually exclusive with On. + + +  +## Command help Upload + + Help Strlen = 626 + Upload - Uploads new program to the robot. + code - Upload file is the main application. (Mutually exclusive with sound, LDS) + sound - Upload file is a sound module. (Mutually exclusive with code, LDS) + LDS - Upload file is an LDS module. (Mutually exclusive with sound, code) + mainboard - Upload file is mainboard module. (Mutually exclusive with sound, code and LDS) + size - data size to upload to device. + noburn - test option -- do NOT burn the flash after the upload. + readflash - test option -- read the flash at the current region. + reboot - Reset the robot after performing the upload. + +  +## Command GetAccel + + Label,Value + PitchInDegrees, 1.98 + RollInDegrees, -0.85 + XInG, 0.035 + YInG,-0.015 + ZInG, 1.011 + SumInG, 1.012 +  +## Command GetAnalogSensors + + SensorName,Value + WallSensorInMM,60, + BatteryVoltageInmV,16348, + LeftDropInMM,0, + RightDropInMM,0, + LeftMagSensor,32768, + RightMagSensor,32768, + UIButtonInmV,3330, + VacuumCurrentInmA,0, + ChargeVoltInmV,24024, + BatteryTemp0InC,30, + BatteryTemp1InC,28, + CurrentInmA,40, + SideBrushCurrentInmA,0, + VoltageReferenceInmV,1225, + AccelXInmG,36, + AccelYInmG,-16, + AccelZInmG,1008, +  +## Command GetButtons + + Button Name,Pressed + BTN_SOFT_KEY,0 + BTN_SCROLL_UP,0 + BTN_START,0 + BTN_BACK,0 + BTN_SCROLL_DOWN,0 +  +## Command GetCalInfo + + Parameter,Value + LDSOffset,0 + XAccel,0 + YAccel,0 + ZAccel,0 + RTCOffset,0 + LCDContrast,17 + RDropMin,293 + RDropMid,168 + RDropMax,84 + LDropMin,295 + LDropMid,168 + LDropMax,68 + WallMin,721 + WallMid,265 + WallMax,140 + QAState,0 + CleaningTestSurface,carpet + CleaningTestHardSpeed,200 + CleaningTestCarpetSpeed,100 + CleaningTestHardDistance,1200 + CleaningTestCarpetDistance,1200 +  +## Command GetCharger + + Label,Value + FuelPercent,93 + BatteryOverTemp,0 + ChargingActive,0 + ChargingEnabled,1 + ConfidentOnFuel,0 + OnReservedFuel,0 + EmptyFuel,0 + BatteryFailure,0 + ExtPwrPresent,1 + ThermistorPresent[0],1 + ThermistorPresent[1],1 + BattTempCAvg[0],30 + BattTempCAvg[1],28 + VBattV,16.34 + VExtV,24.03 + Charger_mAH,0 +  +## Command GetDigitalSensors + + Digital Sensor Name, Value + SNSR_DC_JACK_CONNECT,0 + SNSR_DUSTBIN_IS_IN,1 + SNSR_LEFT_WHEEL_EXTENDED,0 + SNSR_RIGHT_WHEEL_EXTENDED,0 + LSIDEBIT,0 + LFRONTBIT,0 + RSIDEBIT,0 + RFRONTBIT,0 +  +## Command GetErr + +## Command GetLDSScan + + AngleInDegrees,DistInMM,Intensity,ErrorCodeHEX + 0,0,0,0 + 1,0,0,0 + 2,0,0,0 + 3,0,0,0 + 4,0,0,0 + 5,0,0,0 + 6,0,0,0 + 7,0,0,0 + 8,0,0,0 + 9,0,0,0 + 10,0,0,0 + 11,0,0,0 + 12,0,0,0 + 13,0,0,0 + 14,0,0,0 + 15,0,0,0 + 16,0,0,0 + 17,0,0,0 + 18,0,0,0 + 19,0,0,0 + 20,0,0,0 + 21,0,0,0 + 22,0,0,0 + 23,0,0,0 + 24,0,0,0 + 25,0,0,0 + 26,0,0,0 + 27,0,0,0 + 28,0,0,0 + 29,0,0,0 + 30,0,0,0 + 31,0,0,0 + 32,0,0,0 + 33,0,0,0 + 34,0,0,0 + 35,0,0,0 + 36,0,0,0 + 37,0,0,0 + 38,0,0,0 + 39,0,0,0 + 40,0,0,0 + 41,0,0,0 + 42,0,0,0 + 43,0,0,0 + 44,0,0,0 + 45,0,0,0 + 46,0,0,0 + 47,0,0,0 + 48,0,0,0 + 49,0,0,0 + 50,0,0,0 + 51,0,0,0 + 52,0,0,0 + 53,0,0,0 + 54,0,0,0 + 55,0,0,0 + 56,0,0,0 + 57,0,0,0 + 58,0,0,0 + 59,0,0,0 + 60,0,0,0 + 61,0,0,0 + 62,0,0,0 + 63,0,0,0 + 64,0,0,0 + 65,0,0,0 + 66,0,0,0 + 67,0,0,0 + 68,0,0,0 + 69,0,0,0 + 70,0,0,0 + 71,0,0,0 + 72,0,0,0 + 73,0,0,0 + 74,0,0,0 + 75,0,0,0 + 76,0,0,0 + 77,0,0,0 + 78,0,0,0 + 79,0,0,0 + 80,0,0,0 + 81,0,0,0 + 82,0,0,0 + 83,0,0,0 + 84,0,0,0 + 85,0,0,0 + 86,0,0,0 + 87,0,0,0 + 88,0,0,0 + 89,0,0,0 + 90,0,0,0 + 91,0,0,0 + 92,0,0,0 + 93,0,0,0 + 94,0,0,0 + 95,0,0,0 + 96,0,0,0 + 97,0,0,0 + 98,0,0,0 + 99,0,0,0 + 100,0,0,0 + 101,0,0,0 + 102,0,0,0 + 103,0,0,0 + 104,0,0,0 + 105,0,0,0 + 106,0,0,0 + 107,0,0,0 + 108,0,0,0 + 109,0,0,0 + 110,0,0,0 + 111,0,0,0 + 112,0,0,0 + 113,0,0,0 + 114,0,0,0 + 115,0,0,0 + 116,0,0,0 + 117,0,0,0 + 118,0,0,0 + 119,0,0,0 + 120,0,0,0 + 121,0,0,0 + 122,0,0,0 + 123,0,0,0 + 124,0,0,0 + 125,0,0,0 + 126,0,0,0 + 127,0,0,0 + 128,0,0,0 + 129,0,0,0 + 130,0,0,0 + 131,0,0,0 + 132,0,0,0 + 133,0,0,0 + 134,0,0,0 + 135,0,0,0 + 136,0,0,0 + 137,0,0,0 + 138,0,0,0 + 139,0,0,0 + 140,0,0,0 + 141,0,0,0 + 142,0,0,0 + 143,0,0,0 + 144,0,0,0 + 145,0,0,0 + 146,0,0,0 + 147,0,0,0 + 148,0,0,0 + 149,0,0,0 + 150,0,0,0 + 151,0,0,0 + 152,0,0,0 + 153,0,0,0 + 154,0,0,0 + 155,0,0,0 + 156,0,0,0 + 157,0,0,0 + 158,0,0,0 + 159,0,0,0 + 160,0,0,0 + 161,0,0,0 + 162,0,0,0 + 163,0,0,0 + 164,0,0,0 + 165,0,0,0 + 166,0,0,0 + 167,0,0,0 + 168,0,0,0 + 169,0,0,0 + 170,0,0,0 + 171,0,0,0 + 172,0,0,0 + 173,0,0,0 + 174,0,0,0 + 175,0,0,0 + 176,0,0,0 + 177,0,0,0 + 178,0,0,0 + 179,0,0,0 + 180,0,0,0 + 181,0,0,0 + 182,0,0,0 + 183,0,0,0 + 184,0,0,0 + 185,0,0,0 + 186,0,0,0 + 187,0,0,0 + 188,0,0,0 + 189,0,0,0 + 190,0,0,0 + 191,0,0,0 + 192,0,0,0 + 193,0,0,0 + 194,0,0,0 + 195,0,0,0 + 196,0,0,0 + 197,0,0,0 + 198,0,0,0 + 199,0,0,0 + 200,0,0,0 + 201,0,0,0 + 202,0,0,0 + 203,0,0,0 + 204,0,0,0 + 205,0,0,0 + 206,0,0,0 + 207,0,0,0 + 208,0,0,0 + 209,0,0,0 + 210,0,0,0 + 211,0,0,0 + 212,0,0,0 + 213,0,0,0 + 214,0,0,0 + 215,0,0,0 + 216,0,0,0 + 217,0,0,0 + 218,0,0,0 + 219,0,0,0 + 220,0,0,0 + 221,0,0,0 + 222,0,0,0 + 223,0,0,0 + 224,0,0,0 + 225,0,0,0 + 226,0,0,0 + 227,0,0,0 + 228,0,0,0 + 229,0,0,0 + 230,0,0,0 + 231,0,0,0 + 232,0,0,0 + 233,0,0,0 + 234,0,0,0 + 235,0,0,0 + 236,0,0,0 + 237,0,0,0 + 238,0,0,0 + 239,0,0,0 + 240,0,0,0 + 241,0,0,0 + 242,0,0,0 + 243,0,0,0 + 244,0,0,0 + 245,0,0,0 + 246,0,0,0 + 247,0,0,0 + 248,0,0,0 + 249,0,0,0 + 250,0,0,0 + 251,0,0,0 + 252,0,0,0 + 253,0,0,0 + 254,0,0,0 + 255,0,0,0 + 256,0,0,0 + 257,0,0,0 + 258,0,0,0 + 259,0,0,0 + 260,0,0,0 + 261,0,0,0 + 262,0,0,0 + 263,0,0,0 + 264,0,0,0 + 265,0,0,0 + 266,0,0,0 + 267,0,0,0 + 268,0,0,0 + 269,0,0,0 + 270,0,0,0 + 271,0,0,0 + 272,0,0,0 + 273,0,0,0 + 274,0,0,0 + 275,0,0,0 + 276,0,0,0 + 277,0,0,0 + 278,0,0,0 + 279,0,0,0 + 280,0,0,0 + 281,0,0,0 + 282,0,0,0 + 283,0,0,0 + 284,0,0,0 + 285,0,0,0 + 286,0,0,0 + 287,0,0,0 + 288,0,0,0 + 289,0,0,0 + 290,0,0,0 + 291,0,0,0 + 292,0,0,0 + 293,0,0,0 + 294,0,0,0 + 295,0,0,0 + 296,0,0,0 + 297,0,0,0 + 298,0,0,0 + 299,0,0,0 + 300,0,0,0 + 301,0,0,0 + 302,0,0,0 + 303,0,0,0 + 304,0,0,0 + 305,0,0,0 + 306,0,0,0 + 307,0,0,0 + 308,0,0,0 + 309,0,0,0 + 310,0,0,0 + 311,0,0,0 + 312,0,0,0 + 313,0,0,0 + 314,0,0,0 + 315,0,0,0 + 316,0,0,0 + 317,0,0,0 + 318,0,0,0 + 319,0,0,0 + 320,0,0,0 + 321,0,0,0 + 322,0,0,0 + 323,0,0,0 + 324,0,0,0 + 325,0,0,0 + 326,0,0,0 + 327,0,0,0 + 328,0,0,0 + 329,0,0,0 + 330,0,0,0 + 331,0,0,0 + 332,0,0,0 + 333,0,0,0 + 334,0,0,0 + 335,0,0,0 + 336,0,0,0 + 337,0,0,0 + 338,0,0,0 + 339,0,0,0 + 340,0,0,0 + 341,0,0,0 + 342,0,0,0 + 343,0,0,0 + 344,0,0,0 + 345,0,0,0 + 346,0,0,0 + 347,0,0,0 + 348,0,0,0 + 349,0,0,0 + 350,0,0,0 + 351,0,0,0 + 352,0,0,0 + 353,0,0,0 + 354,0,0,0 + 355,0,0,0 + 356,0,0,0 + 357,0,0,0 + 358,0,0,0 + 359,0,0,0 + ROTATION_SPEED,0.00 +  +## Command GetMotors + + Parameter,Value + Brush_RPM,0 + Brush_mA,0 + Vacuum_RPM,0 + Vacuum_mA,0 + LeftWheel_RPM,0 + LeftWheel_Load%,0 + LeftWheel_PositionInMM,0 + LeftWheel_Speed,0 + RightWheel_RPM,0 + RightWheel_Load%,0 + RightWheel_PositionInMM,0 + RightWheel_Speed,0 + Charger_mAH, 0 + SideBrush_mA,0 +  +## Command GetSchedule + + Schedule is Disabled + Sun 00:00 - None - + Mon 08:15 H + Tue 00:00 - None - + Wed 08:15 H + Thu 00:00 - None - + Fri 08:15 H + Sat 00:00 - None - +  +## Command GetTime + + Sunday 6:28:22 +  +## Command GetVersion + + Component,Major,Minor,Build + ModelID,-1,XV28, + ConfigID,1,, + Software,3,4,24079 + BatteryType,1,NIMH_12CELL, + BlowerType,1,BLOWER_ORIG, + BrushSpeed,1200,, + BrushMotorType,1,BRUSH_MOTOR_ORIG, + SideBrushType,1,SIDE_BRUSH_NONE, + WheelPodType,1,WHEEL_POD_ORIG, + DropSensorType,1,DROP_SENSOR_ORIG, + MagSensorType,1,MAG_SENSOR_ORIG, + WallSensorType,1,WALL_SENSOR_ORIG, + Locale,1,LOCALE_USA, + LDS Software,V2.6.15295,0000000000, + LDS CPU,F2802x/c001,, + MainBoard Vendor ID,505,, + BootLoader Software,18119,P,p + MainBoard Software,23179,1, + MainBoard Boot,16219, + MainBoard Version,4,0, + ChassisRev,2,, + UIPanelRev,1,, +  +## Command GetWarranty + + 000ae7e8 + 0123 + cc2dca46 +  \ No newline at end of file diff --git a/scripts/power_off_neato.py b/scripts/power_off_neato.py new file mode 100644 index 0000000..58363ff --- /dev/null +++ b/scripts/power_off_neato.py @@ -0,0 +1,17 @@ +# Script will turn off neato. + +# Author: Brannon Vann - brannon.vann@gmail.com +# License: MIT + +# Run this script: python stop_neato_lidar.py + +import serial + +try: + serial = serial.Serial('/dev/ttyACM0', timeout=1) + serial.write('SetSystemMode Shutdown\n') + # close serial port + serial.close() + print("Done shutting Down Neato") +except: + print("An error occurred while shuting down neato.") diff --git a/scripts/stop_neato.py b/scripts/stop_neato.py new file mode 100644 index 0000000..b50c5b9 --- /dev/null +++ b/scripts/stop_neato.py @@ -0,0 +1,23 @@ +# Script will stop neato lidar. +# If the ros neato driver fails the lidar continues to +# spin. This scrip is used to stop the spinning, especially +# during development. + +# Author: Brannon Vann brannon.vann@gmail.com +# License: MIT + +# Run this script: python stop_neato_lidar.py + +import serial + +try: + serial = serial.Serial('/dev/ttyACM0', timeout=1) + serial.write('setldsrotation off\n') + serial.write('setled buttonoff\n') + serial.write('testmode off\n') + + # close serial port + serial.close() + print("Done stopping down neato.") +except: + print("an error occurred while stoping neato lidar.")