├── .DS_Store ├── Learning Robotics Using Python_code.zip ├── README.md ├── arduino ├── chefbot │ ├── .DS_Store │ └── chefbot.ino ├── imu │ ├── .DS_Store │ ├── chefbot │ │ └── chefbot.ino │ ├── i2c_scanner │ │ └── i2c_scanner.ino │ ├── imu.ino │ └── imu_with_dmp │ │ └── imu_with_dmp.ino ├── imu_test │ └── imu_test.ino ├── libs │ ├── .DS_Store │ ├── I2Cdev │ │ ├── I2Cdev.cpp │ │ ├── I2Cdev.h │ │ ├── keywords.txt │ │ └── library.json │ ├── MPU6050 │ │ ├── MPU6050.cpp │ │ ├── MPU6050.h │ │ ├── MPU6050_6Axis_MotionApps20.h │ │ ├── MPU6050_9Axis_MotionApps41.h │ │ ├── examples │ │ │ ├── IMU_Zero │ │ │ │ └── IMU_Zero.ino │ │ │ ├── MPU6050_DMP6 │ │ │ │ ├── MPU6050_DMP6.ino │ │ │ │ └── Processing │ │ │ │ │ └── MPUTeapot │ │ │ │ │ └── MPUTeapot.pde │ │ │ ├── MPU6050_DMP6_ESPWiFi │ │ │ │ ├── MPU6050_DMP6_ESPWiFi.ino │ │ │ │ └── Processing │ │ │ │ │ └── MPUOSCTeapot │ │ │ │ │ └── MPUOSCTeapot.pde │ │ │ ├── MPU6050_DMP6_Ethernet │ │ │ │ └── MPU6050_DMP6_Ethernet.ino │ │ │ └── MPU6050_raw │ │ │ │ └── MPU6050_raw.ino │ │ ├── helper_3dmath.h │ │ └── library.json │ ├── Messenger │ │ ├── .DS_Store │ │ ├── Messenger.cpp │ │ └── Messenger.h │ └── PinChangeInterrupt │ │ ├── Readme.md │ │ ├── examples │ │ ├── PinChangeInterrupt_HowItWorks │ │ │ └── PinChangeInterrupt_HowItWorks.ino │ │ ├── PinChangeInterrupt_Led │ │ │ └── PinChangeInterrupt_Led.ino │ │ ├── PinChangeInterrupt_LowLevel │ │ │ └── PinChangeInterrupt_LowLevel.ino │ │ └── PinChangeInterrupt_TickTock │ │ │ └── PinChangeInterrupt_TickTock.ino │ │ ├── header.png │ │ ├── keywords.txt │ │ ├── library.properties │ │ └── src │ │ ├── PinChangeInterrupt.cpp │ │ ├── PinChangeInterrupt.h │ │ ├── PinChangeInterrupt0.cpp │ │ ├── PinChangeInterrupt1.cpp │ │ ├── PinChangeInterrupt2.cpp │ │ ├── PinChangeInterrupt3.cpp │ │ ├── PinChangeInterruptBoards.h │ │ ├── PinChangeInterruptPins.h │ │ └── PinChangeInterruptSettings.h ├── simple_encoder_test │ └── simple_encoder_test.ino ├── simple_motor_test │ └── simple_motor_test.ino └── ultrasonic_sensor │ └── ultrasonic_sensor.ino ├── chefbot ├── chefbot_bringup │ ├── CMakeLists.txt │ ├── launch │ │ ├── 3dsensor.launch │ │ ├── amcl_demo.launch │ │ ├── gmapping_demo.launch │ │ ├── includes │ │ │ ├── amcl.launch.xml │ │ │ ├── gmapping.launch.xml │ │ │ ├── gmapping.launch.xml~ │ │ │ ├── move_base.launch.xml │ │ │ ├── move_base.launch.xml~ │ │ │ ├── safety_controller.launch.xml │ │ │ └── velocity_smoother.launch.xml │ │ ├── keyboard_teleop.launch │ │ ├── model_robot.launch │ │ ├── robot_standalone.launch │ │ ├── view_navigation.launch │ │ └── view_robot.launch │ ├── map │ │ ├── hotel1.pgm │ │ └── hotel1.yaml │ ├── package.xml │ ├── param │ │ ├── .svn │ │ │ ├── all-wcprops │ │ │ ├── entries │ │ │ └── text-base │ │ │ │ ├── ardros.yaml.svn-base │ │ │ │ ├── base_local_planner_params.yaml.svn-base │ │ │ │ ├── costmap_common_params.yaml.svn-base │ │ │ │ ├── global_costmap_params.yaml.svn-base │ │ │ │ ├── joystick.yaml.svn-base │ │ │ │ ├── local_costmap_params.yaml.svn-base │ │ │ │ └── teleop.yaml.svn-base │ │ ├── ardros.yaml │ │ ├── base_local_planner_params.yaml │ │ ├── board_config.yaml │ │ ├── costmap_common_params.yaml │ │ ├── dwa_local_planner_params.yaml │ │ ├── encoders.yaml │ │ ├── global_costmap_params.yaml │ │ ├── joystick.yaml │ │ ├── local_costmap_params.yaml │ │ ├── move_base_params.yaml │ │ ├── mux.yaml │ │ ├── serial.yaml │ │ └── teleop.yaml │ ├── rviz │ │ ├── model.rviz │ │ ├── navigation.rviz │ │ └── robot.rviz │ ├── scripts │ │ ├── SerialDataGateway.py │ │ ├── SerialDataGateway.pyc │ │ ├── bkup_working │ │ │ ├── DeadReckoning.py │ │ │ ├── GoalsSequencer.py │ │ │ ├── SerialDataGateway.pyc │ │ │ ├── arduino (copy).py │ │ │ ├── arduino.py │ │ │ ├── arduino.py_bkup │ │ │ ├── arduino_bkup.py │ │ │ ├── base_controller.py │ │ │ ├── err │ │ │ ├── launchpad_node (copy).py │ │ │ ├── launchpad_process_node (copy).py │ │ │ ├── launchpad_process_node.py │ │ │ ├── launchpad_process_node.py_edited_not_working │ │ │ ├── launchpad_robot_teleop_key │ │ │ ├── lpid_velocity.py │ │ │ ├── murray_demo.py │ │ │ ├── murray_demo_test.py │ │ │ ├── pid_velocity_sim.py │ │ │ ├── rpid_velocity.py │ │ │ ├── velocityLogger.py │ │ │ ├── velocityLogger.pyc │ │ │ ├── virtual_joystick.py │ │ │ ├── wheel_loopback.py │ │ │ └── wheel_scaler.py │ │ ├── chefbot_teleop_key │ │ ├── diff_tf.py │ │ ├── launchpad_node.py │ │ ├── pid_velocity.py │ │ ├── robot_gui.py │ │ ├── simple_navig_goals.py │ │ └── twist_to_motors.py │ └── src │ │ └── send_robot_goal.cpp ├── chefbot_description │ ├── CMakeLists.txt │ ├── meshes │ │ ├── base_plate.dae │ │ ├── create_body.tga │ │ └── wheel.dae │ ├── package.xml │ ├── robots │ │ ├── chefbot_circles_kinect.urdf.xacro │ │ └── roomba_circles_kinect.urdf.xacro │ └── urdf │ │ ├── bkup │ │ ├── chefbot_base.urdf.xacro │ │ ├── chefbot_base_gazebo.urdf.xacro │ │ ├── chefbot_gazebo.urdf.xacro │ │ ├── chefbot_library.urdf.xacro │ │ ├── chefbot_properties.urdf.xacro │ │ └── common_properties.urdf.xacro │ │ ├── chefbot_base.urdf.xacro │ │ ├── chefbot_base.xacro │ │ ├── chefbot_base_gazebo.urdf.xacro │ │ ├── chefbot_gazebo.urdf.xacro │ │ ├── chefbot_library.urdf.xacro │ │ ├── chefbot_properties.urdf.xacro │ │ ├── chefbot_rviz_base.urdf.xacro │ │ ├── chefbot_rviz_library.urdf.xacro │ │ ├── common_properties.urdf.xacro │ │ └── sensors │ │ ├── asus_xtion_pro.urdf.xacro │ │ └── kinect.urdf.xacro └── chefbot_simulator │ └── chefbot_gazebo │ ├── CMakeLists.txt │ ├── launch │ ├── amcl_demo.launch │ ├── chefbot_empty_world.launch │ ├── chefbot_hotel_world.launch │ ├── chefbot_playground.launch │ ├── chefbot_room_world.launch │ ├── gmapping_demo.launch │ └── includes │ │ ├── chefbot_base.launch.xml │ │ └── chefbot_base.launch.xml~ │ ├── maps │ ├── playground.pgm │ ├── playground.yaml │ ├── room.pgm │ └── room.yaml │ ├── package.xml │ └── worlds │ ├── complete_hotel.sdf │ ├── empty.world │ ├── empty_.world │ ├── original │ └── empty.world │ ├── playground.world │ └── test_room.sdf └── tiva_c_energia_code_final ├── MPU6050.zip ├── Messenger.zip └── new_sensor_sketch_with_dmp.ino /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/.DS_Store -------------------------------------------------------------------------------- /Learning Robotics Using Python_code.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/Learning Robotics Using Python_code.zip -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Learning Robotics using Python 2 | 3 | #### [Learning Robotics using Python](http://learn-robotics.com) book tutorials source code 4 | ![book_cover](http://learn-robotics.com/images/section-image-1.jpg 5 | "Learning Robotics using Python") 6 | 7 | ### Buy book 8 | 9 | * [PACKT](https://www.packtpub.com/application-development/learning-robotics-using-python) 10 | * [Amazon.com](http://amzn.com/1783287535) 11 | * [Amazon.in](http://www.amazon.in/dp/B00YEVZ6UK) 12 | 13 | ### Author 14 | 15 | * [Lentin Joseph](https://in.linkedin.com/in/lentinjoseph) 16 | 17 | ### Installation 18 | The code is comaptible with ROS Jade and ROS Indigo. The detail installation instruction of each packages is mentioned on the book 19 | 20 | ### Tutorials 21 | * **Chapter 1**: Introduction to Robotics 22 | * **Chapter 2**: Mechanical design of a service Robot 23 | * **Chapter 3**: Working with Robot Simulation using ROS and Gazebo 24 | * **Chapter 4**: Designing Chefbot Hardware 25 | * **Chapter 5**: Working with Robotic Actuators and Wheel Encoders 26 | * **Chapter 6**: Working with Robotic Sensors 27 | * **Chapter 7**: Programming Vision Sensors using Python and ROS 28 | * **Chapter 8**: Working with Speech Recognition and Synthesis using Python and ROS 29 | * **Chapter 9**: Applying Artificial Intelligence to Chefbot using Python 30 | * **Chapter 10**: Integration of Chefbot hardware and interfacing it into ROS, using Python 31 | * **Chapter 11**: Designing a GUI for a robot using QT and Python 32 | * **Chapter 12**: The calibration and testing of Chefbot 33 | 34 | 35 | # ROS Robotics Projects 36 | 37 | #### [ROS Robotics Projects](http://rosrobots.com) 38 | ![book_cover](http://rosrobots.com/img/ebook.png 39 | "ROS Robotics Projects") 40 | 41 | ### Buy book 42 | 43 | * [PACKT](https://www.packtpub.com/hardware-and-creative/ros-robotics-projects) 44 | * [Amazon.com](https://www.amazon.com/ROS-Robotic-Projects-Lentin-Joseph/dp/1783554711) 45 | * [Amazon.in](https://www.amazon.in/ROS-Robotics-Projects-Lentin-Joseph-ebook/dp/B01MTJWNKI) 46 | 47 | 48 | ### Author 49 | 50 | * [Lentin Joseph](https://in.linkedin.com/in/lentinjoseph) 51 | 52 | ### Installation 53 | The code is comaptible with ROS Kinetic and ROS Indigo. The detail installation instruction of each packages is mentioned on the book 54 | 55 | ### Tutorials 56 | * **Chapter 1:** Getting Started with ROS Robotics Application Development 57 | * **Chapter 2**: Face Detection and Tracking Using ROS, OpenCV and Dynamixel Servos 58 | * **Chapter 3**: Building a Siri-Like Chatbot in ROS 59 | * **Chapter 4**: Controlling Embedded Boards Using ROS 60 | * **Chapter 5**: Teleoperate a Robot Using Hand Gestures 61 | * **Chapter 6**: Object Detection and Recognition 62 | * **Chapter 7**: Deep Learning Using ROS and TensorFlow 63 | * **Chapter 8**: ROS on MATLAB and Android 64 | * **Chapter 9**: Building an Autonomous Mobile Robot 65 | * **Chapter 10**: Creating a Self-driving Car Using ROS! 66 | * **Chapter 11**: Teleoperating Robot Using VR Headset and Leap Motion 67 | * **Chapter 12**: Controlling Your Robots over the Web 68 | 69 | 70 | 71 | # Mastering ROS for Robotics Programming 72 | 73 | #### [Mastering ROS for Robotics Programming](http://mastering-ros.com) book tutorials source code 74 | ![book_cover](http://mastering-ros.com/images/section-image-1.jpg 75 | "Mastering ROS for Robotics Programming") 76 | 77 | ### Buy book 78 | 79 | * [PACKT](https://www.packtpub.com/hardware-and-creative/mastering-ros-robotics-programming) 80 | * [Amazon.com](http://amzn.com/B0198DXFEW) 81 | * [Amazon.in](http://www.amazon.in/dp/B0198DXFEW) 82 | 83 | 84 | ### Author 85 | 86 | * [Lentin Joseph](https://in.linkedin.com/in/lentinjoseph) 87 | 88 | ### Installation 89 | The code is comaptible with ROS Jade and ROS Indigo. The detail installation instruction of each packages is mentioned on the book 90 | 91 | ### Tutorials 92 | * **Chapter 1:** Introduction to ROS and its Package Management 93 | * **Chapter 2**: Working with 3D Robot Modeling in ROS 94 | * **Chapter 3**: Simulating Robots Using ROS and Gazebo 95 | * **Chapter 4**: Using ROS MoveIt! and Navigation stack 96 | * **Chapter 5**: Working with Pluginlib, Nodelets and Gazebo plugins 97 | * **Chapter 6**: Writing ROS Controllers and Visualization plugins 98 | * **Chapter 7**: Interfacing I/O boards, sensors and actuators to ROS 99 | * **Chapter 8**: Programming Vision sensors using ROS, Open-CV and PCL 100 | * **Chapter 9**: Building and interfacing a differential drive mobile robot hardware in ROS 101 | * **Chapter 10**: Exploring advanced capabilities of ROS-MoveIt! 102 | * **Chapter 11**: ROS for Industrial Robots 103 | * **Chapter 12**: Troubleshooting and best practices in ROS 104 | -------------------------------------------------------------------------------- /arduino/chefbot/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/arduino/chefbot/.DS_Store -------------------------------------------------------------------------------- /arduino/imu/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/arduino/imu/.DS_Store -------------------------------------------------------------------------------- /arduino/imu/i2c_scanner/i2c_scanner.ino: -------------------------------------------------------------------------------- 1 | // -------------------------------------- 2 | // i2c_scanner 3 | // 4 | // Version 1 5 | // This program (or code that looks like it) 6 | // can be found in many places. 7 | // For example on the Arduino.cc forum. 8 | // The original author is not know. 9 | // Version 2, Juni 2012, Using Arduino 1.0.1 10 | // Adapted to be as simple as possible by Arduino.cc user Krodal 11 | // Version 3, Feb 26 2013 12 | // V3 by louarnold 13 | // Version 4, March 3, 2013, Using Arduino 1.0.3 14 | // by Arduino.cc user Krodal. 15 | // Changes by louarnold removed. 16 | // Scanning addresses changed from 0...127 to 1...119, 17 | // according to the i2c scanner by Nick Gammon 18 | // http://www.gammon.com.au/forum/?id=10896 19 | // Version 5, March 28, 2013 20 | // As version 4, but address scans now to 127. 21 | // A sensor seems to use address 120. 22 | // Version 6, November 27, 2015. 23 | // Added waiting for the Leonardo serial communication. 24 | // 25 | // 26 | // This sketch tests the standard 7-bit addresses 27 | // Devices with higher bit address might not be seen properly. 28 | // 29 | 30 | #include 31 | 32 | 33 | void setup() 34 | { 35 | Wire.begin(); 36 | 37 | Serial.begin(9600); 38 | while (!Serial); // Leonardo: wait for serial monitor 39 | Serial.println("\nI2C Scanner"); 40 | } 41 | 42 | 43 | void loop() 44 | { 45 | byte error, address; 46 | int nDevices; 47 | 48 | Serial.println("Scanning..."); 49 | 50 | nDevices = 0; 51 | for(address = 1; address < 127; address++ ) 52 | { 53 | // The i2c_scanner uses the return value of 54 | // the Write.endTransmisstion to see if 55 | // a device did acknowledge to the address. 56 | Wire.beginTransmission(address); 57 | error = Wire.endTransmission(); 58 | 59 | if (error == 0) 60 | { 61 | Serial.print("I2C device found at address 0x"); 62 | if (address<16) 63 | Serial.print("0"); 64 | Serial.print(address,HEX); 65 | Serial.println(" !"); 66 | 67 | nDevices++; 68 | } 69 | else if (error==4) 70 | { 71 | Serial.print("Unknown error at address 0x"); 72 | if (address<16) 73 | Serial.print("0"); 74 | Serial.println(address,HEX); 75 | } 76 | } 77 | if (nDevices == 0) 78 | Serial.println("No I2C devices found\n"); 79 | else 80 | Serial.println("done\n"); 81 | 82 | delay(5000); // wait 5 seconds for next scan 83 | } 84 | -------------------------------------------------------------------------------- /arduino/imu/imu.ino: -------------------------------------------------------------------------------- 1 | /* Service robot code, sensor only version 2 | 3 | */ 4 | //MPU 6050 Interfacing libraries 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | MPU6050 accelgyro(0x68); 11 | 12 | void setup() 13 | { 14 | 15 | //Init Serial port with 115200 buad rate 16 | Serial.begin(115200); 17 | Setup_MPU6050(); 18 | } 19 | 20 | void Setup_MPU6050() 21 | { 22 | Wire.begin(); 23 | 24 | // initialize device 25 | Serial.println("Initializing I2C devices..."); 26 | accelgyro.initialize(); 27 | 28 | // verify connection 29 | Serial.println("Testing device connections..."); 30 | Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 31 | 32 | } 33 | 34 | void loop() 35 | { 36 | 37 | //Update MPU 6050 38 | Update_MPU6050(); 39 | 40 | } 41 | 42 | void Update_MPU6050() 43 | { 44 | 45 | int16_t ax, ay, az; 46 | int16_t gx, gy, gz; 47 | 48 | // read raw accel/gyro measurements from device 49 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 50 | 51 | // display tab-separated accel/gyro x/y/z values 52 | Serial.print("i");Serial.print("\t"); 53 | Serial.print(ax); Serial.print("\t"); 54 | Serial.print(ay); Serial.print("\t"); 55 | Serial.print(az); Serial.print("\t"); 56 | Serial.print(gx); Serial.print("\t"); 57 | Serial.print(gy); Serial.print("\t"); 58 | Serial.println(gz); 59 | Serial.print("\n"); 60 | 61 | } 62 | 63 | 64 | -------------------------------------------------------------------------------- /arduino/imu/imu_with_dmp/imu_with_dmp.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Wire.h" 4 | #include "I2Cdev.h" 5 | #include "MPU6050_6Axis_MotionApps20.h" 6 | 7 | //Creating MPU6050 Object 8 | MPU6050 accelgyro(0x68); 9 | 10 | //define interrupt pins 11 | #define PUSH2 13 12 | 13 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 14 | //DMP options 15 | //Set true if DMP init was successful 16 | bool dmpReady = false; 17 | //Holds actual interrupt status byte from MPU 18 | uint8_t mpuIntStatus; 19 | //return status after each device operation 20 | uint8_t devStatus; 21 | //Expected DMP packet size 22 | uint16_t packetSize; 23 | //count of all bytes currently in FIFO 24 | uint16_t fifoCount; 25 | //FIFO storate buffer 26 | uint8_t fifoBuffer[64]; 27 | 28 | 29 | #define OUTPUT_READABLE_QUATERNION 30 | 31 | //////////////////////////////////////////////////////////////////////////////////////////////// 32 | 33 | //orientation/motion vars 34 | 35 | Quaternion q; 36 | //VectorInt16 aa; 37 | //VectorInt16 aaReal; 38 | //VectorInt16 aaWorld; 39 | //VectorFloat gravity; 40 | 41 | //float euler[3]; 42 | //float ypr[3]; 43 | 44 | //INTERRUPT DETECTION ROUTINE 45 | 46 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 47 | void dmpDataReady() { 48 | mpuInterrupt = true; 49 | } 50 | 51 | 52 | void setup() 53 | { 54 | //Init Serial port with 115200 buad rate 55 | Serial.begin(115200); 56 | Setup_MPU6050(); 57 | } 58 | 59 | void Setup_MPU6050() 60 | { 61 | Wire.begin(); 62 | // initialize device 63 | accelgyro.initialize(); 64 | 65 | // verify connection 66 | Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 67 | 68 | //Initialize DMP in MPU 6050 69 | Setup_MPU6050_DMP(); 70 | } 71 | 72 | //Setup MPU 6050 DMP 73 | void Setup_MPU6050_DMP() 74 | { 75 | //DMP Initialization 76 | devStatus = accelgyro.dmpInitialize(); 77 | accelgyro.setXGyroOffset(220); 78 | accelgyro.setXGyroOffset(76); 79 | accelgyro.setXGyroOffset(-85); 80 | accelgyro.setXGyroOffset(1788); 81 | if(devStatus == 0){ 82 | accelgyro.setDMPEnabled(true); 83 | attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PUSH2), dmpDataReady, RISING); 84 | mpuIntStatus = accelgyro.getIntStatus(); 85 | dmpReady = true; 86 | packetSize = accelgyro.dmpGetFIFOPacketSize(); 87 | } 88 | else{ 89 | ; 90 | } 91 | 92 | 93 | } 94 | 95 | void loop() 96 | { 97 | 98 | //Update MPU 6050 99 | Update_MPU6050(); 100 | } 101 | void Update_MPU6050() 102 | { 103 | 104 | Update_MPU6050_DMP(); 105 | 106 | } 107 | 108 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 109 | //Update MPU6050 DMP functions 110 | 111 | void Update_MPU6050_DMP() 112 | { 113 | //DMP Processing 114 | if (!dmpReady) return; 115 | int lastPrint = millis(); 116 | while (!mpuInterrupt && fifoCount < packetSize) { 117 | mpuInterrupt = true; 118 | // ; 119 | 120 | } 121 | 122 | 123 | 124 | mpuInterrupt = false; 125 | mpuIntStatus = accelgyro.getIntStatus(); 126 | 127 | //get current FIFO count 128 | fifoCount = accelgyro.getFIFOCount(); 129 | 130 | if ((mpuIntStatus & 0x10) || fifoCount > 512) { 131 | // reset so we can continue cleanly 132 | accelgyro.resetFIFO(); 133 | } 134 | 135 | else if (mpuIntStatus & 0x02) { 136 | // wait for correct available data length, should be a VERY short wait 137 | while (fifoCount < packetSize) fifoCount = accelgyro.getFIFOCount(); 138 | 139 | // read a packet from FIFO 140 | accelgyro.getFIFOBytes(fifoBuffer, packetSize); 141 | 142 | // track FIFO count here in case there is > 1 packet available 143 | // (this lets us immediately read more without waiting for an interrupt) 144 | fifoCount -= packetSize; 145 | 146 | #ifdef OUTPUT_READABLE_QUATERNION 147 | // display quaternion values in easy matrix form: w x y z 148 | accelgyro.dmpGetQuaternion(&q, fifoBuffer); 149 | 150 | 151 | Serial.print("i");Serial.print("\t"); 152 | Serial.print(q.x); Serial.print("\t"); 153 | Serial.print(q.y); Serial.print("\t"); 154 | Serial.print(q.z); Serial.print("\t"); 155 | Serial.print(q.w); 156 | Serial.print("\n"); 157 | 158 | #endif 159 | } 160 | } 161 | 162 | 163 | -------------------------------------------------------------------------------- /arduino/libs/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/arduino/libs/.DS_Store -------------------------------------------------------------------------------- /arduino/libs/I2Cdev/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For I2Cdev 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | I2Cdev KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | 14 | readBit KEYWORD2 15 | readBitW KEYWORD2 16 | readBits KEYWORD2 17 | readBitsW KEYWORD2 18 | readByte KEYWORD2 19 | readBytes KEYWORD2 20 | readWord KEYWORD2 21 | readWords KEYWORD2 22 | writeBit KEYWORD2 23 | writeBitW KEYWORD2 24 | writeBits KEYWORD2 25 | writeBitsW KEYWORD2 26 | writeByte KEYWORD2 27 | writeBytes KEYWORD2 28 | writeWord KEYWORD2 29 | writeWords KEYWORD2 30 | 31 | ####################################### 32 | # Instances (KEYWORD2) 33 | ####################################### 34 | 35 | ####################################### 36 | # Constants (LITERAL1) 37 | ####################################### 38 | 39 | -------------------------------------------------------------------------------- /arduino/libs/I2Cdev/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-Core", 3 | "keywords": "i2cdevlib, i2c", 4 | "description": "The I2C Device Library (I2Cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices.", 5 | "include": "Arduino/I2Cdev", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "frameworks": "arduino", 12 | "platforms": "atmelavr" 13 | } 14 | -------------------------------------------------------------------------------- /arduino/libs/MPU6050/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-MPU6050", 3 | "keywords": "gyroscope, accelerometer, sensor, i2cdevlib, i2c", 4 | "description": "The MPU6050 combines a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with an onboard Digital Motion Processor(DMP) which processes complex 6-axis MotionFusion algorithms", 5 | "include": "Arduino/MPU6050", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "dependencies": 12 | { 13 | "name": "I2Cdevlib-Core", 14 | "frameworks": "arduino" 15 | }, 16 | "frameworks": "arduino", 17 | "platforms": "atmelavr" 18 | } 19 | -------------------------------------------------------------------------------- /arduino/libs/Messenger/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/arduino/libs/Messenger/.DS_Store -------------------------------------------------------------------------------- /arduino/libs/Messenger/Messenger.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | //ADDED FOR COMPATIBILITY WITH WIRING 4 | extern "C" { 5 | #include 6 | } 7 | 8 | #include "Arduino.h" 9 | #include "Messenger.h" 10 | 11 | 12 | 13 | 14 | Messenger::Messenger() 15 | { 16 | init(' '); 17 | } 18 | 19 | Messenger::Messenger(char separator) 20 | { 21 | if (separator == 10 || separator == 13 || separator == 0) separator = 32; 22 | init(separator); 23 | } 24 | 25 | void Messenger::init(char separator) 26 | { 27 | callback = NULL; 28 | token[0] = separator; 29 | token[1] = 0; 30 | bufferLength = MESSENGERBUFFERSIZE; 31 | bufferLastIndex = MESSENGERBUFFERSIZE -1; 32 | reset(); 33 | } 34 | 35 | void Messenger::attach(messengerCallbackFunction newFunction) { 36 | callback = newFunction; 37 | } 38 | 39 | void Messenger::reset() { 40 | bufferIndex = 0; 41 | messageState = 0; 42 | current = NULL; 43 | last = NULL; 44 | dumped = 1; 45 | } 46 | 47 | int Messenger::readInt() { 48 | 49 | if (next()) { 50 | dumped = 1; 51 | return atoi(current); 52 | } 53 | return 0; 54 | 55 | } 56 | 57 | // Added based on a suggestion by G. Paolo Sanino 58 | long Messenger::readLong() { 59 | 60 | if (next()) { 61 | dumped = 1; 62 | return atol(current); // atol for long instead of atoi for int variables 63 | } 64 | return 0; 65 | 66 | } 67 | 68 | char Messenger::readChar() { 69 | 70 | if (next()) { 71 | dumped = 1; 72 | return current[0]; 73 | } 74 | return 0; 75 | 76 | } 77 | 78 | double Messenger::readDouble() { 79 | if(next()) { 80 | dumped = 1; 81 | return atof(current); 82 | } 83 | return 0; 84 | } 85 | 86 | void Messenger::copyString(char *string, uint8_t size) { 87 | 88 | if (next()) { 89 | dumped = 1; 90 | strlcpy(string,current,size); 91 | } else { 92 | if ( size ) string[0] = '\0'; 93 | } 94 | } 95 | 96 | uint8_t Messenger::checkString(char *string) { 97 | 98 | if (next()) { 99 | if ( strcmp(string,current) == 0 ) { 100 | dumped = 1; 101 | return 1; 102 | } else { 103 | return 0; 104 | } 105 | } 106 | } 107 | 108 | 109 | 110 | uint8_t Messenger::next() { 111 | 112 | char * temppointer= NULL; 113 | switch (messageState) { 114 | case 0: 115 | return 0; 116 | case 1: 117 | temppointer = buffer; 118 | messageState = 2; 119 | default: 120 | if (dumped) current = strtok_r(temppointer,token,&last); 121 | if (current != NULL) { 122 | dumped = 0; 123 | return 1; 124 | } 125 | } 126 | 127 | return 0; 128 | } 129 | 130 | uint8_t Messenger::available() { 131 | 132 | return next(); 133 | 134 | } 135 | 136 | 137 | uint8_t Messenger::process(int serialByte) { 138 | messageState = 0; 139 | if (serialByte > 0) { 140 | 141 | switch (serialByte) { 142 | case 0: 143 | break; 144 | case 10: // LF 145 | break; 146 | case 13: // CR 147 | buffer[bufferIndex]=0; 148 | reset(); 149 | messageState = 1; 150 | current = buffer; 151 | break; 152 | default: 153 | buffer[bufferIndex]=serialByte; 154 | bufferIndex++; 155 | if (bufferIndex >= bufferLastIndex) reset(); 156 | } 157 | } 158 | if ( messageState == 1 && callback != NULL) (*callback)(); 159 | return messageState; 160 | } 161 | 162 | 163 | 164 | -------------------------------------------------------------------------------- /arduino/libs/Messenger/Messenger.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef Messenger_h 3 | #define Messenger_h 4 | #define MESSENGERBUFFERSIZE 64 5 | 6 | #include 7 | 8 | extern "C" { 9 | // callback function 10 | typedef void (*messengerCallbackFunction)(void); 11 | } 12 | 13 | 14 | class Messenger 15 | { 16 | 17 | public: 18 | Messenger(); 19 | Messenger(char separator); 20 | int readInt(); 21 | long readLong(); // Added based on a suggestion by G. Paolo Sani 22 | char readChar(); 23 | double readDouble(); // Added based on a suggestion by Lorenzo C. 24 | void copyString(char *string, uint8_t size); 25 | uint8_t checkString(char *string); 26 | uint8_t process(int serialByte); 27 | uint8_t available(); 28 | void attach(messengerCallbackFunction newFunction); 29 | 30 | private: 31 | void init(char separator); 32 | uint8_t next(); 33 | void reset(); 34 | 35 | uint8_t messageState; 36 | 37 | messengerCallbackFunction callback; 38 | 39 | char* current; // Pointer to current data 40 | char* last; 41 | 42 | char token[2]; 43 | uint8_t dumped; 44 | 45 | uint8_t bufferIndex; // Index where to write the data 46 | char buffer[MESSENGERBUFFERSIZE]; // Buffer that holds the data 47 | uint8_t bufferLength; // The length of the buffer (defaults to 64) 48 | uint8_t bufferLastIndex; // The last index of the buffer 49 | }; 50 | 51 | #endif 52 | 53 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/examples/PinChangeInterrupt_HowItWorks/PinChangeInterrupt_HowItWorks.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | PinChangeInterrupt_HowItWorks 6 | Shows how to manually setup a single PCINT function with a few helper functions. 7 | 8 | Connect a button/cable to pin 7 and ground. 9 | The led will change its state if pin 7 changes. 10 | 11 | PinChangeInterrupts are different than normal Interrupts. 12 | See readme for more information. 13 | Dont use Serial or delay inside interrupts! 14 | This library is not compatible with SoftSerial. 15 | 16 | The following pins are usable for PinChangeInterrupt: 17 | Arduino Uno/Nano/Mini: All pins are usable 18 | Arduino Mega: 10, 11, 12, 13, 50, 51, 52, 53, A8 (62), A9 (63), A10 (64), 19 | A11 (65), A12 (66), A13 (67), A14 (68), A15 (69) 20 | Arduino Leonardo/Micro: 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI) 21 | HoodLoader2: All (broken out 1-7) pins are usable 22 | Attiny 24/44/84: All pins are usable 23 | Attiny 25/45/85: All pins are usable 24 | Attiny 13: All pins are usable 25 | Attiny 441/841: All pins are usable 26 | ATmega644P/ATmega1284P: All pins are usable 27 | */ 28 | 29 | //================================================================================ 30 | // User Settings 31 | //================================================================================ 32 | 33 | // choose a valid PinChangeInterrupt pin of your Arduino board 34 | #define PCINT_PIN 7 35 | #define PCINT_MODE CHANGE 36 | #define PCINT_FUNCTION blinkLed 37 | 38 | void setup() 39 | { 40 | // set pins to input with a pullup, led to output 41 | pinMode(PCINT_PIN, INPUT_PULLUP); 42 | pinMode(LED_BUILTIN, OUTPUT); 43 | 44 | // attach the new PinChangeInterrupt 45 | attachPinChangeInterrupt(); 46 | } 47 | 48 | void loop() { 49 | // empty 50 | } 51 | 52 | void blinkLed(void) { 53 | // switch Led state 54 | digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); 55 | } 56 | 57 | //================================================================================ 58 | // PCINT Definitions 59 | //================================================================================ 60 | 61 | #define PCMSK *digitalPinToPCMSK(PCINT_PIN) 62 | #define PCINT digitalPinToPCMSKbit(PCINT_PIN) 63 | #define PCIE digitalPinToPCICRbit(PCINT_PIN) 64 | #define PCPIN *portInputRegister(digitalPinToPort(PCINT_PIN)) 65 | 66 | #if (PCIE == 0) 67 | #define PCINT_vect PCINT0_vect 68 | #elif (PCIE == 1) 69 | #define PCINT_vect PCINT1_vect 70 | #elif (PCIE == 2) 71 | #define PCINT_vect PCINT2_vect 72 | #else 73 | #error This board doesnt support PCINT ? 74 | #endif 75 | 76 | volatile uint8_t oldPort = 0x00; 77 | 78 | void attachPinChangeInterrupt(void) { 79 | // update the old state to the actual state 80 | oldPort = PCPIN; 81 | 82 | // pin change mask registers decide which pins are enabled as triggers 83 | PCMSK |= (1 << PCINT); 84 | 85 | // PCICR: Pin Change Interrupt Control Register - enables interrupt vectors 86 | PCICR |= (1 << PCIE); 87 | } 88 | 89 | void detachPinChangeInterrupt(void) { 90 | // disable the mask. 91 | PCMSK &= ~(1 << PCINT); 92 | 93 | // if that's the last one, disable the interrupt. 94 | if (PCMSK == 0) 95 | PCICR &= ~(0x01 << PCIE); 96 | } 97 | 98 | ISR(PCINT_vect) { 99 | // get the new and old pin states for port 100 | uint8_t newPort = PCPIN; 101 | 102 | // compare with the old value to detect a rising or falling 103 | uint8_t change = newPort ^ oldPort; 104 | 105 | // check which pins are triggered, compared with the settings 106 | uint8_t trigger = 0x00; 107 | #if (PCINT_MODE == RISING) || (PCINT_MODE == CHANGE) 108 | uint8_t rising = change & newPort; 109 | trigger |= (rising & (1 << PCINT)); 110 | #endif 111 | #if (PCINT_MODE == FALLING) || (PCINT_MODE == CHANGE) 112 | uint8_t falling = change & oldPort; 113 | trigger |= (falling & (1 << PCINT)); 114 | #endif 115 | 116 | // save the new state for next comparison 117 | oldPort = newPort; 118 | 119 | // if our needed pin has changed, call the IRL interrupt function 120 | if (trigger & (1 << PCINT)) 121 | PCINT_FUNCTION(); 122 | } 123 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/examples/PinChangeInterrupt_Led/PinChangeInterrupt_Led.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | PinChangeInterrupt_TickTock 6 | Demonstrates how to use the library 7 | 8 | Connect a button/cable to pin 7 and ground. 9 | The Led state will change if the pin state does. 10 | 11 | PinChangeInterrupts are different than normal Interrupts. 12 | See readme for more information. 13 | Dont use Serial or delay inside interrupts! 14 | This library is not compatible with SoftSerial. 15 | 16 | The following pins are usable for PinChangeInterrupt: 17 | Arduino Uno/Nano/Mini: All pins are usable 18 | Arduino Mega: 10, 11, 12, 13, 50, 51, 52, 53, A8 (62), A9 (63), A10 (64), 19 | A11 (65), A12 (66), A13 (67), A14 (68), A15 (69) 20 | Arduino Leonardo/Micro: 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI) 21 | HoodLoader2: All (broken out 1-7) pins are usable 22 | Attiny 24/44/84: All pins are usable 23 | Attiny 25/45/85: All pins are usable 24 | Attiny 13: All pins are usable 25 | Attiny 441/841: All pins are usable 26 | ATmega644P/ATmega1284P: All pins are usable 27 | */ 28 | 29 | #include "PinChangeInterrupt.h" 30 | 31 | // Choose a valid PinChangeInterrupt pin of your Arduino board 32 | #define pinBlink 7 33 | 34 | void setup() { 35 | // set pin to input with a pullup, led to output 36 | pinMode(pinBlink, INPUT_PULLUP); 37 | pinMode(LED_BUILTIN, OUTPUT); 38 | 39 | // Manually blink once to test if LED is functional 40 | blinkLed(); 41 | delay(1000); 42 | blinkLed(); 43 | 44 | // Attach the new PinChangeInterrupt and enable event function below 45 | attachPCINT(digitalPinToPCINT(pinBlink), blinkLed, CHANGE); 46 | } 47 | 48 | void blinkLed(void) { 49 | // Switch Led state 50 | digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); 51 | } 52 | 53 | void loop() { 54 | // Nothing to do here 55 | } 56 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/examples/PinChangeInterrupt_LowLevel/PinChangeInterrupt_LowLevel.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | PinChangeInterrupt_LowLevel 6 | Demonstrates how to use the library without the API 7 | 8 | Make sure to comment "//#define PCINT_API" in the settings file. 9 | 10 | To maximize speed and size also uncomment all not used pins above. 11 | Then you could also uncomment "#define PCINT_COMPILE_ENABLED_ISR" 12 | to get away the .a linkage overhead. 13 | 14 | Connect a button/cable to pin 7 and ground (Uno). 15 | Strong overwritten callback functions are called when an interrupt occurs. 16 | The Led state will change if the pin state does. 17 | 18 | PinChangeInterrupts are different than normal Interrupts. 19 | See readme for more information. 20 | Dont use Serial or delay inside interrupts! 21 | This library is not compatible with SoftSerial. 22 | 23 | The following pins are usable for PinChangeInterrupt: 24 | Arduino Uno/Nano/Mini: All pins are usable 25 | Arduino Mega: 10, 11, 12, 13, 50, 51, 52, 53, A8 (62), A9 (63), A10 (64), 26 | A11 (65), A12 (66), A13 (67), A14 (68), A15 (69) 27 | Arduino Leonardo/Micro: 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI) 28 | HoodLoader2: All (broken out 1-7) pins are usable 29 | Attiny 24/44/84: All pins are usable 30 | Attiny 25/45/85: All pins are usable 31 | Attiny 13: All pins are usable 32 | Attiny 441/841: All pins are usable 33 | ATmega644P/ATmega1284P: All pins are usable 34 | */ 35 | 36 | #include "PinChangeInterrupt.h" 37 | 38 | // choose a valid PinChangeInterrupt pin of your Arduino board 39 | // manually defined pcint number 40 | #define pinBlink 7 41 | #define interruptBlink 23 42 | 43 | void setup() 44 | { 45 | // set pin to input with a pullup, led to output 46 | pinMode(pinBlink, INPUT_PULLUP); 47 | pinMode(LED_BUILTIN, OUTPUT); 48 | 49 | // attach the new PinChangeInterrupts and enable event functions below 50 | attachPinChangeInterrupt(interruptBlink, CHANGE); 51 | } 52 | 53 | void PinChangeInterruptEvent(interruptBlink)(void) { 54 | // switch Led state 55 | digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); 56 | } 57 | 58 | void loop() { 59 | // nothing to do here 60 | } 61 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/examples/PinChangeInterrupt_TickTock/PinChangeInterrupt_TickTock.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | PinChangeInterrupt_TickTock 6 | Demonstrates how to use the library 7 | 8 | Connect a button/cable to pin 10/11 and ground. 9 | The value printed on the serial port will increase 10 | if pin 10 is rising and decrease if pin 11 is falling. 11 | 12 | PinChangeInterrupts are different than normal Interrupts. 13 | See readme for more information. 14 | Dont use Serial or delay inside interrupts! 15 | This library is not compatible with SoftSerial. 16 | 17 | The following pins are usable for PinChangeInterrupt: 18 | Arduino Uno/Nano/Mini: All pins are usable 19 | Arduino Mega: 10, 11, 12, 13, 50, 51, 52, 53, A8 (62), A9 (63), A10 (64), 20 | A11 (65), A12 (66), A13 (67), A14 (68), A15 (69) 21 | Arduino Leonardo/Micro: 8, 9, 10, 11, 14 (MISO), 15 (SCK), 16 (MOSI) 22 | HoodLoader2: All (broken out 1-7) pins are usable 23 | Attiny 24/44/84: All pins are usable 24 | Attiny 25/45/85: All pins are usable 25 | Attiny 13: All pins are usable 26 | Attiny 441/841: All pins are usable 27 | ATmega644P/ATmega1284P: All pins are usable 28 | */ 29 | 30 | #include "PinChangeInterrupt.h" 31 | 32 | // choose a valid PinChangeInterrupt pin of your Arduino board 33 | #define pinTick 10 34 | #define pinTock 11 35 | 36 | volatile long ticktocks = 0; 37 | 38 | void setup() 39 | { 40 | // start serial debug output 41 | Serial.begin(115200); 42 | Serial.println(F("Startup")); 43 | 44 | // set pins to input with a pullup 45 | pinMode(pinTick, INPUT_PULLUP); 46 | pinMode(pinTock, INPUT_PULLUP); 47 | 48 | // attach the new PinChangeInterrupts and enable event functions below 49 | attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTick), tick, RISING); 50 | attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTock), tock, FALLING); 51 | } 52 | 53 | void loop() { 54 | // integer to count the number of prints 55 | static int i = 0; 56 | delay(1000); 57 | 58 | // print values 59 | Serial.print(i, DEC); 60 | Serial.print(F(" ")); 61 | Serial.println(ticktocks); 62 | 63 | // abort if we printed 100 times 64 | if (i >= 100) { 65 | Serial.println(F("Detaching Interrupts.")); 66 | detachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTick)); 67 | detachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTock)); 68 | return; 69 | } 70 | else 71 | i++; 72 | 73 | // Temporary pause interrupts 74 | if (ticktocks > 500) { 75 | Serial.println(F("Disabling Tick Interrupt.")); 76 | disablePinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTick)); 77 | enablePinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTock)); 78 | } 79 | else if (ticktocks < -500) { 80 | Serial.println(F("Disabling Tock Interrupt.")); 81 | disablePinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTock)); 82 | enablePinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTick)); 83 | } 84 | else { 85 | enablePinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTick)); 86 | enablePinChangeInterrupt(digitalPinToPinChangeInterrupt(pinTock)); 87 | } 88 | } 89 | 90 | void tick(void) { 91 | // increase value 92 | ticktocks++; 93 | } 94 | 95 | void tock(void) { 96 | // decrease value 97 | ticktocks--; 98 | } 99 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/header.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/arduino/libs/PinChangeInterrupt/header.png -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For PinChangeInterrupt 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ####################################### 10 | # Methods and Functions (KEYWORD2) 11 | ####################################### 12 | 13 | attachPinChangeInterrupt KEYWORD2 14 | detachPinChangeInterrupt KEYWORD2 15 | attachPCINT KEYWORD2 16 | detachPCINT KEYWORD2 17 | PinChangeInterruptEvent KEYWORD2 18 | PCINTEvent KEYWORD2 19 | enablePCINT KEYWORD2 20 | enablePinChangeInterrupt KEYWORD2 21 | disablePCINT KEYWORD2 22 | disablePinChangeInterrupt KEYWORD2 23 | getPCINTTrigger KEYWORD2 24 | getPinChangeInterruptTrigger KEYWORD2 25 | 26 | ####################################### 27 | # Instances (KEYWORD2) 28 | ####################################### 29 | 30 | ####################################### 31 | # Constants (LITERAL1) 32 | ####################################### 33 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/library.properties: -------------------------------------------------------------------------------- 1 | name=PinChangeInterrupt 2 | version=1.2.4 3 | author=NicoHood 4 | maintainer=NicoHood 5 | sentence=A simple & compact PinChangeInterrupt library for Arduino. 6 | paragraph=PinChangeInterrupt library with a resource friendly implementation (API and LowLevel). PinChangeInterrupts are different than normal Interrupts. See readme for more information. 7 | category=Signal Input/Output 8 | url=https://github.com/NicoHood/PinChangeInterrupt 9 | architectures=avr 10 | dot_a_linkage=true 11 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/src/PinChangeInterrupt0.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "PinChangeInterrupt.h" 25 | 26 | //================================================================================ 27 | // Interrupt Handler 28 | //================================================================================ 29 | 30 | // prevent compilation twice if included from the .cpp to force compile all ISRs 31 | #if defined(PCINT_ALINKAGE) && defined(PCINT_COMPILE_ENABLED_ISR) && defined(PCINT_INCLUDE_FROM_CPP) \ 32 | || !defined(PCINT_ALINKAGE) || !defined(PCINT_COMPILE_ENABLED_ISR) 33 | 34 | #if (PCINT_USE_PORT0 == true) 35 | 36 | void attachPinChangeInterrupt0(void) { 37 | // fake function to make the IDE link this file 38 | } 39 | 40 | ISR(PCINT0_vect) { 41 | // get the new and old pin states for port 42 | uint8_t newPort = PCINT_INPUT_PORT0; 43 | 44 | // compare with the old value to detect a rising or falling 45 | uint8_t arrayPos = getArrayPosPCINT(0); 46 | uint8_t change = newPort ^ oldPorts[arrayPos]; 47 | uint8_t rising = change & newPort; 48 | uint8_t falling = change & oldPorts[arrayPos]; 49 | 50 | // check which pins are triggered, compared with the settings 51 | uint8_t risingTrigger = rising & risingPorts[arrayPos]; 52 | uint8_t fallingTrigger = falling & fallingPorts[arrayPos]; 53 | uint8_t trigger = risingTrigger | fallingTrigger; 54 | 55 | // save the new state for next comparison 56 | oldPorts[arrayPos] = newPort; 57 | 58 | // Execute all functions that should be triggered 59 | // This way we can exclude a single function 60 | // and the calling is also much faster 61 | // We may also reorder the pins for different priority 62 | #if !defined(PCINT_CALLBACK_PORT0) 63 | PCINT_CALLBACK(0, 0); 64 | PCINT_CALLBACK(1, 1); 65 | PCINT_CALLBACK(2, 2); 66 | PCINT_CALLBACK(3, 3); 67 | PCINT_CALLBACK(4, 4); 68 | PCINT_CALLBACK(5, 5); 69 | PCINT_CALLBACK(6, 6); 70 | PCINT_CALLBACK(7, 7); 71 | #else 72 | PCINT_CALLBACK_PORT0 73 | #endif 74 | } 75 | 76 | #if defined(PCINT_API) 77 | 78 | /* 79 | for (int i = 0; i < 32; i++) { 80 | Serial.print("#if (PCINT_USE_PCINT"); 81 | Serial.print(i); 82 | Serial.println(" == true)"); 83 | Serial.print("volatile callback callbackPCINT"); 84 | Serial.print(i); 85 | Serial.println(" = pcint_null_callback;"); 86 | Serial.print("void PinChangeInterruptEventPCINT"); 87 | Serial.print(i); 88 | Serial.println("(void){"); 89 | Serial.print(" callbackPCINT"); 90 | Serial.print(i); 91 | Serial.println("();"); 92 | Serial.println("}"); 93 | Serial.println("#endif"); 94 | } 95 | */ 96 | #if (PCINT_USE_PCINT0 == true) 97 | volatile callback callbackPCINT0 = pcint_null_callback; 98 | void PinChangeInterruptEventPCINT0(void) { 99 | callbackPCINT0(); 100 | } 101 | #endif 102 | #if (PCINT_USE_PCINT1 == true) 103 | volatile callback callbackPCINT1 = pcint_null_callback; 104 | void PinChangeInterruptEventPCINT1(void) { 105 | callbackPCINT1(); 106 | } 107 | #endif 108 | #if (PCINT_USE_PCINT2 == true) 109 | volatile callback callbackPCINT2 = pcint_null_callback; 110 | void PinChangeInterruptEventPCINT2(void) { 111 | callbackPCINT2(); 112 | } 113 | #endif 114 | #if (PCINT_USE_PCINT3 == true) 115 | volatile callback callbackPCINT3 = pcint_null_callback; 116 | void PinChangeInterruptEventPCINT3(void) { 117 | callbackPCINT3(); 118 | } 119 | #endif 120 | #if (PCINT_USE_PCINT4 == true) 121 | volatile callback callbackPCINT4 = pcint_null_callback; 122 | void PinChangeInterruptEventPCINT4(void) { 123 | callbackPCINT4(); 124 | } 125 | #endif 126 | #if (PCINT_USE_PCINT5 == true) 127 | volatile callback callbackPCINT5 = pcint_null_callback; 128 | void PinChangeInterruptEventPCINT5(void) { 129 | callbackPCINT5(); 130 | } 131 | #endif 132 | #if (PCINT_USE_PCINT6 == true) 133 | volatile callback callbackPCINT6 = pcint_null_callback; 134 | void PinChangeInterruptEventPCINT6(void) { 135 | callbackPCINT6(); 136 | } 137 | #endif 138 | #if (PCINT_USE_PCINT7 == true) 139 | volatile callback callbackPCINT7 = pcint_null_callback; 140 | void PinChangeInterruptEventPCINT7(void) { 141 | callbackPCINT7(); 142 | } 143 | #endif 144 | 145 | #endif // PCINT_API 146 | 147 | #endif // PCINT_USE_PORT0 148 | 149 | #endif // PCINT_INCLUDE_FROM_CPP 150 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/src/PinChangeInterrupt1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "PinChangeInterrupt.h" 25 | 26 | //================================================================================ 27 | // Interrupt Handler 28 | //================================================================================ 29 | 30 | // prevent compilation twice if included from the .cpp to force compile all ISRs 31 | #if defined(PCINT_ALINKAGE) && defined(PCINT_COMPILE_ENABLED_ISR) && defined(PCINT_INCLUDE_FROM_CPP) \ 32 | || !defined(PCINT_ALINKAGE) || !defined(PCINT_COMPILE_ENABLED_ISR) 33 | 34 | #if (PCINT_USE_PORT1 == true) 35 | 36 | void attachPinChangeInterrupt1(void) { 37 | // fake function to make the IDE link this file 38 | } 39 | 40 | ISR(PCINT1_vect) { 41 | // get the new and old pin states for port 42 | uint8_t newPort = PCINT_INPUT_PORT1; 43 | 44 | // compare with the old value to detect a rising or falling 45 | uint8_t arrayPos = getArrayPosPCINT(1); 46 | uint8_t change = newPort ^ oldPorts[arrayPos]; 47 | uint8_t rising = change & newPort; 48 | uint8_t falling = change & oldPorts[arrayPos]; 49 | 50 | // check which pins are triggered, compared with the settings 51 | uint8_t risingTrigger = rising & risingPorts[arrayPos]; 52 | uint8_t fallingTrigger = falling & fallingPorts[arrayPos]; 53 | uint8_t trigger = risingTrigger | fallingTrigger; 54 | 55 | // save the new state for next comparison 56 | oldPorts[arrayPos] = newPort; 57 | 58 | // Execute all functions that should be triggered 59 | // This way we can exclude a single function 60 | // and the calling is also much faster 61 | // We may also reorder the pins for different priority 62 | #if !defined(PCINT_CALLBACK_PORT1) 63 | PCINT_CALLBACK(0, 8); 64 | PCINT_CALLBACK(1, 9); 65 | PCINT_CALLBACK(2, 10); 66 | PCINT_CALLBACK(3, 11); 67 | PCINT_CALLBACK(4, 12); 68 | PCINT_CALLBACK(5, 13); 69 | PCINT_CALLBACK(6, 14); 70 | PCINT_CALLBACK(7, 15); 71 | #else 72 | PCINT_CALLBACK_PORT1 73 | #endif 74 | } 75 | 76 | #if defined(PCINT_API) 77 | 78 | /* 79 | for (int i = 0; i < 32; i++) { 80 | Serial.print("#if (PCINT_USE_PCINT"); 81 | Serial.print(i); 82 | Serial.println(" == true)"); 83 | Serial.print("volatile callback callbackPCINT"); 84 | Serial.print(i); 85 | Serial.println(" = pcint_null_callback;"); 86 | Serial.print("void PinChangeInterruptEventPCINT"); 87 | Serial.print(i); 88 | Serial.println("(void){"); 89 | Serial.print(" callbackPCINT"); 90 | Serial.print(i); 91 | Serial.println("();"); 92 | Serial.println("}"); 93 | Serial.println("#endif"); 94 | } 95 | */ 96 | #if (PCINT_USE_PCINT8 == true) 97 | volatile callback callbackPCINT8 = pcint_null_callback; 98 | void PinChangeInterruptEventPCINT8(void) { 99 | callbackPCINT8(); 100 | } 101 | #endif 102 | #if (PCINT_USE_PCINT9 == true) 103 | volatile callback callbackPCINT9 = pcint_null_callback; 104 | void PinChangeInterruptEventPCINT9(void) { 105 | callbackPCINT9(); 106 | } 107 | #endif 108 | #if (PCINT_USE_PCINT10 == true) 109 | volatile callback callbackPCINT10 = pcint_null_callback; 110 | void PinChangeInterruptEventPCINT10(void) { 111 | callbackPCINT10(); 112 | } 113 | #endif 114 | #if (PCINT_USE_PCINT11 == true) 115 | volatile callback callbackPCINT11 = pcint_null_callback; 116 | void PinChangeInterruptEventPCINT11(void) { 117 | callbackPCINT11(); 118 | } 119 | #endif 120 | #if (PCINT_USE_PCINT12 == true) 121 | volatile callback callbackPCINT12 = pcint_null_callback; 122 | void PinChangeInterruptEventPCINT12(void) { 123 | callbackPCINT12(); 124 | } 125 | #endif 126 | #if (PCINT_USE_PCINT13 == true) 127 | volatile callback callbackPCINT13 = pcint_null_callback; 128 | void PinChangeInterruptEventPCINT13(void) { 129 | callbackPCINT13(); 130 | } 131 | #endif 132 | #if (PCINT_USE_PCINT14 == true) 133 | volatile callback callbackPCINT14 = pcint_null_callback; 134 | void PinChangeInterruptEventPCINT14(void) { 135 | callbackPCINT14(); 136 | } 137 | #endif 138 | #if (PCINT_USE_PCINT15 == true) 139 | volatile callback callbackPCINT15 = pcint_null_callback; 140 | void PinChangeInterruptEventPCINT15(void) { 141 | callbackPCINT15(); 142 | } 143 | #endif 144 | 145 | #endif // PCINT_API 146 | 147 | #endif // PCINT_USE_PORT1 148 | 149 | #endif // PCINT_INCLUDE_FROM_CPP 150 | -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/src/PinChangeInterrupt2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "PinChangeInterrupt.h" 25 | 26 | //================================================================================ 27 | // Interrupt Handler 28 | //================================================================================ 29 | 30 | // prevent compilation twice if included from the .cpp to force compile all ISRs 31 | #if defined(PCINT_ALINKAGE) && defined(PCINT_COMPILE_ENABLED_ISR) && defined(PCINT_INCLUDE_FROM_CPP) \ 32 | || !defined(PCINT_ALINKAGE) || !defined(PCINT_COMPILE_ENABLED_ISR) 33 | 34 | #if (PCINT_USE_PORT2 == true) 35 | 36 | void attachPinChangeInterrupt2(void) { 37 | // fake function to make the IDE link this file 38 | } 39 | 40 | ISR(PCINT2_vect) { 41 | // get the new and old pin states for port 42 | uint8_t newPort = PCINT_INPUT_PORT2; 43 | 44 | // compare with the old value to detect a rising or falling 45 | uint8_t arrayPos = getArrayPosPCINT(2); 46 | uint8_t change = newPort ^ oldPorts[arrayPos]; 47 | uint8_t rising = change & newPort; 48 | uint8_t falling = change & oldPorts[arrayPos]; 49 | 50 | // check which pins are triggered, compared with the settings 51 | uint8_t risingTrigger = rising & risingPorts[arrayPos]; 52 | uint8_t fallingTrigger = falling & fallingPorts[arrayPos]; 53 | uint8_t trigger = risingTrigger | fallingTrigger; 54 | 55 | // save the new state for next comparison 56 | oldPorts[arrayPos] = newPort; 57 | 58 | // Execute all functions that should be triggered 59 | // This way we can exclude a single function 60 | // and the calling is also much faster 61 | // We may also reorder the pins for different priority 62 | #if !defined(PCINT_CALLBACK_PORT2) 63 | PCINT_CALLBACK(0, 16); 64 | PCINT_CALLBACK(1, 17); 65 | PCINT_CALLBACK(2, 18); 66 | PCINT_CALLBACK(3, 19); 67 | PCINT_CALLBACK(4, 20); 68 | PCINT_CALLBACK(5, 21); 69 | PCINT_CALLBACK(6, 22); 70 | PCINT_CALLBACK(7, 23); 71 | #else 72 | PCINT_CALLBACK_PORT2 73 | #endif 74 | } 75 | 76 | #if defined(PCINT_API) 77 | 78 | /* 79 | for (int i = 0; i < 32; i++) { 80 | Serial.print("#if (PCINT_USE_PCINT"); 81 | Serial.print(i); 82 | Serial.println(" == true)"); 83 | Serial.print("volatile callback callbackPCINT"); 84 | Serial.print(i); 85 | Serial.println(" = pcint_null_callback;"); 86 | Serial.print("void PinChangeInterruptEventPCINT"); 87 | Serial.print(i); 88 | Serial.println("(void){"); 89 | Serial.print(" callbackPCINT"); 90 | Serial.print(i); 91 | Serial.println("();"); 92 | Serial.println("}"); 93 | Serial.println("#endif"); 94 | } 95 | */ 96 | #if (PCINT_USE_PCINT16 == true) 97 | volatile callback callbackPCINT16 = pcint_null_callback; 98 | void PinChangeInterruptEventPCINT16(void) { 99 | callbackPCINT16(); 100 | } 101 | #endif 102 | #if (PCINT_USE_PCINT17 == true) 103 | volatile callback callbackPCINT17 = pcint_null_callback; 104 | void PinChangeInterruptEventPCINT17(void) { 105 | callbackPCINT17(); 106 | } 107 | #endif 108 | #if (PCINT_USE_PCINT18 == true) 109 | volatile callback callbackPCINT18 = pcint_null_callback; 110 | void PinChangeInterruptEventPCINT18(void) { 111 | callbackPCINT18(); 112 | } 113 | #endif 114 | #if (PCINT_USE_PCINT19 == true) 115 | volatile callback callbackPCINT19 = pcint_null_callback; 116 | void PinChangeInterruptEventPCINT19(void) { 117 | callbackPCINT19(); 118 | } 119 | #endif 120 | #if (PCINT_USE_PCINT20 == true) 121 | volatile callback callbackPCINT20 = pcint_null_callback; 122 | void PinChangeInterruptEventPCINT20(void) { 123 | callbackPCINT20(); 124 | } 125 | #endif 126 | #if (PCINT_USE_PCINT21 == true) 127 | volatile callback callbackPCINT21 = pcint_null_callback; 128 | void PinChangeInterruptEventPCINT21(void) { 129 | callbackPCINT21(); 130 | } 131 | #endif 132 | #if (PCINT_USE_PCINT22 == true) 133 | volatile callback callbackPCINT22 = pcint_null_callback; 134 | void PinChangeInterruptEventPCINT22(void) { 135 | callbackPCINT22(); 136 | } 137 | #endif 138 | #if (PCINT_USE_PCINT23 == true) 139 | volatile callback callbackPCINT23 = pcint_null_callback; 140 | void PinChangeInterruptEventPCINT23(void) { 141 | callbackPCINT23(); 142 | } 143 | #endif 144 | 145 | #endif // PCINT_API 146 | 147 | #endif // PCINT_USE_PORT2 148 | 149 | #endif // PCINT_INCLUDE_FROM_CPP -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/src/PinChangeInterrupt3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | #include "PinChangeInterrupt.h" 25 | 26 | //================================================================================ 27 | // Interrupt Handler 28 | //================================================================================ 29 | 30 | // prevent compilation twice if included from the .cpp to force compile all ISRs 31 | #if defined(PCINT_ALINKAGE) && defined(PCINT_COMPILE_ENABLED_ISR) && defined(PCINT_INCLUDE_FROM_CPP) \ 32 | || !defined(PCINT_ALINKAGE) || !defined(PCINT_COMPILE_ENABLED_ISR) 33 | 34 | #if (PCINT_USE_PORT3 == true) 35 | 36 | void attachPinChangeInterrupt3(void) { 37 | // fake function to make the IDE link this file 38 | } 39 | 40 | ISR(PCINT3_vect) { 41 | // get the new and old pin states for port 42 | uint8_t newPort = PCINT_INPUT_PORT3; 43 | 44 | // compare with the old value to detect a rising or falling 45 | uint8_t arrayPos = getArrayPosPCINT(3); 46 | uint8_t change = newPort ^ oldPorts[arrayPos]; 47 | uint8_t rising = change & newPort; 48 | uint8_t falling = change & oldPorts[arrayPos]; 49 | 50 | // check which pins are triggered, compared with the settings 51 | uint8_t risingTrigger = rising & risingPorts[arrayPos]; 52 | uint8_t fallingTrigger = falling & fallingPorts[arrayPos]; 53 | uint8_t trigger = risingTrigger | fallingTrigger; 54 | 55 | // save the new state for next comparison 56 | oldPorts[arrayPos] = newPort; 57 | 58 | // Execute all functions that should be triggered 59 | // This way we can exclude a single function 60 | // and the calling is also much faster 61 | // We may also reorder the pins for different priority 62 | #if !defined(PCINT_CALLBACK_PORT3) 63 | PCINT_CALLBACK(0, 24); 64 | PCINT_CALLBACK(1, 25); 65 | PCINT_CALLBACK(2, 26); 66 | PCINT_CALLBACK(3, 27); 67 | PCINT_CALLBACK(4, 28); 68 | PCINT_CALLBACK(5, 29); 69 | PCINT_CALLBACK(6, 30); 70 | PCINT_CALLBACK(7, 31); 71 | #else 72 | PCINT_CALLBACK_PORT3 73 | #endif 74 | } 75 | 76 | #if defined(PCINT_API) 77 | 78 | /* 79 | for (int i = 0; i < 32; i++) { 80 | Serial.print("#if (PCINT_USE_PCINT"); 81 | Serial.print(i); 82 | Serial.println(" == true)"); 83 | Serial.print("volatile callback callbackPCINT"); 84 | Serial.print(i); 85 | Serial.println(" = pcint_null_callback;"); 86 | Serial.print("void PinChangeInterruptEventPCINT"); 87 | Serial.print(i); 88 | Serial.println("(void){"); 89 | Serial.print(" callbackPCINT"); 90 | Serial.print(i); 91 | Serial.println("();"); 92 | Serial.println("}"); 93 | Serial.println("#endif"); 94 | } 95 | */ 96 | #if (PCINT_USE_PCINT24 == true) 97 | volatile callback callbackPCINT24 = pcint_null_callback; 98 | void PinChangeInterruptEventPCINT24(void) { 99 | callbackPCINT24(); 100 | } 101 | #endif 102 | #if (PCINT_USE_PCINT25 == true) 103 | volatile callback callbackPCINT25 = pcint_null_callback; 104 | void PinChangeInterruptEventPCINT25(void) { 105 | callbackPCINT25(); 106 | } 107 | #endif 108 | #if (PCINT_USE_PCINT26 == true) 109 | volatile callback callbackPCINT26 = pcint_null_callback; 110 | void PinChangeInterruptEventPCINT26(void) { 111 | callbackPCINT26(); 112 | } 113 | #endif 114 | #if (PCINT_USE_PCINT27 == true) 115 | volatile callback callbackPCINT27 = pcint_null_callback; 116 | void PinChangeInterruptEventPCINT27(void) { 117 | callbackPCINT27(); 118 | } 119 | #endif 120 | #if (PCINT_USE_PCINT28 == true) 121 | volatile callback callbackPCINT28 = pcint_null_callback; 122 | void PinChangeInterruptEventPCINT28(void) { 123 | callbackPCINT28(); 124 | } 125 | #endif 126 | #if (PCINT_USE_PCINT29 == true) 127 | volatile callback callbackPCINT29 = pcint_null_callback; 128 | void PinChangeInterruptEventPCINT29(void) { 129 | callbackPCINT29(); 130 | } 131 | #endif 132 | #if (PCINT_USE_PCINT30 == true) 133 | volatile callback callbackPCINT30 = pcint_null_callback; 134 | void PinChangeInterruptEventPCINT30(void) { 135 | callbackPCINT30(); 136 | } 137 | #endif 138 | #if (PCINT_USE_PCINT31 == true) 139 | volatile callback callbackPCINT31 = pcint_null_callback; 140 | void PinChangeInterruptEventPCINT31(void) { 141 | callbackPCINT31(); 142 | } 143 | #endif 144 | 145 | #endif // PCINT_API 146 | 147 | #endif // PCINT_USE_PORT3 148 | 149 | #endif // PCINT_INCLUDE_FROM_CPP -------------------------------------------------------------------------------- /arduino/libs/PinChangeInterrupt/src/PinChangeInterruptBoards.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2014-2015 NicoHood 3 | See the readme for credit to other people. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | */ 23 | 24 | // include guard 25 | #pragma once 26 | 27 | //================================================================================ 28 | // Board Definitions 29 | //================================================================================ 30 | 31 | // Microcontroller specific definitions 32 | 33 | #if defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega88__) 34 | // Arduino Uno 35 | #define PCINT_INPUT_PORT0 PINB 36 | #define PCINT_INPUT_PORT1 PINC 37 | #define PCINT_INPUT_PORT2 PIND 38 | 39 | #elif defined(__AVR_ATmega2560__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega640__) 40 | // Arduino Mega/Mega2560 41 | #define PCINT_INPUT_PORT0 PINB 42 | #define PCINT_INPUT_PORT2 PINK 43 | 44 | // special Port1 case, pins are on 2 HW Pin Ports (E,J) 45 | #if defined(PCINT_ENABLE_PCINT16) // PortE 46 | #if defined(PCINT_ENABLE_PCINT17) || defined(PCINT_ENABLE_PCINT18) \ 47 | || defined(PCINT_ENABLE_PCINT19) || defined(PCINT_ENABLE_PCINT20) \ 48 | || defined(PCINT_ENABLE_PCINT21) || defined(PCINT_ENABLE_PCINT22) \ 49 | || defined(PCINT_ENABLE_PCINT23) // PortJ 50 | // PortE and PortJ selected 51 | #define PCINT_INPUT_PORT1 ((PINE & 0x01) | (PINJ << 1)) 52 | #else 53 | // PortE only selected 54 | #define PCINT_INPUT_PORT1 PINE 55 | #endif 56 | #else 57 | // PortJ only selected 58 | // we still have to do the shift because the attach 59 | // function is not designed for this optimization 60 | #define PCINT_INPUT_PORT1 (PINJ << 1) 61 | #endif 62 | 63 | #elif defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega16U4__) 64 | // Arduino Leonardo/Micro 65 | #define PCINT_INPUT_PORT0 PINB 66 | 67 | #elif defined(__AVR_AT90USB82__) || defined(__AVR_AT90USB162__) || defined(__AVR_ATmega32U2__) || defined(__AVR_ATmega16U2__) || defined(__AVR_ATmega8U2__) 68 | // u2 Series/HoodLoader2 69 | // u2 Series has crappy pin mappings for port 1 70 | #define PCINT_INPUT_PORT0 PINB 71 | #define PCINT_INPUT_PORT1 (((PINC >> 6) & (1 << 0)) | ((PINC >> 4) & (1 << 1)) | ((PINC >> 2) & (1 << 2)) | ((PINC << 1) & (1 << 3)) | ((PIND >> 1) & (1 << 4))) 72 | 73 | #elif defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) 74 | // Attiny x5 75 | #define PCINT_INPUT_PORT0 PINB 76 | 77 | #elif defined(__AVR_ATtiny13__) 78 | // Attiny 13A 79 | #define PCINT_INPUT_PORT0 PINB 80 | #ifndef portInputRegister 81 | #define portInputRegister(P) ( (volatile uint8_t *)(PINB) ) 82 | #endif 83 | 84 | #elif defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) 85 | // Attiny x4 86 | #define PCINT_INPUT_PORT0 PINA 87 | #define PCINT_INPUT_PORT1 PINB 88 | 89 | #elif defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) 90 | // 1284p and 644p, special 4 port case 91 | #define PCINT_INPUT_PORT0 PINA 92 | #define PCINT_INPUT_PORT1 PINB 93 | #define PCINT_INPUT_PORT2 PINC 94 | #define PCINT_INPUT_PORT3 PIND 95 | 96 | #elif defined(__AVR_ATtinyX41__) || defined(__AVR_ATtiny441__) || defined(__AVR_ATtiny841__) 97 | // Attiny x41 98 | #define PCINT_INPUT_PORT0 PINA 99 | #define PCINT_INPUT_PORT1 PINB 100 | 101 | // "iotn841.h" is missing those definitions, so we add them here 102 | #define PCINT0 0 103 | #define PCINT1 1 104 | #define PCINT2 2 105 | #define PCINT3 3 106 | #define PCINT4 4 107 | #define PCINT5 5 108 | #define PCINT6 6 109 | #define PCINT7 7 110 | 111 | #define PCINT8 0 112 | #define PCINT9 1 113 | #define PCINT10 2 114 | #define PCINT11 3 115 | 116 | #elif defined(__AVR_ATtinyX313__) 117 | // ATtiny x313, PORT A is almost useless, left out here 118 | #define PCINT_INPUT_PORT1 PINB 119 | #define PCINT_INPUT_PORT2 PIND 120 | 121 | #else // Microcontroller not supported 122 | #error PinChangeInterrupt library does not support this MCU. 123 | #endif 124 | 125 | //================================================================================ 126 | // Add missing definitions 127 | //================================================================================ 128 | 129 | // add fakes if ports are not used 130 | #ifndef PCINT_INPUT_PORT0 131 | #define PCINT_INPUT_PORT0 0 132 | #else 133 | #define PCINT_INPUT_PORT0_USED 134 | #endif 135 | #ifndef PCINT_INPUT_PORT1 136 | #define PCINT_INPUT_PORT1 0 137 | #else 138 | #define PCINT_INPUT_PORT1_USED 139 | #endif 140 | #ifndef PCINT_INPUT_PORT2 141 | #define PCINT_INPUT_PORT2 0 142 | #else 143 | #define PCINT_INPUT_PORT2_USED 144 | #endif 145 | #ifndef PCINT_INPUT_PORT3 146 | #define PCINT_INPUT_PORT3 0 147 | #else 148 | #define PCINT_INPUT_PORT3_USED 149 | #endif 150 | -------------------------------------------------------------------------------- /arduino/simple_encoder_test/simple_encoder_test.ino: -------------------------------------------------------------------------------- 1 | //Encoder pins definition 2 | 3 | // Left encoder 4 | 5 | int Left_Encoder_PinA = 2; 6 | int Left_Encoder_PinB = 9; 7 | 8 | volatile long Left_Encoder_Ticks = 0; 9 | 10 | //Variable to read current state of left encoder pin 11 | volatile bool LeftEncoderBSet; 12 | 13 | //Right Encoder 14 | 15 | int Right_Encoder_PinA = 3; 16 | int Right_Encoder_PinB = 10; 17 | volatile long Right_Encoder_Ticks = 0; 18 | //Variable to read current state of right encoder pin 19 | volatile bool RightEncoderBSet; 20 | 21 | void setup() 22 | { 23 | //Init Serial port with 115200 buad rate 24 | Serial.begin(115200); 25 | SetupEncoders(); 26 | } 27 | 28 | 29 | void SetupEncoders() 30 | { 31 | // Quadrature encoders 32 | // Left encoder 33 | pinMode(Left_Encoder_PinA, INPUT); // sets pin A pullup 34 | pinMode(Left_Encoder_PinB, INPUT); // sets pin B pullup 35 | attachInterrupt(digitalPinToInterrupt(Left_Encoder_PinA), do_Left_Encoder, RISING); 36 | 37 | 38 | // Right encoder 39 | pinMode(Right_Encoder_PinA, INPUT); // sets pin A pullup 40 | pinMode(Right_Encoder_PinB, INPUT); // sets pin B pullup 41 | 42 | attachInterrupt(digitalPinToInterrupt(Right_Encoder_PinA), do_Right_Encoder, RISING); 43 | } 44 | 45 | void loop() 46 | { 47 | Update_Encoders(); 48 | } 49 | 50 | 51 | void Update_Encoders() 52 | { 53 | Serial.print("e"); 54 | Serial.print("\t"); 55 | Serial.print(Left_Encoder_Ticks); 56 | Serial.print("\t"); 57 | Serial.print(Right_Encoder_Ticks); 58 | Serial.print("\n"); 59 | } 60 | 61 | void do_Left_Encoder() 62 | { 63 | LeftEncoderBSet = digitalRead(Left_Encoder_PinB); // read the input pin 64 | Left_Encoder_Ticks += LeftEncoderBSet ? -1 : +1; 65 | 66 | } 67 | void do_Right_Encoder() 68 | { 69 | RightEncoderBSet = digitalRead(Right_Encoder_PinB); // read the input pin 70 | Right_Encoder_Ticks += RightEncoderBSet ? -1 : +1; 71 | } 72 | -------------------------------------------------------------------------------- /arduino/simple_motor_test/simple_motor_test.ino: -------------------------------------------------------------------------------- 1 | ///Left Motor Pins 2 | #define INA_1 7 3 | #define INB_1 4 4 | #define PWM_1 5 5 | 6 | ///Right Motor Pins 7 | #define INA_2 8 8 | #define INB_2 12 9 | #define PWM_2 6 10 | 11 | void setup() 12 | { 13 | 14 | //Setting Left Motor pin as OUTPUT 15 | pinMode(INA_1,OUTPUT); 16 | pinMode(INB_1,OUTPUT); 17 | 18 | //Setting Right Motor pin as OUTPUT 19 | pinMode(INA_2,OUTPUT); 20 | pinMode(INB_2,OUTPUT); 21 | } 22 | 23 | 24 | void loop() 25 | { 26 | 27 | //Move forward for 5 sec 28 | move_forward(); 29 | delay(5000); 30 | 31 | //Stop for 1 sec 32 | stop(); 33 | delay(1000); 34 | 35 | //Move backward for 5 sec 36 | move_backward(); 37 | delay(5000); 38 | 39 | //Stop for 1 sec 40 | stop(); 41 | delay(1000); 42 | 43 | //Move left for 5 sec 44 | move_left(); 45 | delay(5000); 46 | 47 | //Stop for 1 sec 48 | stop(); 49 | delay(1000); 50 | 51 | //Move right for 5 sec 52 | move_right(); 53 | delay(5000); 54 | 55 | //Stop for 1 sec 56 | stop(); 57 | delay(1000); 58 | 59 | } 60 | 61 | void move_forward() 62 | { 63 | //Setting CW rotation to and Left Motor and CCW to Right Motor 64 | //Left Motor 65 | 66 | digitalWrite(INA_1,HIGH); 67 | digitalWrite(INB_1,LOW); 68 | analogWrite(PWM_1,255); 69 | 70 | //Right Motor 71 | digitalWrite(INA_2,LOW); 72 | digitalWrite(INB_2,HIGH); 73 | analogWrite(PWM_2,255); 74 | 75 | } 76 | 77 | 78 | 79 | void move_left() 80 | { 81 | 82 | //Left Motor 83 | digitalWrite(INA_1,HIGH); 84 | digitalWrite(INB_1,LOW); 85 | analogWrite(PWM_1,255); 86 | 87 | //Right Motor 88 | digitalWrite(INA_2,HIGH); 89 | digitalWrite(INB_2,HIGH); 90 | analogWrite(PWM_2,0); 91 | 92 | 93 | } 94 | 95 | void move_right() 96 | { 97 | 98 | //Left Motor 99 | digitalWrite(INA_1,HIGH); 100 | digitalWrite(INB_1,HIGH); 101 | analogWrite(PWM_1,0); 102 | 103 | //Right Motor 104 | digitalWrite(INA_2,LOW); 105 | digitalWrite(INB_2,HIGH); 106 | analogWrite(PWM_2,255); 107 | } 108 | 109 | void stop() 110 | { 111 | 112 | //Left Motor 113 | digitalWrite(INA_1,HIGH); 114 | digitalWrite(INB_1,HIGH); 115 | analogWrite(PWM_1,0); 116 | 117 | //Right Motor 118 | digitalWrite(INA_2,HIGH); 119 | digitalWrite(INB_2,HIGH); 120 | analogWrite(PWM_2,0); 121 | } 122 | 123 | ///////////////////////////////////////////////// 124 | 125 | void move_backward() 126 | 127 | { 128 | 129 | //Left Motor 130 | digitalWrite(INA_1,LOW); 131 | digitalWrite(INB_1,HIGH); 132 | analogWrite(PWM_1,255); 133 | 134 | //Right Motor 135 | digitalWrite(INA_2,HIGH); 136 | digitalWrite(INB_2,LOW); 137 | analogWrite(PWM_2,255); 138 | } 139 | -------------------------------------------------------------------------------- /arduino/ultrasonic_sensor/ultrasonic_sensor.ino: -------------------------------------------------------------------------------- 1 | 2 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 3 | //Ultrasonic pins definition 4 | const int echo = A0, Trig = 11; 5 | long duration, cm; 6 | 7 | ///////////////////////////////////////////////////////////////////////////////////////// 8 | //Time update variables 9 | 10 | unsigned long LastUpdateMicrosecs = 0; 11 | unsigned long LastUpdateMillisecs = 0; 12 | unsigned long CurrentMicrosecs = 0; 13 | unsigned long MicrosecsSinceLastUpdate = 0; 14 | float SecondsSinceLastUpdate = 0; 15 | 16 | 17 | void setup() 18 | { 19 | 20 | //Init Serial port with 115200 buad rate 21 | Serial.begin(115200); 22 | SetupUltrasonic(); 23 | 24 | 25 | 26 | } 27 | 28 | 29 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 30 | //Setup UltrasonicsSensor() function 31 | void SetupUltrasonic() 32 | { 33 | pinMode(Trig, OUTPUT); 34 | pinMode(echo, INPUT); 35 | 36 | } 37 | 38 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 39 | //MAIN LOOP 40 | 41 | 42 | 43 | void loop() 44 | { 45 | 46 | Update_Ultra_Sonic(); 47 | delay(300); 48 | 49 | } 50 | 51 | 52 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 53 | //Will update ultrasonic sensors through serial port 54 | 55 | void Update_Ultra_Sonic() 56 | { 57 | digitalWrite(Trig, LOW); 58 | delayMicroseconds(2); 59 | digitalWrite(Trig, HIGH); 60 | delayMicroseconds(10); 61 | digitalWrite(Trig, LOW); 62 | // The echo pin is used to read the signal from the PING))): a HIGH 63 | // pulse whose duration is the time (in microseconds) from the sending 64 | // of the ping to the reception of its echo off of an object. 65 | duration = pulseIn(echo, HIGH); 66 | // convert the time into a distance 67 | cm = microsecondsToCentimeters(duration); 68 | 69 | //Sending through serial port 70 | Serial.print("Distance="); 71 | Serial.print("\t"); 72 | Serial.print(cm); 73 | Serial.print("\n"); 74 | 75 | } 76 | 77 | 78 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 79 | 80 | //Conversion of microsecond to centimeter 81 | long microsecondsToCentimeters(long microseconds) 82 | { 83 | // The speed of sound is 340 m/s or 29 microseconds per centimeter. 84 | // The ping travels out and back, so to find the distance of the 85 | // object we take half of the distance travelled. 86 | return microseconds / 29 / 2; 87 | } 88 | 89 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 90 | 91 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chefbot_bringup) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | actionlib 9 | depthimage_to_laserscan 10 | geometry_msgs 11 | move_base_msgs 12 | nav_msgs 13 | openni_launch 14 | robot_pose_ekf 15 | robot_state_publisher 16 | roscpp 17 | rospy 18 | std_msgs 19 | tf 20 | ) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 42 | ## pulled in transitively but can be declared for certainty nonetheless: 43 | ## * add a build_depend tag for "message_generation" 44 | ## * add a run_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # geometry_msgs# move_base_msgs# nav_msgs# std_msgs 80 | # ) 81 | 82 | ################################### 83 | ## catkin specific configuration ## 84 | ################################### 85 | ## The catkin_package macro generates cmake config files for your package 86 | ## Declare things to be passed to dependent projects 87 | ## INCLUDE_DIRS: uncomment this if you package contains header files 88 | ## LIBRARIES: libraries you create in this project that dependent projects also need 89 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 90 | ## DEPENDS: system dependencies of this project that dependent projects also need 91 | catkin_package( 92 | # INCLUDE_DIRS include 93 | # LIBRARIES chefbot_bringup 94 | # CATKIN_DEPENDS actionlib depthimage_to_laserscan geometry_msgs move_base_msgs nav_msgs openni_launch robot_pose_ekf robot_state_publisher roscpp rospy std_msgs tf 95 | # DEPENDS system_lib 96 | ) 97 | 98 | ########### 99 | ## Build ## 100 | ########### 101 | 102 | ## Specify additional locations of header files 103 | ## Your package locations should be listed before other locations 104 | # include_directories(include) 105 | include_directories( 106 | ${catkin_INCLUDE_DIRS} 107 | ) 108 | 109 | ## Declare a cpp library 110 | # add_library(chefbot_bringup 111 | # src/${PROJECT_NAME}/chefbot_bringup.cpp 112 | # ) 113 | 114 | ## Declare a cpp executable 115 | add_executable(send_goal src/send_robot_goal.cpp) 116 | 117 | ## Add cmake target dependencies of the executable/library 118 | ## as an example, message headers may need to be generated before nodes 119 | # add_dependencies(chefbot_bringup_node chefbot_bringup_generate_messages_cpp) 120 | 121 | ## Specify libraries to link a library or executable target against 122 | target_link_libraries(send_goal 123 | ${catkin_LIBRARIES} 124 | ) 125 | 126 | ############# 127 | ## Install ## 128 | ############# 129 | 130 | # all install targets should use catkin DESTINATION variables 131 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 132 | 133 | ## Mark executable scripts (Python etc.) for installation 134 | ## in contrast to setup.py, you can choose the destination 135 | # install(PROGRAMS 136 | # scripts/my_python_script 137 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 138 | # ) 139 | 140 | ## Mark executables and/or libraries for installation 141 | # install(TARGETS chefbot_bringup chefbot_bringup_node 142 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 143 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 144 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 145 | # ) 146 | 147 | ## Mark cpp header files for installation 148 | # install(DIRECTORY include/${PROJECT_NAME}/ 149 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 150 | # FILES_MATCHING PATTERN "*.h" 151 | # PATTERN ".svn" EXCLUDE 152 | # ) 153 | 154 | ## Mark other files for installation (e.g. launch and bag files, etc.) 155 | # install(FILES 156 | # # myfile1 157 | # # myfile2 158 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 159 | # ) 160 | 161 | ############# 162 | ## Testing ## 163 | ############# 164 | 165 | ## Add gtest based cpp test target and link libraries 166 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_chefbot_bringup.cpp) 167 | # if(TARGET ${PROJECT_NAME}-test) 168 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 169 | # endif() 170 | 171 | ## Add folders to be run by python nosetests 172 | # catkin_add_nosetests(test) 173 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/3dsensor.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 60 | 61 | 62 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/amcl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/amcl.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/gmapping.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/gmapping.launch.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/move_base.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/move_base.launch.xml~: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/safety_controller.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/includes/velocity_smoother.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/keyboard_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/model_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/robot_standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 0.3 25 | 14865 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 400 41 | 100 42 | 0 43 | -1023 44 | 1023 45 | 30 46 | 4 47 | 5 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 400 57 | 100 58 | 0 59 | -1023 60 | 1023 61 | 30 62 | 4 63 | 5 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/view_navigation.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/launch/view_robot.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/map/hotel1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/chefbot/chefbot_bringup/map/hotel1.pgm -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/map/hotel1.yaml: -------------------------------------------------------------------------------- 1 | image: /home/lentin/hotel1.pgm 2 | resolution: 0.050000 3 | origin: [-12.200000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chefbot_bringup 4 | 0.0.0 5 | The chefbot_bringup package 6 | 7 | 8 | 9 | 10 | lentin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | actionlib 44 | depthimage_to_laserscan 45 | geometry_msgs 46 | move_base_msgs 47 | nav_msgs 48 | openni_launch 49 | robot_pose_ekf 50 | robot_state_publisher 51 | roscpp 52 | rospy 53 | std_msgs 54 | tf 55 | actionlib 56 | depthimage_to_laserscan 57 | geometry_msgs 58 | move_base_msgs 59 | nav_msgs 60 | openni_launch 61 | robot_pose_ekf 62 | robot_state_publisher 63 | roscpp 64 | rospy 65 | std_msgs 66 | tf 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/all-wcprops: -------------------------------------------------------------------------------- 1 | K 25 2 | svn:wc:ra_dav:version-url 3 | V 39 4 | /svn/!svn/ver/107/trunk/ros/ardros/info 5 | END 6 | global_costmap_params.yaml 7 | K 25 8 | svn:wc:ra_dav:version-url 9 | V 66 10 | /svn/!svn/ver/105/trunk/ros/ardros/info/global_costmap_params.yaml 11 | END 12 | ardros.yaml 13 | K 25 14 | svn:wc:ra_dav:version-url 15 | V 51 16 | /svn/!svn/ver/105/trunk/ros/ardros/info/ardros.yaml 17 | END 18 | joystick.yaml 19 | K 25 20 | svn:wc:ra_dav:version-url 21 | V 52 22 | /svn/!svn/ver/54/trunk/ros/ardros/info/joystick.yaml 23 | END 24 | costmap_common_params.yaml 25 | K 25 26 | svn:wc:ra_dav:version-url 27 | V 66 28 | /svn/!svn/ver/107/trunk/ros/ardros/info/costmap_common_params.yaml 29 | END 30 | local_costmap_params.yaml 31 | K 25 32 | svn:wc:ra_dav:version-url 33 | V 65 34 | /svn/!svn/ver/105/trunk/ros/ardros/info/local_costmap_params.yaml 35 | END 36 | base_local_planner_params.yaml 37 | K 25 38 | svn:wc:ra_dav:version-url 39 | V 70 40 | /svn/!svn/ver/107/trunk/ros/ardros/info/base_local_planner_params.yaml 41 | END 42 | teleop.yaml 43 | K 25 44 | svn:wc:ra_dav:version-url 45 | V 50 46 | /svn/!svn/ver/54/trunk/ros/ardros/info/teleop.yaml 47 | END 48 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/entries: -------------------------------------------------------------------------------- 1 | 10 2 | 3 | dir 4 | 108 5 | http://drh-robotics-ros.googlecode.com/svn/trunk/ros/ardros/info 6 | http://drh-robotics-ros.googlecode.com/svn 7 | 8 | 9 | 10 | 2012-02-12T02:56:32.317308Z 11 | 107 12 | rainer.hessmer@gmail.com 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 68273211-c61b-7362-29f3-f04ef5221297 28 | 29 | global_costmap_params.yaml 30 | file 31 | 32 | 33 | 34 | 35 | 2014-06-20T12:39:09.847319Z 36 | c68c7ceb113b3b757645ce2752d6989d 37 | 2012-02-05T18:40:17.337678Z 38 | 105 39 | rainer.hessmer@gmail.com 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 174 62 | 63 | ardros.yaml 64 | file 65 | 66 | 67 | 68 | 69 | 2014-06-20T12:39:09.847319Z 70 | e0272821405b642a0586ea07e9f6e6b1 71 | 2012-02-05T18:40:17.337678Z 72 | 105 73 | rainer.hessmer@gmail.com 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 1458 96 | 97 | joystick.yaml 98 | file 99 | 100 | 101 | 102 | 103 | 2014-06-20T12:39:09.839319Z 104 | 94e19b29093627ec1db6b5914ec07583 105 | 2011-02-20T22:28:44.452775Z 106 | 54 107 | rainer.hessmer 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 20 130 | 131 | costmap_common_params.yaml 132 | file 133 | 134 | 135 | 136 | 137 | 2014-06-20T12:39:09.839319Z 138 | 0f6a9214fe2e4c2f8f399d97b8f68a31 139 | 2012-02-12T02:56:32.317308Z 140 | 107 141 | rainer.hessmer@gmail.com 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 672 164 | 165 | local_costmap_params.yaml 166 | file 167 | 168 | 169 | 170 | 171 | 2014-06-20T12:39:09.843319Z 172 | ccac2c8cc4a646b084988cce6dab752d 173 | 2012-02-05T18:40:17.337678Z 174 | 105 175 | rainer.hessmer@gmail.com 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 265 198 | 199 | base_local_planner_params.yaml 200 | file 201 | 202 | 203 | 204 | 205 | 2014-06-20T12:39:09.843319Z 206 | c7bcf81761eadf7d9154ab9a2e00a1fe 207 | 2012-02-12T02:56:32.317308Z 208 | 107 209 | rainer.hessmer@gmail.com 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 2400 232 | 233 | teleop.yaml 234 | file 235 | 236 | 237 | 238 | 239 | 2014-06-20T12:39:09.843319Z 240 | 12e25af8ec151a8aaa088c3bf35e3281 241 | 2011-02-20T22:28:44.452775Z 242 | 54 243 | rainer.hessmer 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 90 266 | 267 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/ardros.yaml.svn-base: -------------------------------------------------------------------------------- 1 | port: /dev/ttyACM0 2 | #port: /dev/ttyUSB0 3 | baudRate: 115200 4 | batteryStateParams: {voltageLowlimit: 12.0, voltageLowLowlimit: 11.7} 5 | 6 | # wheel diameter [m], trackwidth [m], ticks per revolution 7 | driveGeometry: {wheelDiameter: 0.0763, trackWidth: 0.379, countsPerRevolution: 9750} 8 | #driveGeometry: {wheelDiameter: 0.077, trackWidth: 0.373, countsPerRevolution: 9750} 9 | 10 | # The speed controller parameters inlcude the P and I gains for the three PI controllers and the command timeout for the speed controller in seconds. 11 | # If no velocity command arrives for more than the specified timeout then the speed controller will stop the robot. 12 | #speedController: {velocityPParam: 0.1, velocityIParam: 0.1, turnPParam: 0.4, turnIParam: 0.5, commandTimeout: 1.0} 13 | #speedController: {velocityPParam: 0.01, velocityIParam: 0.01, turnPParam: 0.04, turnIParam: 0.05, commandTimeout: 1.0} 14 | #speedController: {velocityPParam: 0.03, velocityIParam: 0.03, turnPParam: 0.04, turnIParam: 0.03, commandTimeout: 1.0} 15 | #speedController: {velocityPParam: 0.02, velocityIParam: 0.06, turnPParam: 0.04, turnIParam: 0.05, commandTimeout: 1.0} 16 | #speedController: {velocityPParam: 1.5, velocityIParam: 0.2, turnPParam: 0.001, turnIParam: 0.02, commandTimeout: 1.0} 17 | #speedController: {velocityPParam: 1.5, velocityIParam: 0.2, turnPParam: 0.1, turnIParam: 0.1, commandTimeout: 1.0} 18 | speedController: {velocityPParam: 1.0, velocityIParam: 0.2, turnPParam: 0.05, turnIParam: 0.05, commandTimeout: 1.0} 19 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/base_local_planner_params.yaml.svn-base: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | # for details see: http://www.ros.org/wiki/base_local_planner 3 | max_vel_x: 0.5 4 | min_vel_x: 0.02 5 | max_rotational_vel: 1.0 # 0.1 rad/sec = 5.7 degree/sec 6 | min_in_place_rotational_vel: 0.2 7 | 8 | acc_lim_th: 1.0 9 | acc_lim_x: 0.2 10 | acc_lim_y: 0.2 11 | 12 | holonomic_robot: false 13 | 14 | # goal tolerance parameters 15 | yaw_goal_tolerance: 0.1 # 0.1 means 5.7 degrees 16 | xy_goal_tolerance: 0.2 17 | latch_xy_goal_tolerance: true 18 | 19 | # Forward Simulation Parameters 20 | sim_time: 1.5 # The amount of time to forward-simulate trajectories in seconds 21 | sim_granularity: 0.025 # The step size, in meters, to take between points on a given trajectory 22 | angular_sim_granularity: 0.05 # The step size, in radians, to take between angular samples on a given trajectory. 23 | vx_samples: 10 # The number of samples to use when exploring the x velocity space 24 | vtheta_samples: 20 # The number of samples to use when exploring the theta velocity space 25 | 26 | 27 | # Trajectory Scoring Parameters 28 | meter_scoring: true # If true, distances are expressed in meters; otherwise grid cells 29 | path_distance_bias: 0.6 # The weighting for how much the controller should stay close to the path it was given 30 | goal_distance_bias: 0.8 # The weighting for how much the controller should attempt to reach its local goal, also controls speed 31 | occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles 32 | # occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254)) 33 | 34 | heading_lookahead: 0.325 # How far to look ahead in meters when scoring different in-place-rotation trajectories 35 | oscillation_reset_dist: 0.05 # How far the robot must travel in meters before oscillation flags are reset 36 | 37 | publish_cost_grid: true 38 | dwa: false # Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout 39 | # (NOTE: In our experience DWA worked as well as Trajectory Rollout and is computationally less expensive. 40 | # It is possible that robots with extremely poor acceleration limits could gain from running Trajectory Rollout, 41 | # but we recommend trying DWA first.) 42 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/costmap_common_params.yaml.svn-base: -------------------------------------------------------------------------------- 1 | # for details see: http://www.ros.org/wiki/navigation/Tutorials/RobotSetup 2 | 3 | obstacle_range: 2.5 4 | raytrace_range: 3.0 5 | footprint: [[0.265, 0.278], [0.04, 0.193], [-0.04, 0.193], [-0.282, 0.178], [-0.282, -0.178], [-0.04, -0.193], [0.04, -0.193], [0.265, -0.278]] 6 | #footprint: [[0.075, 0.178], [0.04, 0.193], [-0.04, 0.193], [-0.282, 0.178], [-0.282, -0.178], [-0.04, -0.193], [0.04, -0.193], [0.075, -0.178]] 7 | #robot_radius: ir_of_robot 8 | inflation_radius: 0.4 9 | 10 | observation_sources: laser_scan_sensor 11 | 12 | laser_scan_sensor: {sensor_frame: openni_depth_frame, data_type: LaserScan, topic: scan, marking: true, clearing: true} 13 | 14 | map_type: costmap 15 | transform_tolerance: 0.4 # seconds 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/global_costmap_params.yaml.svn-base: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: base_link 4 | update_frequency: 2.0 5 | static_map: true 6 | 7 | transform_tolerance: 0.5 # seconds 8 | publish_frequency: 1.0 9 | 10 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/joystick.yaml.svn-base: -------------------------------------------------------------------------------- 1 | dev: /dev/input/js0 2 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/local_costmap_params.yaml.svn-base: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | # origin_x: -0.115 11 | resolution: 0.05 12 | transform_tolerance: 0.5 # seconds 13 | 14 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/.svn/text-base/teleop.yaml.svn-base: -------------------------------------------------------------------------------- 1 | linearAxisIndex: 3 2 | angularAxisIndex: 2 3 | linearScalingFactor: 0.4 4 | angularScalingFactor: 0.5 5 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/ardros.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyACM0 2 | #port: /dev/ttyUSB0 3 | baudRate: 115200 4 | batteryStateParams: {voltageLowlimit: 12.0, voltageLowLowlimit: 11.7} 5 | 6 | # wheel diameter [m], trackwidth [m], ticks per revolution 7 | driveGeometry: {wheelDiameter: 0.09, trackWidth: 0.01, countsPerRevolution: 4200} 8 | #driveGeometry: {wheelDiameter: 0.077, trackWidth: 0.373, countsPerRevolution: 9750} 9 | 10 | # The speed controller parameters inlcude the P and I gains for the three PI controllers and the command timeout for the speed controller in seconds. 11 | # If no velocity command arrives for more than the specified timeout then the speed controller will stop the robot. 12 | #speedController: {velocityPParam: 0.1, velocityIParam: 0.1, turnPParam: 0.4, turnIParam: 0.5, commandTimeout: 1.0} 13 | speedController: {velocityPParam: 0, velocityIParam: 0, turnPParam: 0, turnIParam: 0, commandTimeout: 1.0} 14 | 15 | #speedController: {velocityPParam: 0.01, velocityIParam: 0.01, turnPParam: 0.04, turnIParam: 0.05, commandTimeout: 1.0} 16 | #speedController: {velocityPParam: 0.03, velocityIParam: 0.03, turnPParam: 0.04, turnIParam: 0.03, commandTimeout: 1.0} 17 | #speedController: {velocityPParam: 0.02, velocityIParam: 0.06, turnPParam: 0.04, turnIParam: 0.05, commandTimeout: 1.0} 18 | #speedController: {velocityPParam: 1.5, velocityIParam: 0.2, turnPParam: 0.001, turnIParam: 0.02, commandTimeout: 1.0} 19 | #speedController: {velocityPParam: 1.5, velocityIParam: 0.2, turnPParam: 0.1, turnIParam: 0.1, commandTimeout: 1.0} 20 | #speedController: {velocityPParam: 0.1, velocityIParam: 0.2, turnPParam: 0.05, turnIParam: 0.5, commandTimeout: 1.0} 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | 3 | # Robot Configuration Parameters 4 | max_vel_x: 0.3 5 | min_vel_x: 0.1 6 | 7 | max_vel_theta: 1.0 8 | min_vel_theta: -1.0 9 | min_in_place_vel_theta: 0.6 10 | 11 | acc_lim_x: 0.5 12 | acc_lim_theta: 1.0 13 | 14 | # Goal Tolerance Parameters 15 | yaw_goal_tolerance: 0.3 16 | xy_goal_tolerance: 0.15 17 | 18 | # Forward Simulation Parameters 19 | sim_time: 3.0 20 | vx_samples: 6 21 | vtheta_samples: 20 22 | 23 | # Trajectory Scoring Parameters 24 | meter_scoring: true 25 | pdist_scale: 0.6 26 | gdist_scale: 0.8 27 | occdist_scale: 0.01 28 | heading_lookahead: 0.325 29 | dwa: true 30 | 31 | # Oscillation Prevention Parameters 32 | oscillation_reset_dist: 0.05 33 | 34 | # Differential-drive robot configuration 35 | holonomic_robot: false 36 | max_vel_y: 0.0 37 | min_vel_y: 0.0 38 | acc_lim_y: 0.0 39 | vy_samples: 1 40 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/board_config.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyACM0 2 | #port: /dev/ttyUSB0 3 | baudRate: 115200 4 | 5 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # Customized for ArloBot 2 | max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot 3 | obstacle_range: 2.5 4 | raytrace_range: 3.0 5 | 6 | # Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) 7 | robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki: 0.18) 8 | #robot_radius: 0.4509 # ArloBot 9 | # footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular 10 | inflation_radius: 0.50 # max. distance from an obstacle at which costs are incurred for planning paths. 11 | cost_scaling_factor: 5 # exponential rate at which the obstacle cost drops off (default: 10) 12 | 13 | # voxel map configuration; z-voxels 0 are filled by bumpers and 1 by laser scan (kinect) 14 | map_type: voxel 15 | origin_z: 0.0 16 | z_resolution: 0.2 17 | z_voxels: 2 18 | publish_voxel_map: false 19 | 20 | observation_sources: scan bump 21 | #observation_sources: scan 22 | 23 | # scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35} 24 | # Our lasers (Xtion and fake) either needs to publish a height, or set min_obstacle_height to 0.0: 25 | # http://wiki.ros.org/navigation/Troubleshooting#Missing_Obstacles_in_Costmap2D 26 | # Note taht the max_obstacle_height is very important too! 27 | scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.0, max_obstacle_height: 3} 28 | # Can we just set up two of these here? 29 | bump: {data_type: PointCloud2, topic: mobile_base/sensors/bumper_pointcloud, marking: true, clearing: false, min_obstacle_height: 0.0, max_obstacle_height: 0.15} 30 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | # Robot Configuration Parameters - Kobuki 4 | max_vel_x: 0.5 # 0.55 5 | min_vel_x: 0.0 6 | 7 | max_vel_y: 0.0 # diff drive robot 8 | min_vel_y: 0.0 # diff drive robot 9 | 10 | max_trans_vel: 0.5 # choose slightly less than the base's capability 11 | min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity 12 | trans_stopped_vel: 0.1 13 | 14 | # Warning! 15 | # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities 16 | # are non-negligible and small in place rotational velocities will be created. 17 | 18 | max_rot_vel: 5.0 # choose slightly less than the base's capability 19 | min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity 20 | rot_stopped_vel: 0.4 21 | 22 | acc_lim_x: 1.0 # maximum is theoretically 2.0, but we 23 | acc_lim_theta: 2.0 24 | acc_lim_y: 0.0 # diff drive robot 25 | 26 | # Goal Tolerance Parameters 27 | yaw_goal_tolerance: 0.3 # 0.05 28 | xy_goal_tolerance: 0.15 # 0.10 29 | # latch_xy_goal_tolerance: false 30 | 31 | # Forward Simulation Parameters 32 | sim_time: 1.0 # 1.7 33 | vx_samples: 6 # 3 34 | vy_samples: 1 # diff drive robot, there is only one sample 35 | vtheta_samples: 20 # 20 36 | 37 | # Trajectory Scoring Parameters 38 | path_distance_bias: 64.0 # 32.0 - weighting for how much it should stick to the global path plan 39 | goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal 40 | occdist_scale: 0.50 # 0.01 - weighting for how much the controller should avoid obstacles 41 | forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point 42 | stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. 43 | scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint 44 | max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. 45 | 46 | # Oscillation Prevention Parameters 47 | oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags 48 | 49 | # Debugging 50 | publish_traj_pc : true 51 | publish_cost_grid_pc: true 52 | global_frame_id: odom 53 | 54 | 55 | # Differential-drive robot configuration - necessary? 56 | # holonomic_robot: false 57 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/encoders.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyUSB1 2 | baudRate: 115200 3 | # trackwidth [m], distancePerCount [m] 4 | # http://learn.parallax.com/activitybot/calculating-angles-rotation 5 | # Distance Per Tick for Arlo: http://forums.parallax.com/showthread.php/154274-The-quot-Artist-quot-robot?p=1271544&viewfull=1#post1271544 6 | # Track Width for Arlo is from measurement and then testing 7 | #driveGeometry: {trackWidth: 0.403, distancePerCount: 0.00676} 8 | driveGeometry: {trackWidth: 0.3, distancePerCount: 0.00676} 9 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/joystick.yaml: -------------------------------------------------------------------------------- 1 | dev: /dev/input/js0 2 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: /base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | resolution: 0.05 11 | transform_tolerance: 0.5 12 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | # Move base node parameters. For full documentation of the parameters in this file, please see 2 | # 3 | # http://www.ros.org/wiki/move_base 4 | # 5 | shutdown_costmaps: false 6 | 7 | controller_frequency: 5.0 8 | controller_patience: 3.0 9 | 10 | planner_frequency: 1.0 11 | planner_patience: 5.0 12 | 13 | oscillation_timeout: 10.0 14 | oscillation_distance: 0.2 15 | 16 | # local planner - default is trajectory rollout 17 | base_local_planner: "dwa_local_planner/DWAPlannerROS" 18 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/mux.yaml: -------------------------------------------------------------------------------- 1 | # Created on: Oct 29, 2012 2 | # Author: jorge 3 | # Configuration for subscribers to multiple cmd_vel sources. 4 | # 5 | # Individual subscriber configuration: 6 | # name: Source name 7 | # topic: The topic that provides cmd_vel messages 8 | # timeout: Time in seconds without incoming messages to consider this topic inactive 9 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 10 | # short_desc: Short description (optional) 11 | 12 | subscribers: 13 | - name: "Safe reactive controller" 14 | topic: "input/safety_controller" 15 | timeout: 0.2 16 | priority: 10 17 | - name: "Teleoperation" 18 | topic: "input/teleop" 19 | timeout: 1.0 20 | priority: 7 21 | - name: "Navigation" 22 | topic: "input/navi" 23 | timeout: 1.0 24 | priority: 5 25 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/serial.yaml: -------------------------------------------------------------------------------- 1 | port: /dev/ttyACM0 2 | baudRate: 115200 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/param/teleop.yaml: -------------------------------------------------------------------------------- 1 | linearAxisIndex: 3 2 | angularAxisIndex: 2 3 | linearScalingFactor: 0.4 4 | angularScalingFactor: 0.5 5 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/SerialDataGateway.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | Created on November 20, 2010 4 | 5 | @author: Dr. Rainer Hessmer 6 | ''' 7 | import threading 8 | import serial 9 | from cStringIO import StringIO 10 | import time 11 | import rospy 12 | 13 | def _OnLineReceived(line): 14 | print(line) 15 | 16 | 17 | class SerialDataGateway(object): 18 | ''' 19 | Helper class for receiving lines from a serial port 20 | ''' 21 | 22 | def __init__(self, port="/dev/ttyUSB0", baudrate=115200, lineHandler = _OnLineReceived): 23 | ''' 24 | Initializes the receiver class. 25 | port: The serial port to listen to. 26 | receivedLineHandler: The function to call when a line was received. 27 | ''' 28 | self._Port = port 29 | self._Baudrate = baudrate 30 | self.ReceivedLineHandler = lineHandler 31 | self._KeepRunning = False 32 | 33 | def Start(self): 34 | self._Serial = serial.Serial(port = self._Port, baudrate = self._Baudrate, timeout = 1) 35 | 36 | self._KeepRunning = True 37 | self._ReceiverThread = threading.Thread(target=self._Listen) 38 | self._ReceiverThread.setDaemon(True) 39 | self._ReceiverThread.start() 40 | 41 | def Stop(self): 42 | rospy.loginfo("Stopping serial gateway") 43 | self._KeepRunning = False 44 | time.sleep(.1) 45 | self._Serial.close() 46 | 47 | def _Listen(self): 48 | stringIO = StringIO() 49 | while self._KeepRunning: 50 | data = self._Serial.read() 51 | if data == '\r': 52 | pass 53 | if data == '\n': 54 | self.ReceivedLineHandler(stringIO.getvalue()) 55 | stringIO.close() 56 | stringIO = StringIO() 57 | else: 58 | stringIO.write(data) 59 | 60 | def Write(self, data): 61 | info = "Writing to serial port: %s" %data 62 | rospy.loginfo(info) 63 | self._Serial.write(data) 64 | 65 | if __name__ == '__main__': 66 | dataReceiver = SerialDataGateway("/dev/ttyUSB0", 115200) 67 | dataReceiver.Start() 68 | 69 | raw_input("Hit to end.") 70 | dataReceiver.Stop() 71 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/SerialDataGateway.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/chefbot/chefbot_bringup/scripts/SerialDataGateway.pyc -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/bkup_working/SerialDataGateway.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/chefbot/chefbot_bringup/scripts/bkup_working/SerialDataGateway.pyc -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/bkup_working/err: -------------------------------------------------------------------------------- 1 | [INFO] [WallTime: 1419579912.152619] /twist_to_motors started 2 | [INFO] [WallTime: 1419579911.666583] /lpid_velocity started 3 | [INFO] [WallTime: 1419579912.081762] /rpid_velocity started 4 | [INFO] [WallTime: 1419579912.984866] -I- /diff_tf started 5 | ... logging to /home/lentin/.ros/log/1d3bc780-8cd3-11e4-ba0e-9439e54d7dda/roslaunch-lentin-Aspire-4755-15144.log 6 | Checking log directory for disk usage. This may take awhile. 7 | Press Ctrl-C to interrupt 8 | Done checking log file disk usage. Usage is <1GB. 9 | ]2;/home/lentin/catkin_ws/src/launchpad_robot_bringup/launch/robot_standalone.launch 10 | started roslaunch server http://lentin-Aspire-4755:57837/ 11 | 12 | SUMMARY 13 | ======== 14 | 15 | PARAMETERS 16 | * /launchpad_node/batteryStateParams/voltageLowLowlimit: 11.7 17 | * /launchpad_node/batteryStateParams/voltageLowlimit: 12.0 18 | * /launchpad_node/baudRate: 115200 19 | * /launchpad_node/driveGeometry/countsPerRevolution: 4200 20 | * /launchpad_node/driveGeometry/trackWidth: 0.01 21 | * /launchpad_node/driveGeometry/wheelDiameter: 0.09 22 | * /launchpad_node/port: /dev/ttyACM0 23 | * /launchpad_node/speedController/commandTimeout: 1.0 24 | * /launchpad_node/speedController/turnIParam: 0 25 | * /launchpad_node/speedController/turnPParam: 0 26 | * /launchpad_node/speedController/velocityIParam: 0 27 | * /launchpad_node/speedController/velocityPParam: 0 28 | * /lpid_velocity/Kd: 10 29 | * /lpid_velocity/Ki: 100 30 | * /lpid_velocity/Kp: 500 31 | * /lpid_velocity/out_max: 1023 32 | * /lpid_velocity/out_min: -1023 33 | * /lpid_velocity/rate: 30 34 | * /lpid_velocity/rolling_pts: 5 35 | * /lpid_velocity/timeout_ticks: 4 36 | * /robot/name: turtlebot 37 | * /robot/type: turtlebot 38 | * /robot_description: . 23 | 24 | import rospy 25 | import roslib 26 | from std_msgs.msg import Float32 27 | from std_msgs.msg import Int16 28 | 29 | ################################################ 30 | ################################################ 31 | class WheelLoopback(): 32 | ################################################ 33 | ################################################ 34 | 35 | ############################################### 36 | def __init__(self): 37 | ############################################### 38 | rospy.init_node("wheel_loopback"); 39 | self.nodename = rospy.get_name() 40 | rospy.loginfo("%s started" % self.nodename) 41 | 42 | self.rate = rospy.get_param("~rate", 200) 43 | self.timeout_secs = rospy.get_param("~timeout_secs", 0.5) 44 | self.ticks_meter = float(rospy.get_param('~ticks_meter', 50)) 45 | self.velocity_scale = float(rospy.get_param('~velocity_scale', 255)) 46 | self.latest_motor = 0 47 | 48 | self.wheel = 0 49 | 50 | rospy.Subscriber('motor', Int16, self.motor_callback) 51 | 52 | self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10) 53 | 54 | ############################################### 55 | def spin(self): 56 | ############################################### 57 | r = rospy.Rate 58 | self.secs_since_target = self.timeout_secs 59 | self.then = rospy.Time.now() 60 | self.latest_msg_time = rospy.Time.now() 61 | rospy.loginfo("-D- spinning") 62 | 63 | ###### main loop ######### 64 | while not rospy.is_shutdown(): 65 | while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs: 66 | self.spinOnce() 67 | r.sleep 68 | self.secs_since_target = rospy.Time.now() - self.latest_msg_time 69 | self.secs_since_target = self.secs_since_target.to_sec() 70 | #rospy.loginfo(" inside: secs_since_target: %0.3f" % self.secs_since_target) 71 | 72 | # it's been more than timeout_ticks since we recieved a message 73 | self.secs_since_target = rospy.Time.now() - self.latest_msg_time 74 | self.secs_since_target = self.secs_since_target.to_sec() 75 | # rospy.loginfo(" outside: secs_since_target: %0.3f" % self.secs_since_target) 76 | self.velocity = 0 77 | r.sleep 78 | 79 | ############################################### 80 | def spinOnce(self): 81 | ############################################### 82 | self.velocity = self.latest_motor / self.velocity_scale 83 | if abs(self.velocity) > 0: 84 | self.seconds_per_tick = abs( 1 / (self.velocity * self.ticks_meter)) 85 | elapsed = rospy.Time.now() - self.then 86 | elapsed = elapsed.to_sec() 87 | rospy.loginfo("spinOnce: vel=%0.3f sec/tick=%0.3f elapsed:%0.3f" % (self.velocity, self.seconds_per_tick, elapsed)) 88 | 89 | if (elapsed > self.seconds_per_tick): 90 | rospy.loginfo("incrementing wheel") 91 | if self.velocity > 0: 92 | self.wheel += 1 93 | else: 94 | self.wheel -= 1 95 | self.pub_wheel.publish(self.wheel) 96 | self.then = rospy.Time.now() 97 | 98 | 99 | 100 | ############################################### 101 | def motor_callback(self, msg): 102 | ############################################### 103 | # rospy.loginfo("%s recieved %d" % (self.nodename, msg.data)) 104 | self.latest_motor = msg.data 105 | self.latest_msg_time = rospy.Time.now() 106 | 107 | 108 | 109 | ################################################ 110 | ################################################ 111 | if __name__ == '__main__': 112 | """ main """ 113 | wheelLoopback = WheelLoopback() 114 | wheelLoopback.spin() 115 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/bkup_working/wheel_scaler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | wheel_scaler 5 | scales the wheel readings (and inverts the sign) 6 | 7 | Copyright (C) 2012 Jon Stephan. 8 | 9 | This program is free software: you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation, either version 3 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details. 18 | 19 | You should have received a copy of the GNU General Public License 20 | along with this program. If not, see . 21 | 22 | """ 23 | 24 | import rospy 25 | import roslib 26 | 27 | from std_msgs.msg import Int16 28 | 29 | def lwheelCallback(msg): 30 | lscaled_pub.publish( msg.data * -1 * scale) 31 | 32 | def rwheelCallback(msg): 33 | rscaled_pub.publish( msg.data * -1 * scale) 34 | 35 | 36 | if __name__ == '__main__': 37 | """main""" 38 | rospy.init_node("wheel_scaler") 39 | rospy.loginfo("wheel_scaler started") 40 | 41 | scale = rospy.get_param('distance_scale', 1) 42 | rospy.loginfo("wheel_scaler scale: %0.2f", scale) 43 | 44 | rospy.Subscriber("lwheel", Int16, lwheelCallback) 45 | rospy.Subscriber("rwheel", Int16, rwheelCallback) 46 | 47 | lscaled_pub = rospy.Publisher("lwheel_scaled", Int16,queue_size=10) 48 | rscaled_pub = rospy.Publisher("rwheel_scaled", Int16,queue_size=10) 49 | 50 | ### testing sleep CPU usage 51 | r = rospy.Rate(50) 52 | while not rospy.is_shutdown: 53 | r.sleep() 54 | 55 | rospy.spin() 56 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/chefbot_teleop_key: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Willow Garage, Inc. nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | 30 | import roslib 31 | #roslib.load_manifest('turtlebot_teleop') # No longer needed in catkin! 32 | import rospy 33 | 34 | from geometry_msgs.msg import Twist 35 | 36 | import sys, select, termios, tty 37 | 38 | msg = """ 39 | Control Your Turtlebot! 40 | --------------------------- 41 | Moving around: 42 | u i o 43 | j k l 44 | m , . 45 | 46 | q/z : increase/decrease max speeds by 10% 47 | w/x : increase/decrease only linear speed by 10% 48 | e/c : increase/decrease only angular speed by 10% 49 | space key, k : force stop 50 | anything else : stop smoothly 51 | 52 | CTRL-C to quit 53 | """ 54 | 55 | moveBindings = { 56 | 'i':(1,0), 57 | 'o':(1,-1), 58 | 'j':(0,1), 59 | 'l':(0,-1), 60 | 'u':(1,1), 61 | ',':(-1,0), 62 | '.':(-1,1), 63 | 'm':(-1,-1), 64 | } 65 | 66 | speedBindings={ 67 | 'q':(1.1,1.1), 68 | 'z':(.9,.9), 69 | 'w':(1.1,1), 70 | 'x':(.9,1), 71 | 'e':(1,1.1), 72 | 'c':(1,.9), 73 | } 74 | 75 | def getKey(): 76 | tty.setraw(sys.stdin.fileno()) 77 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 78 | if rlist: 79 | key = sys.stdin.read(1) 80 | else: 81 | key = '' 82 | 83 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 84 | return key 85 | 86 | #speed = .2 87 | speed = 1 88 | turn = 1 89 | 90 | def vels(speed,turn): 91 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 92 | 93 | if __name__=="__main__": 94 | settings = termios.tcgetattr(sys.stdin) 95 | 96 | rospy.init_node('chefbot_teleop') 97 | pub = rospy.Publisher('~cmd_vel', Twist, queue_size=5) 98 | 99 | x = 0 100 | th = 0 101 | status = 0 102 | count = 0 103 | acc = 0.1 104 | target_speed = 0 105 | target_turn = 0 106 | control_speed = 0 107 | control_turn = 0 108 | try: 109 | print msg 110 | print vels(speed,turn) 111 | while(1): 112 | key = getKey() 113 | if key in moveBindings.keys(): 114 | x = moveBindings[key][0] 115 | th = moveBindings[key][1] 116 | count = 0 117 | elif key in speedBindings.keys(): 118 | speed = speed * speedBindings[key][0] 119 | turn = turn * speedBindings[key][1] 120 | count = 0 121 | 122 | print vels(speed,turn) 123 | if (status == 14): 124 | print msg 125 | status = (status + 1) % 15 126 | elif key == ' ' or key == 'k' : 127 | x = 0 128 | th = 0 129 | control_speed = 0 130 | control_turn = 0 131 | else: 132 | count = count + 1 133 | if count > 4: 134 | x = 0 135 | th = 0 136 | if (key == '\x03'): 137 | break 138 | 139 | target_speed = speed * x 140 | target_turn = turn * th 141 | 142 | if target_speed > control_speed: 143 | control_speed = min( target_speed, control_speed + 0.02 ) 144 | elif target_speed < control_speed: 145 | control_speed = max( target_speed, control_speed - 0.02 ) 146 | else: 147 | control_speed = target_speed 148 | 149 | if target_turn > control_turn: 150 | control_turn = min( target_turn, control_turn + 0.1 ) 151 | elif target_turn < control_turn: 152 | control_turn = max( target_turn, control_turn - 0.1 ) 153 | else: 154 | control_turn = target_turn 155 | 156 | twist = Twist() 157 | twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 158 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn 159 | pub.publish(twist) 160 | 161 | #print("loop: {0}".format(count)) 162 | #print("target: vx: {0}, wz: {1}".format(target_speed, target_turn)) 163 | #print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z)) 164 | 165 | except: 166 | print e 167 | 168 | finally: 169 | twist = Twist() 170 | twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 171 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 172 | pub.publish(twist) 173 | 174 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 175 | 176 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/simple_navig_goals.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import actionlib 5 | 6 | import sys 7 | 8 | #move_base_msgs 9 | from move_base_msgs.msg import * 10 | 11 | def simple_move(x,w): 12 | 13 | rospy.init_node('simple_move') 14 | 15 | #Simple Action Client 16 | sac = actionlib.SimpleActionClient('move_base', MoveBaseAction ) 17 | 18 | #create goal 19 | goal = MoveBaseGoal() 20 | 21 | #use self? 22 | #set goal 23 | 24 | rospy.loginfo("Set X = "+x) 25 | rospy.loginfo("Set W = "+w) 26 | 27 | goal.target_pose.pose.position.x = float(x) 28 | goal.target_pose.pose.orientation.w = float(w) 29 | goal.target_pose.header.frame_id = 'first_move' 30 | goal.target_pose.header.stamp = rospy.Time.now() 31 | 32 | #start listner 33 | rospy.loginfo("Waiting for server") 34 | 35 | sac.wait_for_server() 36 | 37 | 38 | rospy.loginfo("Sending Goals") 39 | 40 | #send goal 41 | 42 | sac.send_goal(goal) 43 | rospy.loginfo("Waiting for server") 44 | 45 | #finish 46 | sac.wait_for_result() 47 | 48 | #print result 49 | print sac.get_result() 50 | 51 | 52 | if __name__ == '__main__': 53 | try: 54 | simple_move(sys.argv[1],sys.argv[2]) 55 | except rospy.ROSInterruptException: 56 | print "Keyboard Interrupt" 57 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/scripts/twist_to_motors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | twist_to_motors - converts a twist message to motor commands. Needed for navigation stack 4 | 5 | 6 | Copyright (C) 2012 Jon Stephan. 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | This program is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License 19 | along with this program. If not, see . 20 | """ 21 | 22 | import rospy 23 | import roslib 24 | from std_msgs.msg import Float32 25 | from geometry_msgs.msg import Twist 26 | 27 | ############################################################# 28 | ############################################################# 29 | class TwistToMotors(): 30 | ############################################################# 31 | ############################################################# 32 | 33 | ############################################################# 34 | def __init__(self): 35 | ############################################################# 36 | rospy.init_node("twist_to_motors") 37 | nodename = rospy.get_name() 38 | rospy.loginfo("%s started" % nodename) 39 | 40 | self.w = rospy.get_param("~base_width", 0.2) 41 | 42 | self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10) 43 | self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10) 44 | rospy.Subscriber('cmd_vel', Twist, self.twistCallback) 45 | 46 | 47 | self.rate = rospy.get_param("~rate", 50) 48 | self.timeout_ticks = rospy.get_param("~timeout_ticks", 2) 49 | self.left = 0 50 | self.right = 0 51 | 52 | ############################################################# 53 | def spin(self): 54 | ############################################################# 55 | 56 | r = rospy.Rate(self.rate) 57 | idle = rospy.Rate(10) 58 | then = rospy.Time.now() 59 | self.ticks_since_target = self.timeout_ticks 60 | 61 | ###### main loop ###### 62 | while not rospy.is_shutdown(): 63 | 64 | while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: 65 | self.spinOnce() 66 | r.sleep() 67 | idle.sleep() 68 | 69 | ############################################################# 70 | def spinOnce(self): 71 | ############################################################# 72 | 73 | # dx = (l + r) / 2 74 | # dr = (r - l) / w 75 | 76 | self.right = 1.0 * self.dx + self.dr * self.w / 2 77 | self.left = 1.0 * self.dx - self.dr * self.w / 2 78 | # rospy.loginfo("publishing: (%d, %d)", left, right) 79 | 80 | self.pub_lmotor.publish(self.left) 81 | self.pub_rmotor.publish(self.right) 82 | 83 | self.ticks_since_target += 1 84 | 85 | ############################################################# 86 | def twistCallback(self,msg): 87 | ############################################################# 88 | # rospy.loginfo("-D- twistCallback: %s" % str(msg)) 89 | self.ticks_since_target = 0 90 | self.dx = msg.linear.x 91 | self.dr = msg.angular.z 92 | self.dy = msg.linear.y 93 | 94 | ############################################################# 95 | ############################################################# 96 | if __name__ == '__main__': 97 | """ main """ 98 | twistToMotors = TwistToMotors() 99 | twistToMotors.spin() 100 | -------------------------------------------------------------------------------- /chefbot/chefbot_bringup/src/send_robot_goal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | typedef actionlib::SimpleActionClient MoveBaseClient; 9 | 10 | int main(int argc, char** argv){ 11 | ros::init(argc, argv, "navigation_goals"); 12 | 13 | MoveBaseClient ac("move_base", true); 14 | 15 | while(!ac.waitForServer(ros::Duration(5.0))){ 16 | ROS_INFO("Waiting for the move_base action server"); 17 | } 18 | 19 | move_base_msgs::MoveBaseGoal goal; 20 | 21 | goal.target_pose.header.frame_id = "map"; 22 | goal.target_pose.header.stamp = ros::Time::now(); 23 | try{ 24 | goal.target_pose.pose.position.x = atof(argv[1]); 25 | goal.target_pose.pose.position.y = atof(argv[2]); 26 | goal.target_pose.pose.orientation.w = atof(argv[3]); 27 | } 28 | catch(int e){ 29 | 30 | 31 | goal.target_pose.pose.position.x = 1.0; 32 | goal.target_pose.pose.position.y = 1.0; 33 | goal.target_pose.pose.orientation.w = 1.0; 34 | 35 | 36 | } 37 | ROS_INFO("Sending move base goal"); 38 | ac.sendGoal(goal); 39 | 40 | ac.waitForResult(); 41 | 42 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 43 | ROS_INFO("Robot has arrived to the goal position"); 44 | else{ 45 | ROS_INFO("The base failed for some reason"); 46 | } 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chefbot_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | urdf 9 | xacro 10 | kobuki_description 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 33 | ## pulled in transitively but can be declared for certainty nonetheless: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs # Or other packages containing msgs 71 | # ) 72 | 73 | ################################### 74 | ## catkin specific configuration ## 75 | ################################### 76 | ## The catkin_package macro generates cmake config files for your package 77 | ## Declare things to be passed to dependent projects 78 | ## INCLUDE_DIRS: uncomment this if you package contains header files 79 | ## LIBRARIES: libraries you create in this project that dependent projects also need 80 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 81 | ## DEPENDS: system dependencies of this project that dependent projects also need 82 | catkin_package( 83 | # INCLUDE_DIRS include 84 | # LIBRARIES chefbot_description 85 | # CATKIN_DEPENDS urdf xacro 86 | # DEPENDS system_lib 87 | ) 88 | 89 | ########### 90 | ## Build ## 91 | ########### 92 | 93 | ## Specify additional locations of header files 94 | ## Your package locations should be listed before other locations 95 | # include_directories(include) 96 | include_directories( 97 | ${catkin_INCLUDE_DIRS} 98 | ) 99 | 100 | ## Declare a cpp library 101 | # add_library(chefbot_description 102 | # src/${PROJECT_NAME}/chefbot_description.cpp 103 | # ) 104 | 105 | ## Declare a cpp executable 106 | # add_executable(chefbot_description_node src/chefbot_description_node.cpp) 107 | 108 | ## Add cmake target dependencies of the executable/library 109 | ## as an example, message headers may need to be generated before nodes 110 | # add_dependencies(chefbot_description_node chefbot_description_generate_messages_cpp) 111 | 112 | ## Specify libraries to link a library or executable target against 113 | # target_link_libraries(chefbot_description_node 114 | # ${catkin_LIBRARIES} 115 | # ) 116 | 117 | ############# 118 | ## Install ## 119 | ############# 120 | 121 | # all install targets should use catkin DESTINATION variables 122 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 123 | 124 | ## Mark executable scripts (Python etc.) for installation 125 | ## in contrast to setup.py, you can choose the destination 126 | # install(PROGRAMS 127 | # scripts/my_python_script 128 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 129 | # ) 130 | 131 | ## Mark executables and/or libraries for installation 132 | # install(TARGETS chefbot_description chefbot_description_node 133 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 134 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 135 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 136 | # ) 137 | 138 | ## Mark cpp header files for installation 139 | # install(DIRECTORY include/${PROJECT_NAME}/ 140 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 141 | # FILES_MATCHING PATTERN "*.h" 142 | # PATTERN ".svn" EXCLUDE 143 | # ) 144 | 145 | ## Mark other files for installation (e.g. launch and bag files, etc.) 146 | # install(FILES 147 | # # myfile1 148 | # # myfile2 149 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 150 | # ) 151 | 152 | ############# 153 | ## Testing ## 154 | ############# 155 | 156 | ## Add gtest based cpp test target and link libraries 157 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_chefbot_description.cpp) 158 | # if(TARGET ${PROJECT_NAME}-test) 159 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 160 | # endif() 161 | 162 | ## Add folders to be run by python nosetests 163 | # catkin_add_nosetests(test) 164 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/meshes/create_body.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/chefbot/chefbot_description/meshes/create_body.tga -------------------------------------------------------------------------------- /chefbot/chefbot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chefbot_description 4 | 0.0.0 5 | The chefbot_description package 6 | 7 | 8 | 9 | 10 | lentin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | urdf 44 | xacro 45 | 46 | urdf 47 | xacro 48 | kobuki_description 49 | create_description 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/robots/chefbot_circles_kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/robots/roomba_circles_kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/bkup/chefbot_gazebo.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | true 8 | 20.0 9 | 10 | ${60.0*M_PI/180.0} 11 | 12 | R8G8B8 13 | 640 14 | 480 15 | 16 | 17 | 0.05 18 | 8.0 19 | 20 | 21 | 22 | camera 23 | true 24 | 10 25 | rgb/image_raw 26 | depth/image_raw 27 | depth/points 28 | rgb/camera_info 29 | depth/camera_info 30 | camera_depth_optical_frame 31 | 0.1 32 | 0.0 33 | 0.0 34 | 0.0 35 | 0.0 36 | 0.0 37 | 0.4 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/bkup/chefbot_library.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/bkup/chefbot_properties.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | 11 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/bkup/common_properties.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/chefbot_base.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/chefbot_gazebo.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | true 8 | 20.0 9 | 10 | ${60.0*M_PI/180.0} 11 | 12 | R8G8B8 13 | 640 14 | 480 15 | 16 | 17 | 0.05 18 | 8.0 19 | 20 | 21 | 22 | camera 23 | true 24 | 10 25 | rgb/image_raw 26 | depth/image_raw 27 | depth/points 28 | rgb/camera_info 29 | depth/camera_info 30 | camera_depth_optical_frame 31 | 0.1 32 | 0.0 33 | 0.0 34 | 0.0 35 | 0.0 36 | 0.0 37 | 0.4 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/chefbot_library.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/chefbot_properties.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | 11 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/chefbot_rviz_library.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/common_properties.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/sensors/asus_xtion_pro.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | f.xacro"/> 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /chefbot/chefbot_description/urdf/sensors/kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 61 | 62 | 63 | 64 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chefbot_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | depthimage_to_laserscan 9 | diagnostic_aggregator 10 | gazebo_ros 11 | kobuki_gazebo_plugins 12 | robot_pose_ekf 13 | robot_state_publisher 14 | xacro 15 | yocs_cmd_vel_mux 16 | 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 39 | ## pulled in transitively but can be declared for certainty nonetheless: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # std_msgs # Or other packages containing msgs 77 | # ) 78 | 79 | ################################### 80 | ## catkin specific configuration ## 81 | ################################### 82 | ## The catkin_package macro generates cmake config files for your package 83 | ## Declare things to be passed to dependent projects 84 | ## INCLUDE_DIRS: uncomment this if you package contains header files 85 | ## LIBRARIES: libraries you create in this project that dependent projects also need 86 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 87 | ## DEPENDS: system dependencies of this project that dependent projects also need 88 | catkin_package( 89 | # INCLUDE_DIRS include 90 | # LIBRARIES chefbot_gazebo 91 | # CATKIN_DEPENDS chefbot_bringup chefbot_description depthimage_to_laserscan diagnostic_aggregator gazebo_ros kobuki_gazebo_plugins robot_pose_ekf robot_state_publisher xacro yocs_cmd_vel_mux 92 | # DEPENDS system_lib 93 | ) 94 | 95 | ########### 96 | ## Build ## 97 | ########### 98 | 99 | ## Specify additional locations of header files 100 | ## Your package locations should be listed before other locations 101 | # include_directories(include) 102 | include_directories( 103 | ${catkin_INCLUDE_DIRS} 104 | ) 105 | 106 | ## Declare a cpp library 107 | # add_library(chefbot_gazebo 108 | # src/${PROJECT_NAME}/chefbot_gazebo.cpp 109 | # ) 110 | 111 | ## Declare a cpp executable 112 | # add_executable(chefbot_gazebo_node src/chefbot_gazebo_node.cpp) 113 | 114 | ## Add cmake target dependencies of the executable/library 115 | ## as an example, message headers may need to be generated before nodes 116 | # add_dependencies(chefbot_gazebo_node chefbot_gazebo_generate_messages_cpp) 117 | 118 | ## Specify libraries to link a library or executable target against 119 | # target_link_libraries(chefbot_gazebo_node 120 | # ${catkin_LIBRARIES} 121 | # ) 122 | 123 | ############# 124 | ## Install ## 125 | ############# 126 | 127 | # all install targets should use catkin DESTINATION variables 128 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 129 | 130 | ## Mark executable scripts (Python etc.) for installation 131 | ## in contrast to setup.py, you can choose the destination 132 | # install(PROGRAMS 133 | # scripts/my_python_script 134 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 135 | # ) 136 | 137 | ## Mark executables and/or libraries for installation 138 | # install(TARGETS chefbot_gazebo chefbot_gazebo_node 139 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 140 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 141 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 142 | # ) 143 | 144 | ## Mark cpp header files for installation 145 | # install(DIRECTORY include/${PROJECT_NAME}/ 146 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 147 | # FILES_MATCHING PATTERN "*.h" 148 | # PATTERN ".svn" EXCLUDE 149 | # ) 150 | 151 | ## Mark other files for installation (e.g. launch and bag files, etc.) 152 | # install(FILES 153 | # # myfile1 154 | # # myfile2 155 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 156 | # ) 157 | 158 | ############# 159 | ## Testing ## 160 | ############# 161 | 162 | ## Add gtest based cpp test target and link libraries 163 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_chefbot_gazebo.cpp) 164 | # if(TARGET ${PROJECT_NAME}-test) 165 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 166 | # endif() 167 | 168 | ## Add folders to be run by python nosetests 169 | # catkin_add_nosetests(test) 170 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/amcl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/chefbot_empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/chefbot_hotel_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/chefbot_playground.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/chefbot_room_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | --> 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/includes/chefbot_base.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/launch/includes/chefbot_base.launch.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/maps/playground.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/chefbot/chefbot_simulator/chefbot_gazebo/maps/playground.pgm -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/maps/playground.yaml: -------------------------------------------------------------------------------- 1 | free_thresh: 0.196 2 | image: playground.pgm 3 | negate: 0 4 | occupied_thresh: 0.65 5 | origin: [-6.8999999999999915, -5.8999999999999915, 0.0] 6 | resolution: 0.05 7 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/maps/room.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/chefbot/chefbot_simulator/chefbot_gazebo/maps/room.pgm -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/maps/room.yaml: -------------------------------------------------------------------------------- 1 | image: room.pgm 2 | resolution: 0.010000 3 | origin: [-11.560000, -11.240000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chefbot_gazebo 4 | 0.0.0 5 | The chefbot_gazebo package 6 | 7 | 8 | 9 | 10 | lentin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 54 | 56 | depthimage_to_laserscan 57 | diagnostic_aggregator 58 | gazebo_ros 59 | kobuki_gazebo_plugins 60 | robot_pose_ekf 61 | robot_state_publisher 62 | xacro 63 | yocs_cmd_vel_mux 64 | create_gazebo_plugins 65 | create_description 66 | create_driver 67 | create_node 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /chefbot/chefbot_simulator/chefbot_gazebo/worlds/empty_.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 0.01 15 | 1 16 | 100 17 | 0 0 -9.8 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /tiva_c_energia_code_final/MPU6050.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/tiva_c_energia_code_final/MPU6050.zip -------------------------------------------------------------------------------- /tiva_c_energia_code_final/Messenger.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qboticslabs/Chefbot_ROS_pkg/3d389a1cafcdbe7ff2e9930f8ca82cea4b5cea46/tiva_c_energia_code_final/Messenger.zip --------------------------------------------------------------------------------