├── .gitignore ├── vesc ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── vesc_msgs ├── msg │ ├── VescStateStamped.msg │ └── VescState.msg ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── README.md ├── vesc_driver ├── vesc_driver_nodelet.xml ├── launch │ ├── vesc_driver_node.launch │ └── vesc_driver_nodelet.launch ├── CHANGELOG.rst ├── package.xml ├── src │ ├── vesc_driver_node.cpp │ ├── vesc_driver_nodelet.cpp │ ├── vesc_packet_factory.cpp │ ├── vesc_interface.cpp │ ├── vesc_driver.cpp │ └── vesc_packet.cpp ├── CMakeLists.txt └── include │ └── vesc_driver │ ├── vesc_driver.h │ ├── vesc_packet_factory.h │ ├── vesc_interface.h │ ├── datatypes.h │ ├── vesc_packet.h │ └── crc.h ├── vesc_ackermann ├── vesc_ackermann_nodelet.xml ├── launch │ ├── vesc_to_odom_node.launch │ ├── ackermann_to_vesc_node.launch │ ├── vesc_to_odom_nodelet.launch │ └── ackermann_to_vesc_nodelet.launch ├── CHANGELOG.rst ├── package.xml ├── src │ ├── vesc_to_odom_node.cpp │ ├── ackermann_to_vesc_node.cpp │ ├── vesc_to_odom_nodelet.cpp │ ├── ackermann_to_vesc_nodelet.cpp │ ├── ackermann_to_vesc.cpp │ └── vesc_to_odom.cpp ├── include │ └── vesc_ackermann │ │ ├── ackermann_to_vesc.h │ │ └── vesc_to_odom.h └── CMakeLists.txt ├── .github └── workflows │ └── ros1-ci.yaml └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | -------------------------------------------------------------------------------- /vesc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vesc) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /vesc_msgs/msg/VescStateStamped.msg: -------------------------------------------------------------------------------- 1 | # Timestamped VESC open source motor controller state (telemetry) 2 | 3 | Header header 4 | VescState state -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Veddar VESC Interface 2 | 3 | ![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg) 4 | 5 | Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details 6 | -------------------------------------------------------------------------------- /vesc_driver/vesc_driver_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Vedder VESC motor controller interface nodelet. 4 | 5 | 6 | -------------------------------------------------------------------------------- /vesc_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vesc_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | message_generation 7 | ) 8 | 9 | add_message_files( 10 | DIRECTORY msg 11 | FILES 12 | VescState.msg 13 | VescStateStamped.msg 14 | ) 15 | generate_messages( 16 | DEPENDENCIES 17 | std_msgs 18 | ) 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS std_msgs message_runtime 22 | ) 23 | -------------------------------------------------------------------------------- /vesc_ackermann/vesc_ackermann_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Nodelet for translating ackermann messages to VESC motor controller commands. 4 | 5 | 6 | Nodelet for translating VESC motor controller state to odometry messages. 7 | 8 | 9 | -------------------------------------------------------------------------------- /vesc_ackermann/launch/vesc_to_odom_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /vesc/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package vesc 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2020-12-12) 6 | ------------------ 7 | * Merge pull request `#1 `_ from f1tenth/melodic-devel 8 | Updating for Melodic 9 | * Updating package.xml to format 3 and setting C++ standard to 11. 10 | * Contributors: Joshua Whitley 11 | 12 | 1.0.0 (2020-12-02) 13 | ------------------ 14 | * Updating maintainers, authors, and URLs. 15 | * added onboard car 16 | * Contributors: Joshua Whitley, billyzheng 17 | -------------------------------------------------------------------------------- /vesc_ackermann/launch/ackermann_to_vesc_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /vesc_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package vesc_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2020-12-12) 6 | ------------------ 7 | * Merge pull request `#1 `_ from f1tenth/melodic-devel 8 | Updating for Melodic 9 | * Updating package.xml to format 3 and setting C++ standard to 11. 10 | * Contributors: Joshua Whitley 11 | 12 | 1.0.0 (2020-12-02) 13 | ------------------ 14 | * Adding roslint. 15 | * Updating maintainers, authors, and URLs. 16 | * added onboard car 17 | * Contributors: Joshua Whitley, billyzheng 18 | -------------------------------------------------------------------------------- /vesc_driver/launch/vesc_driver_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /vesc_ackermann/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package vesc_ackermann 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2020-12-12) 6 | ------------------ 7 | * Merge pull request `#1 `_ from f1tenth/melodic-devel 8 | Updating for Melodic 9 | * Remove unused Boost dependency. 10 | * Replacing boost::shared_ptr with Standard Library equivalent. 11 | * Fixing roslint error. 12 | * Updating package.xml to format 3 and setting C++ standard to 11. 13 | * Contributors: Joshua Whitley 14 | 15 | 1.0.0 (2020-12-02) 16 | ------------------ 17 | * Applying roslint to vesc_ackerman. 18 | * Adding roslint. 19 | * Adding licenses. 20 | * Updating maintainers, authors, and URLs. 21 | * added onboard car 22 | * Contributors: Joshua Whitley, billyzheng 23 | -------------------------------------------------------------------------------- /vesc_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc_msgs 5 | 1.1.0 6 | 7 | ROS message definitions for the Vedder VESC open source motor controller. 8 | 9 | Johannes Betz 10 | BSD 11 | http://ros.org/wiki/vesc_msgs 12 | https://github.com/f1tenth/vesc 13 | https://github.com/f1tenth/vesc/issues 14 | Michael T. Boulet 15 | Joshua Whitley 16 | 17 | catkin 18 | message_generation 19 | 20 | std_msgs 21 | 22 | message_runtime 23 | 24 | -------------------------------------------------------------------------------- /vesc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc 5 | 1.1.0 6 | 7 | Metapackage for ROS interface to the Vedder VESC open source motor controller. 8 | 9 | Johannes Betz 10 | BSD 11 | http://www.ros.org/wiki/vesc 12 | https://github.com/f1tenth/vesc 13 | https://github.com/f1tenth/vesc/issues 14 | Michael T. Boulet 15 | Joshua Whitley 16 | 17 | catkin 18 | 19 | vesc_driver 20 | vesc_msgs 21 | vesc_ackermann 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /vesc_driver/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package vesc_driver 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.0 (2020-12-12) 6 | ------------------ 7 | * Merge pull request `#1 `_ from f1tenth/melodic-devel 8 | Updating for Melodic 9 | * Exclude crc.h from roslint. 10 | * Replacing boost::crc with CRCPP. 11 | * Replacing boost::begin, boost::end, and boost::distance with Standard Library equivalents. 12 | * Replacing boost::bind with Standard Library equivalent. 13 | * Replaing boost::noncopyable with C++ equivalent. 14 | * Replacing boost::function with Standard Library version. 15 | * Replacing Boost smart pointers with Standard Library equivalents. 16 | * Removing unnecessary v8stdint.h. 17 | * Updating package.xml to format 3 and setting C++ standard to 11. 18 | * Contributors: Joshua Whitley 19 | 20 | 1.0.0 (2020-12-02) 21 | ------------------ 22 | * Applying roslint and replacing registration macro with templated class. 23 | * Adding roslint. 24 | * Adding licenses. 25 | * Updating maintainers, authors, and URLs. 26 | * added onboard car 27 | * Contributors: Joshua Whitley, billyzheng 28 | -------------------------------------------------------------------------------- /vesc_msgs/msg/VescState.msg: -------------------------------------------------------------------------------- 1 | # Vedder VESC open source motor controller state (telemetry) 2 | 3 | # fault codes 4 | int32 FAULT_CODE_NONE=0 5 | int32 FAULT_CODE_OVER_VOLTAGE=1 6 | int32 FAULT_CODE_UNDER_VOLTAGE=2 7 | int32 FAULT_CODE_DRV8302=3 8 | int32 FAULT_CODE_ABS_OVER_CURRENT=4 9 | int32 FAULT_CODE_OVER_TEMP_FET=5 10 | int32 FAULT_CODE_OVER_TEMP_MOTOR=6 11 | 12 | float64 voltage_input # input voltage (volt) 13 | float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) 14 | float64 current_motor # motor current (ampere) 15 | float64 current_input # input current (ampere) 16 | float64 speed # motor electrical speed (revolutions per minute) 17 | float64 duty_cycle # duty cycle (0 to 1) 18 | float64 charge_drawn # electric charge drawn from input (ampere-hour) 19 | float64 charge_regen # electric charge regenerated to input (ampere-hour) 20 | float64 energy_drawn # energy drawn from input (watt-hour) 21 | float64 energy_regen # energy regenerated to input (watt-hour) 22 | float64 displacement # net tachometer (counts) 23 | float64 distance_traveled # total tachnometer (counts) 24 | int32 fault_code 25 | -------------------------------------------------------------------------------- /vesc_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc_driver 5 | 1.1.0 6 | 7 | ROS device driver for the Vedder VESC open source motor driver. 8 | 9 | Johannes Betz 10 | BSD 11 | http://www.ros.org/wiki/vesc_driver 12 | https://github.com/f1tenth/vesc 13 | https://github.com/f1tenth/vesc/issues 14 | Michael T. Boulet 15 | Joshua Whitley 16 | 17 | catkin 18 | roslint 19 | 20 | nodelet 21 | pluginlib 22 | roscpp 23 | serial 24 | std_msgs 25 | vesc_msgs 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /vesc_ackermann/launch/vesc_to_odom_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /.github/workflows/ros1-ci.yaml: -------------------------------------------------------------------------------- 1 | name: ROS1 CI Workflow 2 | 3 | on: 4 | pull_request: 5 | push: 6 | branches: 7 | - main 8 | 9 | jobs: 10 | 11 | build-ros1: 12 | runs-on: ubuntu-latest 13 | strategy: 14 | fail-fast: false 15 | container: 16 | image: ros:melodic 17 | steps: 18 | - name: Checkout Repo 19 | uses: actions/checkout@v2 20 | - name: Create Workspace 21 | run: | 22 | mkdir -p src_tmp/vesc; \ 23 | mv `find -maxdepth 1 -not -name . -not -name src_tmp` src_tmp/vesc/; \ 24 | mv src_tmp/ src/; \ 25 | cd src; \ 26 | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ 27 | catkin_init_workspace' 28 | - name: Install Dependencies 29 | run: | 30 | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ 31 | apt-get update && rosdep update; \ 32 | rosdep install --from-paths src --ignore-src -y' 33 | - name: Build Workspace 34 | run: | 35 | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ 36 | catkin_make' 37 | - name: Run Tests 38 | run: | 39 | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ 40 | catkin_make run_tests; \ 41 | catkin_test_results --verbose' 42 | -------------------------------------------------------------------------------- /vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /vesc_ackermann/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc_ackermann 5 | 1.1.0 6 | 7 | Translate between VESC messages and ROS ackermann and odometry messages. 8 | 9 | Johannes Betz 10 | BSD 11 | http://www.ros.org/wiki/vesc_ackermann 12 | https://github.com/f1tenth/vesc 13 | https://github.com/f1tenth/vesc/issues 14 | Michael T. Boulet 15 | Joshua Whitley 16 | 17 | catkin 18 | 19 | roslint 20 | 21 | ackermann_msgs 22 | geometry_msgs 23 | nav_msgs 24 | nodelet 25 | pluginlib 26 | roscpp 27 | std_msgs 28 | tf 29 | vesc_msgs 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2020 F1TENTH Foundation 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | -------------------------------------------------------------------------------- /vesc_driver/launch/vesc_driver_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | 28 | #include "vesc_driver/vesc_driver.h" 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "vesc_driver_node"); 33 | ros::NodeHandle nh; 34 | ros::NodeHandle private_nh("~"); 35 | 36 | vesc_driver::VescDriver vesc_driver(nh, private_nh); 37 | 38 | ros::spin(); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /vesc_ackermann/src/vesc_to_odom_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | 28 | #include "vesc_ackermann/vesc_to_odom.h" 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "vesc_to_odom_node"); 33 | ros::NodeHandle nh; 34 | ros::NodeHandle private_nh("~"); 35 | 36 | vesc_ackermann::VescToOdom vesc_to_odom(nh, private_nh); 37 | 38 | ros::spin(); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /vesc_ackermann/src/ackermann_to_vesc_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | 28 | #include "vesc_ackermann/ackermann_to_vesc.h" 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "ackermann_to_vesc_node"); 33 | ros::NodeHandle nh; 34 | ros::NodeHandle private_nh("~"); 35 | 36 | vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh); 37 | 38 | ros::spin(); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_driver_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include "vesc_driver/vesc_driver.h" 33 | 34 | namespace vesc_driver 35 | { 36 | 37 | class VescDriverNodelet: public nodelet::Nodelet 38 | { 39 | public: 40 | VescDriverNodelet() {} 41 | 42 | private: 43 | virtual void onInit(void); 44 | 45 | std::shared_ptr vesc_driver_; 46 | }; // class VescDriverNodelet 47 | 48 | void VescDriverNodelet::onInit() 49 | { 50 | NODELET_DEBUG("Initializing VESC driver nodelet"); 51 | vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); 52 | } 53 | 54 | } // namespace vesc_driver 55 | 56 | PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); 57 | -------------------------------------------------------------------------------- /vesc_ackermann/src/vesc_to_odom_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include "vesc_ackermann/vesc_to_odom.h" 33 | 34 | namespace vesc_ackermann 35 | { 36 | 37 | class VescToOdomNodelet: public nodelet::Nodelet 38 | { 39 | public: 40 | VescToOdomNodelet() {} 41 | 42 | private: 43 | virtual void onInit(void); 44 | 45 | std::shared_ptr vesc_to_odom_; 46 | }; // class VescToOdomNodelet 47 | 48 | void VescToOdomNodelet::onInit() 49 | { 50 | NODELET_DEBUG("Initializing RACECAR VESC odometry estimator nodelet"); 51 | vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); 52 | } 53 | 54 | } // namespace vesc_ackermann 55 | 56 | PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); 57 | -------------------------------------------------------------------------------- /vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include "vesc_ackermann/ackermann_to_vesc.h" 33 | 34 | namespace vesc_ackermann 35 | { 36 | 37 | class AckermannToVescNodelet: public nodelet::Nodelet 38 | { 39 | public: 40 | AckermannToVescNodelet() {} 41 | 42 | private: 43 | virtual void onInit(void); 44 | 45 | std::shared_ptr ackermann_to_vesc_; 46 | }; // class AckermannToVescNodelet 47 | 48 | void AckermannToVescNodelet::onInit() 49 | { 50 | NODELET_DEBUG("Initializing ackermann to VESC nodelet"); 51 | ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); 52 | } 53 | 54 | } // namespace vesc_ackermann 55 | 56 | PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); 57 | -------------------------------------------------------------------------------- /vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ 29 | #define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ 30 | 31 | #include 32 | #include 33 | 34 | namespace vesc_ackermann 35 | { 36 | 37 | class AckermannToVesc 38 | { 39 | public: 40 | AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); 41 | 42 | private: 43 | // ROS parameters 44 | // conversion gain and offset 45 | bool previous_mode_speed_ = true; 46 | double speed_to_erpm_gain_, speed_to_erpm_offset_; 47 | double accel_to_current_gain_, accel_to_brake_gain_; 48 | double steering_to_servo_gain_, steering_to_servo_offset_; 49 | 50 | /** @todo consider also providing an interpolated look-up table conversion */ 51 | 52 | // ROS services 53 | ros::Publisher erpm_pub_; 54 | ros::Publisher servo_pub_; 55 | ros::Publisher current_pub_; 56 | ros::Publisher brake_pub_; 57 | ros::Subscriber ackermann_sub_; 58 | 59 | // ROS callbacks 60 | void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); 61 | }; 62 | 63 | } // namespace vesc_ackermann 64 | 65 | #endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ 66 | -------------------------------------------------------------------------------- /vesc_ackermann/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vesc_ackermann) 3 | 4 | # Setting C++ standard to 11 5 | if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") 6 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") 9 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 10 | set(CMAKE_CXX_STANDARD 11) 11 | endif() 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | ackermann_msgs 15 | geometry_msgs 16 | nav_msgs 17 | nodelet 18 | pluginlib 19 | roscpp 20 | roslint 21 | std_msgs 22 | tf 23 | vesc_msgs 24 | ) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS include 28 | CATKIN_DEPENDS 29 | ackermann_msgs 30 | geometry_msgs 31 | nav_msgs 32 | nodelet 33 | pluginlib 34 | std_msgs 35 | tf 36 | vesc_msgs 37 | ) 38 | 39 | ########### 40 | ## Build ## 41 | ########### 42 | 43 | include_directories( 44 | include 45 | ${catkin_INCLUDE_DIRS} 46 | ) 47 | 48 | # node executable 49 | add_executable(vesc_to_odom_node src/vesc_to_odom_node.cpp 50 | src/vesc_to_odom.cpp) 51 | add_dependencies(vesc_to_odom_node ${catkin_EXPORTED_TARGETS}) 52 | target_link_libraries(vesc_to_odom_node 53 | ${catkin_LIBRARIES} 54 | ) 55 | 56 | add_executable(ackermann_to_vesc_node src/ackermann_to_vesc_node.cpp 57 | src/ackermann_to_vesc.cpp) 58 | add_dependencies(ackermann_to_vesc_node ${catkin_EXPORTED_TARGETS}) 59 | target_link_libraries(ackermann_to_vesc_node 60 | ${catkin_LIBRARIES} 61 | ) 62 | 63 | # nodelet library 64 | add_library(vesc_ackermann_nodelet src/ackermann_to_vesc_nodelet.cpp 65 | src/ackermann_to_vesc.cpp 66 | src/vesc_to_odom_nodelet.cpp 67 | src/vesc_to_odom.cpp) 68 | add_dependencies(vesc_ackermann_nodelet ${catkin_EXPORTED_TARGETS}) 69 | target_link_libraries(vesc_ackermann_nodelet 70 | ${catkin_LIBRARIES} 71 | ) 72 | 73 | set(ROSLINT_CPP_OPTS "--filter=-build/c++11") 74 | roslint_cpp() 75 | 76 | ############# 77 | ## Install ## 78 | ############# 79 | 80 | install(TARGETS vesc_to_odom_node ackermann_to_vesc_node 81 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 82 | ) 83 | 84 | install(TARGETS vesc_ackermann_nodelet 85 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | ) 87 | 88 | install(DIRECTORY include/${PROJECT_NAME}/ 89 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 90 | install(FILES vesc_ackermann_nodelet.xml 91 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 92 | install(DIRECTORY launch/ 93 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 94 | 95 | ############# 96 | ## Testing ## 97 | ############# 98 | 99 | if(CATKIN_ENABLE_TESTING) 100 | roslint_add_test() 101 | endif() 102 | -------------------------------------------------------------------------------- /vesc_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vesc_driver) 3 | 4 | # Set minimum C++ standard to C++11 5 | if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") 6 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") 9 | message(STATUS "Changing CXX_STANDARD from C++98 to C++11") 10 | set(CMAKE_CXX_STANDARD 11) 11 | endif() 12 | 13 | find_package(Boost REQUIRED) 14 | 15 | find_package(catkin REQUIRED COMPONENTS 16 | nodelet 17 | pluginlib 18 | roscpp 19 | roslint 20 | serial 21 | std_msgs 22 | vesc_msgs 23 | ) 24 | 25 | catkin_package( 26 | INCLUDE_DIRS include 27 | CATKIN_DEPENDS 28 | nodelet 29 | pluginlib 30 | std_msgs 31 | vesc_msgs serial 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | 38 | include_directories( 39 | include 40 | ${Boost_INCLUDE_DIRS} 41 | ${catkin_INCLUDE_DIRS} 42 | ) 43 | 44 | # node executable 45 | add_executable(vesc_driver_node src/vesc_driver_node.cpp 46 | src/vesc_driver.cpp 47 | src/vesc_interface.cpp 48 | src/vesc_packet.cpp 49 | src/vesc_packet_factory.cpp) 50 | add_dependencies(vesc_driver_node ${catkin_EXPORTED_TARGETS}) 51 | target_link_libraries(vesc_driver_node 52 | ${catkin_LIBRARIES} 53 | ) 54 | 55 | # nodelet library 56 | add_library(vesc_driver_nodelet src/vesc_driver_nodelet.cpp 57 | src/vesc_driver.cpp 58 | src/vesc_interface.cpp 59 | src/vesc_packet.cpp 60 | src/vesc_packet_factory.cpp) 61 | add_dependencies(vesc_driver_nodelet ${catkin_EXPORTED_TARGETS}) 62 | target_link_libraries(vesc_driver_nodelet 63 | ${catkin_LIBRARIES} 64 | ) 65 | 66 | file(GLOB_RECURSE ${PROJECT_NAME}_CPP_FILES 67 | RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} 68 | . *.cpp *.h) 69 | list(REMOVE_ITEM ${PROJECT_NAME}_CPP_FILES 70 | include/${PROJECT_NAME}/crc.h) 71 | 72 | set(ROSLINT_CPP_OPTS "--filter=-build/c++11") 73 | roslint_cpp(${${PROJECT_NAME}_CPP_FILES}) 74 | 75 | ############# 76 | ## Install ## 77 | ############# 78 | 79 | install(TARGETS vesc_driver_node 80 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 81 | ) 82 | 83 | install(TARGETS vesc_driver_nodelet 84 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 85 | ) 86 | 87 | install(DIRECTORY include/${PROJECT_NAME}/ 88 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 89 | install(FILES vesc_driver_nodelet.xml 90 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 91 | install(DIRECTORY launch/ 92 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 93 | 94 | ############# 95 | ## Testing ## 96 | ############# 97 | 98 | if(CATKIN_ENABLE_TESTING) 99 | roslint_add_test() 100 | endif() 101 | -------------------------------------------------------------------------------- /vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_ 29 | #define VESC_ACKERMANN_VESC_TO_ODOM_H_ 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace vesc_ackermann 40 | { 41 | 42 | class VescToOdom 43 | { 44 | public: 45 | VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh); 46 | 47 | private: 48 | // ROS parameters 49 | std::string odom_frame_; 50 | std::string base_frame_; 51 | /** State message does not report servo position, so use the command instead */ 52 | bool use_servo_cmd_; 53 | // conversion gain and offset 54 | double speed_to_erpm_gain_, speed_to_erpm_offset_; 55 | double steering_to_servo_gain_, steering_to_servo_offset_; 56 | double wheelbase_; 57 | bool publish_tf_; 58 | 59 | // odometry state 60 | double x_, y_, yaw_; 61 | std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value 62 | vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message 63 | 64 | // ROS services 65 | ros::Publisher odom_pub_; 66 | ros::Subscriber vesc_state_sub_; 67 | ros::Subscriber servo_sub_; 68 | std::shared_ptr tf_pub_; 69 | 70 | // ROS callbacks 71 | void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state); 72 | void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo); 73 | }; 74 | 75 | } // namespace vesc_ackermann 76 | 77 | #endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ 78 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_driver.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #ifndef VESC_DRIVER_VESC_DRIVER_H_ 29 | #define VESC_DRIVER_VESC_DRIVER_H_ 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "vesc_driver/vesc_interface.h" 39 | #include "vesc_driver/vesc_packet.h" 40 | 41 | namespace vesc_driver 42 | { 43 | 44 | class VescDriver 45 | { 46 | public: 47 | VescDriver(ros::NodeHandle nh, 48 | ros::NodeHandle private_nh); 49 | 50 | private: 51 | // interface to the VESC 52 | VescInterface vesc_; 53 | void vescPacketCallback(const std::shared_ptr& packet); 54 | void vescErrorCallback(const std::string& error); 55 | 56 | // limits on VESC commands 57 | struct CommandLimit 58 | { 59 | CommandLimit(const ros::NodeHandle& nh, const std::string& str, 60 | const boost::optional& min_lower = boost::optional(), 61 | const boost::optional& max_upper = boost::optional()); 62 | double clip(double value); 63 | std::string name; 64 | boost::optional lower; 65 | boost::optional upper; 66 | }; 67 | CommandLimit duty_cycle_limit_; 68 | CommandLimit current_limit_; 69 | CommandLimit brake_limit_; 70 | CommandLimit speed_limit_; 71 | CommandLimit position_limit_; 72 | CommandLimit servo_limit_; 73 | 74 | // ROS services 75 | ros::Publisher state_pub_; 76 | ros::Publisher servo_sensor_pub_; 77 | ros::Subscriber duty_cycle_sub_; 78 | ros::Subscriber current_sub_; 79 | ros::Subscriber brake_sub_; 80 | ros::Subscriber speed_sub_; 81 | ros::Subscriber position_sub_; 82 | ros::Subscriber servo_sub_; 83 | ros::Timer timer_; 84 | 85 | // driver modes (possible states) 86 | typedef enum 87 | { 88 | MODE_INITIALIZING, 89 | MODE_OPERATING 90 | } 91 | driver_mode_t; 92 | 93 | // other variables 94 | driver_mode_t driver_mode_; ///< driver state machine mode (state) 95 | int fw_version_major_; ///< firmware major version reported by vesc 96 | int fw_version_minor_; ///< firmware minor version reported by vesc 97 | 98 | // ROS callbacks 99 | void timerCallback(const ros::TimerEvent& event); 100 | void dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle); 101 | void currentCallback(const std_msgs::Float64::ConstPtr& current); 102 | void brakeCallback(const std_msgs::Float64::ConstPtr& brake); 103 | void speedCallback(const std_msgs::Float64::ConstPtr& speed); 104 | void positionCallback(const std_msgs::Float64::ConstPtr& position); 105 | void servoCallback(const std_msgs::Float64::ConstPtr& servo); 106 | }; 107 | 108 | } // namespace vesc_driver 109 | 110 | #endif // VESC_DRIVER_VESC_DRIVER_H_ 111 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_packet_factory.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_ 29 | #define VESC_DRIVER_VESC_PACKET_FACTORY_H_ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "vesc_driver/vesc_packet.h" 39 | 40 | namespace vesc_driver 41 | { 42 | 43 | /** 44 | * Class for creating VESC packets from raw data. 45 | */ 46 | class VescPacketFactory 47 | { 48 | public: 49 | /** Return the global factory object */ 50 | static VescPacketFactory* getFactory(); 51 | 52 | /** 53 | * Create a VescPacket from a buffer (factory function). Packet must start (start of frame 54 | * character) at @p begin and complete (end of frame character) before *p end. The buffer element 55 | * at @p end is not examined, i.e. it can be the past-the-end element. Only returns a packet if 56 | * the packet is valid, i.e. valid size, matching checksum, complete etc. An empty pointer is 57 | * returned if a packet cannot be found or if it is invalid. If a valid packet is not found, 58 | * optional output parameter @what is set to a string providing a reason why a packet was not 59 | * found. If a packet was not found because additional bytes are needed on the buffer, optional 60 | * output parameter @p num_bytes_needed will contain the number of bytes needed to either 61 | * determine the size of the packet or complete the packet. Output parameters @p num_bytes_needed 62 | * and @p what will be set to 0 and empty if a valid packet is found. 63 | * 64 | * @param begin[in] Iterator to a buffer at the start-of-frame character 65 | * @param end[in] Iterator to the buffer past-the-end element. 66 | * @param num_bytes_needed[out] Number of bytes needed to determine the packet size or complete 67 | * the frame. 68 | * @param what[out] Human readable string giving a reason why the packet was not found. 69 | * 70 | * @return Pointer to a valid VescPacket if successful. Otherwise, an empty pointer. 71 | */ 72 | static VescPacketPtr createPacket(const Buffer::const_iterator& begin, 73 | const Buffer::const_iterator& end, 74 | int* num_bytes_needed, std::string* what); 75 | 76 | typedef std::function)> CreateFn; 77 | 78 | /** Register a packet type with the factory. */ 79 | static void registerPacketType(int payload_id, CreateFn fn); 80 | 81 | /** 82 | * Delete copy constructor and equals operator. 83 | */ 84 | VescPacketFactory(const VescPacketFactory &) = delete; 85 | VescPacketFactory & operator=(const VescPacketFactory &) = delete; 86 | 87 | private: 88 | VescPacketFactory(); 89 | typedef std::map FactoryMap; 90 | static FactoryMap* getMap(); 91 | }; 92 | 93 | template 94 | class PacketFactoryTemplate 95 | { 96 | public: 97 | explicit PacketFactoryTemplate(int payload_id) 98 | { 99 | VescPacketFactory::registerPacketType(payload_id, &PacketFactoryTemplate::create); 100 | } 101 | 102 | static VescPacketPtr create(std::shared_ptr frame) 103 | { 104 | return VescPacketPtr(new PACKETTYPE(frame)); 105 | } 106 | }; 107 | 108 | /** Use this macro to register packets */ 109 | #define REGISTER_PACKET_TYPE(id, klass) \ 110 | static PacketFactoryTemplate global_##klass##Factory((id)); 111 | 112 | } // namespace vesc_driver 113 | 114 | #endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ 115 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #ifndef VESC_DRIVER_VESC_INTERFACE_H_ 29 | #define VESC_DRIVER_VESC_INTERFACE_H_ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "vesc_driver/vesc_packet.h" 39 | 40 | namespace vesc_driver 41 | { 42 | 43 | /** 44 | * Class providing an interface to the Vedder VESC motor controller via a serial port interface. 45 | */ 46 | class VescInterface 47 | { 48 | public: 49 | typedef std::function PacketHandlerFunction; 50 | typedef std::function ErrorHandlerFunction; 51 | 52 | /** 53 | * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not 54 | * empty, otherwise the serial port remains closed until connect() is called. 55 | * 56 | * @param port Address of the serial port, e.g. '/dev/ttyUSB0'. 57 | * @param packet_handler Function this class calls when a VESC packet is received. 58 | * @param error_handler Function this class calls when an error is detected, such as a bad 59 | * checksum. 60 | * 61 | * @throw SerialException 62 | */ 63 | VescInterface(const std::string& port = std::string(), 64 | const PacketHandlerFunction& packet_handler = PacketHandlerFunction(), 65 | const ErrorHandlerFunction& error_handler = ErrorHandlerFunction()); 66 | 67 | /** 68 | * Delete copy constructor and equals operator. 69 | */ 70 | VescInterface(const VescInterface &) = delete; 71 | VescInterface & operator=(const VescInterface &) = delete; 72 | 73 | /** 74 | * VescInterface destructor. 75 | */ 76 | ~VescInterface(); 77 | 78 | /** 79 | * Sets / updates the function that this class calls when a VESC packet is received. 80 | */ 81 | void setPacketHandler(const PacketHandlerFunction& handler); 82 | 83 | /** 84 | * Sets / updates the function that this class calls when an error is detected, such as a bad 85 | * checksum. 86 | */ 87 | void setErrorHandler(const ErrorHandlerFunction& handler); 88 | 89 | /** 90 | * Opens the serial port interface to the VESC. 91 | * 92 | * @throw SerialException 93 | */ 94 | void connect(const std::string& port); 95 | 96 | /** 97 | * Closes the serial port interface to the VESC. 98 | */ 99 | void disconnect(); 100 | 101 | /** 102 | * Gets the status of the serial interface to the VESC. 103 | * 104 | * @return Returns true if the serial port is open, false otherwise. 105 | */ 106 | bool isConnected() const; 107 | 108 | /** 109 | * Send a VESC packet. 110 | */ 111 | void send(const VescPacket& packet); 112 | 113 | void requestFWVersion(); 114 | void requestState(); 115 | void setDutyCycle(double duty_cycle); 116 | void setCurrent(double current); 117 | void setBrake(double brake); 118 | void setSpeed(double speed); 119 | void setPosition(double position); 120 | void setServo(double servo); 121 | 122 | private: 123 | // Pimpl - hide serial port members from class users 124 | class Impl; 125 | std::unique_ptr impl_; 126 | }; 127 | 128 | // todo: review 129 | class SerialException : public std::exception 130 | { 131 | // Disable copy constructors 132 | SerialException& operator=(const SerialException&); 133 | std::string e_what_; 134 | public: 135 | explicit SerialException(const char *description) 136 | { 137 | std::stringstream ss; 138 | ss << "SerialException " << description << " failed."; 139 | e_what_ = ss.str(); 140 | } 141 | SerialException(const SerialException& other) : e_what_(other.e_what_) {} 142 | virtual ~SerialException() throw() {} 143 | virtual const char* what() const throw() 144 | { 145 | return e_what_.c_str(); 146 | } 147 | }; 148 | 149 | } // namespace vesc_driver 150 | 151 | #endif // VESC_DRIVER_VESC_INTERFACE_H_ 152 | -------------------------------------------------------------------------------- /vesc_ackermann/src/ackermann_to_vesc.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #include "vesc_ackermann/ackermann_to_vesc.h" 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace vesc_ackermann 37 | { 38 | 39 | template 40 | inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); 41 | 42 | AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) 43 | { 44 | // get conversion parameters 45 | if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) 46 | return; 47 | if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) 48 | return; 49 | if (!getRequiredParam(nh, "accel_to_current_gain", &accel_to_current_gain_)) 50 | return; 51 | if (!getRequiredParam(nh, "accel_to_brake_gain", &accel_to_brake_gain_)) 52 | return; 53 | if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) 54 | return; 55 | if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) 56 | return; 57 | 58 | // create publishers to vesc electric-RPM (speed) and servo commands 59 | erpm_pub_ = nh.advertise("commands/motor/speed", 10); 60 | current_pub_ = nh.advertise("commands/motor/current", 10); 61 | brake_pub_ = nh.advertise("commands/motor/brake", 10); 62 | servo_pub_ = nh.advertise("commands/servo/position", 10); 63 | 64 | // subscribe to ackermann topic 65 | ackermann_sub_ = nh.subscribe("ackermann_cmd", 10, &AckermannToVesc::ackermannCmdCallback, this); 66 | } 67 | 68 | typedef ackermann_msgs::AckermannDriveStamped::ConstPtr AckermannMsgPtr; 69 | void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) 70 | { 71 | // calc vesc electric RPM (speed) 72 | std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); 73 | erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; 74 | 75 | // calc vesc current/brake (acceleration) 76 | bool is_positive_accel = true; 77 | std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); 78 | std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); 79 | current_msg->data = 0; 80 | brake_msg->data = 0; 81 | if (cmd->drive.acceleration < 0) 82 | { 83 | brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; 84 | is_positive_accel = false; 85 | } 86 | else 87 | { 88 | current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; 89 | } 90 | 91 | // calc steering angle (servo) 92 | std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); 93 | servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; 94 | 95 | // publish 96 | if (ros::ok()) 97 | { 98 | // The below code attempts to stick to the previous mode until a new message forces a mode switch. 99 | if (erpm_msg->data != 0 || previous_mode_speed_) 100 | { 101 | erpm_pub_.publish(erpm_msg); 102 | } 103 | else 104 | { 105 | if (is_positive_accel) 106 | { 107 | current_pub_.publish(current_msg); 108 | } 109 | else 110 | { 111 | brake_pub_.publish(brake_msg); 112 | } 113 | } 114 | servo_pub_.publish(servo_msg); 115 | } 116 | 117 | // The lines below are to determine which mode we are in so we can hold until new messages force a switch. 118 | if (erpm_msg->data != 0) 119 | { 120 | previous_mode_speed_ = true; 121 | } 122 | else if (current_msg->data != 0 || brake_msg->data != 0) 123 | { 124 | previous_mode_speed_ = false; 125 | } 126 | } 127 | 128 | template 129 | inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) 130 | { 131 | if (nh.getParam(name, *value)) 132 | return true; 133 | 134 | ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str()); 135 | return false; 136 | } 137 | 138 | } // namespace vesc_ackermann 139 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/datatypes.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | /* 27 | Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se 28 | 29 | This file is part of VESC Tool. 30 | 31 | VESC Tool is free software: you can redistribute it and/or modify 32 | it under the terms of the GNU General Public License as published by 33 | the Free Software Foundation, either version 3 of the License, or 34 | (at your option) any later version. 35 | 36 | VESC Tool is distributed in the hope that it will be useful, 37 | but WITHOUT ANY WARRANTY; without even the implied warranty of 38 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 39 | GNU General Public License for more details. 40 | 41 | You should have received a copy of the GNU General Public License 42 | along with this program. If not, see . 43 | */ 44 | 45 | #ifndef VESC_DRIVER_DATATYPES_H 46 | #define VESC_DRIVER_DATATYPES_H 47 | 48 | #include 49 | 50 | typedef struct 51 | { 52 | bool isVesc; 53 | } 54 | VSerialInfo_t; 55 | 56 | typedef enum 57 | { 58 | CFG_T_UNDEFINED = 0, 59 | CFG_T_DOUBLE, 60 | CFG_T_INT, 61 | CFG_T_QSTRING, 62 | CFG_T_ENUM, 63 | CFG_T_BOOL 64 | } 65 | CFG_T; 66 | 67 | typedef enum 68 | { 69 | VESC_TX_UNDEFINED = 0, 70 | VESC_TX_UINT8, 71 | VESC_TX_INT8, 72 | VESC_TX_UINT16, 73 | VESC_TX_INT16, 74 | VESC_TX_UINT32, 75 | VESC_TX_INT32, 76 | VESC_TX_DOUBLE16, 77 | VESC_TX_DOUBLE32, 78 | VESC_TX_DOUBLE32_AUTO 79 | } 80 | VESC_TX_T; 81 | 82 | typedef enum 83 | { 84 | FAULT_CODE_NONE = 0, 85 | FAULT_CODE_OVER_VOLTAGE, 86 | FAULT_CODE_UNDER_VOLTAGE, 87 | FAULT_CODE_DRV, 88 | FAULT_CODE_ABS_OVER_CURRENT, 89 | FAULT_CODE_OVER_TEMP_FET, 90 | FAULT_CODE_OVER_TEMP_MOTOR 91 | } 92 | mc_fault_code; 93 | 94 | typedef enum 95 | { 96 | DISP_POS_MODE_NONE = 0, 97 | DISP_POS_MODE_INDUCTANCE, 98 | DISP_POS_MODE_OBSERVER, 99 | DISP_POS_MODE_ENCODER, 100 | DISP_POS_MODE_PID_POS, 101 | DISP_POS_MODE_PID_POS_ERROR, 102 | DISP_POS_MODE_ENCODER_OBSERVER_ERROR 103 | } 104 | disp_pos_mode; 105 | 106 | struct MC_VALUES 107 | { 108 | public: 109 | double v_in; 110 | double temp_mos; 111 | double temp_motor; 112 | double current_motor; 113 | double current_in; 114 | double id; 115 | double iq; 116 | double rpm; 117 | double duty_now; 118 | double amp_hours; 119 | double amp_hours_charged; 120 | double watt_hours; 121 | double watt_hours_charged; 122 | int tachometer; 123 | int tachometer_abs; 124 | double position; 125 | mc_fault_code fault_code; 126 | }; 127 | 128 | typedef enum 129 | { 130 | DEBUG_SAMPLING_OFF = 0, 131 | DEBUG_SAMPLING_NOW, 132 | DEBUG_SAMPLING_START, 133 | DEBUG_SAMPLING_TRIGGER_START, 134 | DEBUG_SAMPLING_TRIGGER_FAULT, 135 | DEBUG_SAMPLING_TRIGGER_START_NOSEND, 136 | DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, 137 | DEBUG_SAMPLING_SEND_LAST_SAMPLES 138 | } 139 | debug_sampling_mode; 140 | 141 | typedef enum 142 | { 143 | COMM_FW_VERSION = 0, 144 | COMM_JUMP_TO_BOOTLOADER, 145 | COMM_ERASE_NEW_APP, 146 | COMM_WRITE_NEW_APP_DATA, 147 | COMM_GET_VALUES, 148 | COMM_SET_DUTY, 149 | COMM_SET_CURRENT, 150 | COMM_SET_CURRENT_BRAKE, 151 | COMM_SET_RPM, 152 | COMM_SET_POS, 153 | COMM_SET_HANDBRAKE, 154 | COMM_SET_DETECT, 155 | COMM_SET_SERVO_POS, 156 | COMM_SET_MCCONF, 157 | COMM_GET_MCCONF, 158 | COMM_GET_MCCONF_DEFAULT, 159 | COMM_SET_APPCONF, 160 | COMM_GET_APPCONF, 161 | COMM_GET_APPCONF_DEFAULT, 162 | COMM_SAMPLE_PRINT, 163 | COMM_TERMINAL_CMD, 164 | COMM_PRINT, 165 | COMM_ROTOR_POSITION, 166 | COMM_EXPERIMENT_SAMPLE, 167 | COMM_DETECT_MOTOR_PARAM, 168 | COMM_DETECT_MOTOR_R_L, 169 | COMM_DETECT_MOTOR_FLUX_LINKAGE, 170 | COMM_DETECT_ENCODER, 171 | COMM_DETECT_HALL_FOC, 172 | COMM_REBOOT, 173 | COMM_ALIVE, 174 | COMM_GET_DECODED_PPM, 175 | COMM_GET_DECODED_ADC, 176 | COMM_GET_DECODED_CHUK, 177 | COMM_FORWARD_CAN, 178 | COMM_SET_CHUCK_DATA, 179 | COMM_CUSTOM_APP_DATA, 180 | COMM_NRF_START_PAIRING 181 | } 182 | COMM_PACKET_ID; 183 | 184 | typedef struct 185 | { 186 | int js_x; 187 | int js_y; 188 | int acc_x; 189 | int acc_y; 190 | int acc_z; 191 | bool bt_c; 192 | bool bt_z; 193 | } 194 | chuck_data; 195 | 196 | struct bldc_detect 197 | { 198 | public: 199 | double cycle_int_limit; 200 | double bemf_coupling_k; 201 | int hall_res; 202 | }; 203 | 204 | typedef enum 205 | { 206 | NRF_PAIR_STARTED = 0, 207 | NRF_PAIR_OK, 208 | NRF_PAIR_FAIL 209 | } 210 | NRF_PAIR_RES; 211 | 212 | #endif // VESC_DRIVER_DATATYPES_H 213 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_packet_factory.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #include "vesc_driver/vesc_packet_factory.h" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "vesc_driver/vesc_packet.h" 36 | 37 | namespace vesc_driver 38 | { 39 | 40 | /** Construct map on first use */ 41 | VescPacketFactory::FactoryMap* VescPacketFactory::getMap() 42 | { 43 | static FactoryMap m; 44 | return &m; 45 | } 46 | 47 | void VescPacketFactory::registerPacketType(int payload_id, CreateFn fn) 48 | { 49 | FactoryMap* p_map(getMap()); 50 | assert(0 == p_map->count(payload_id)); 51 | (*p_map)[payload_id] = fn; 52 | } 53 | 54 | /** Helper function for when createPacket can not create a packet */ 55 | VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, 56 | const std::string& what, int num_bytes_needed = 0) 57 | { 58 | if (p_num_bytes_needed != NULL) *p_num_bytes_needed = num_bytes_needed; 59 | if (p_what != NULL) *p_what = what; 60 | return VescPacketPtr(); 61 | } 62 | 63 | VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, 64 | const Buffer::const_iterator& end, 65 | int* num_bytes_needed, std::string* what) 66 | { 67 | // initialize output variables 68 | if (num_bytes_needed != NULL) *num_bytes_needed = 0; 69 | if (what != NULL) what->clear(); 70 | 71 | // need at least VESC_MIN_FRAME_SIZE bytes in buffer 72 | int buffer_size(std::distance(begin, end)); 73 | if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE) 74 | return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", 75 | VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); 76 | 77 | // buffer must begin with a start-of-frame 78 | if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin && 79 | VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) 80 | return createFailed(num_bytes_needed, what, "Buffer must begin with start-of-frame character"); 81 | 82 | // get a view of the payload 83 | BufferRangeConst view_payload; 84 | if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) 85 | { 86 | // payload size field is one byte 87 | view_payload.first = begin + 2; 88 | view_payload.second = view_payload.first + *(begin + 1); 89 | } 90 | else 91 | { 92 | assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin); 93 | // payload size field is two bytes 94 | view_payload.first = begin + 3; 95 | view_payload.second = view_payload.first + (*(begin + 1) << 8) + *(begin + 2); 96 | } 97 | 98 | // check length 99 | if (std::distance(view_payload.first, view_payload.second) > VescFrame::VESC_MAX_PAYLOAD_SIZE) 100 | return createFailed(num_bytes_needed, what, "Invalid payload length"); 101 | 102 | // get iterators to crc field, end-of-frame field, and a view of the whole frame 103 | Buffer::const_iterator iter_crc(view_payload.second); 104 | Buffer::const_iterator iter_eof(iter_crc + 2); 105 | BufferRangeConst view_frame(begin, iter_eof + 1); 106 | 107 | // do we have enough data in the buffer to complete the frame? 108 | int frame_size = std::distance(view_frame.first, view_frame.second); 109 | if (buffer_size < frame_size) 110 | return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", 111 | frame_size - buffer_size); 112 | 113 | // is the end-of-frame character valid? 114 | if (VescFrame::VESC_EOF_VAL != *iter_eof) 115 | return createFailed(num_bytes_needed, what, "Invalid end-of-frame character"); 116 | 117 | // is the crc valid? 118 | uint16_t crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); 119 | if (crc != CRC::Calculate( 120 | &(*view_payload.first), std::distance(view_payload.first, view_payload.second), VescFrame::CRC_TYPE)) 121 | return createFailed(num_bytes_needed, what, "Invalid checksum"); 122 | 123 | // frame looks good, construct the raw frame 124 | std::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); 125 | 126 | // if the packet has a payload, construct the corresponding subclass 127 | if (std::distance(view_payload.first, view_payload.second) > 0) 128 | { 129 | // get constructor function from payload id 130 | FactoryMap* p_map(getMap()); 131 | FactoryMap::const_iterator search(p_map->find(*view_payload.first)); 132 | if (search != p_map->end()) 133 | { 134 | return search->second(raw_frame); 135 | } 136 | else 137 | { 138 | // no subclass constructor for this packet 139 | return createFailed(num_bytes_needed, what, "Unkown payload type."); 140 | } 141 | } 142 | else 143 | { 144 | // no payload 145 | return createFailed(num_bytes_needed, what, "Frame does not have a payload"); 146 | } 147 | } 148 | 149 | } // namespace vesc_driver 150 | -------------------------------------------------------------------------------- /vesc_ackermann/src/vesc_to_odom.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #include "vesc_ackermann/vesc_to_odom.h" 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | namespace vesc_ackermann 37 | { 38 | 39 | template 40 | inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); 41 | 42 | VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : 43 | odom_frame_("odom"), base_frame_("base_link"), 44 | use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0) 45 | { 46 | // get ROS parameters 47 | private_nh.param("odom_frame", odom_frame_, odom_frame_); 48 | private_nh.param("base_frame", base_frame_, base_frame_); 49 | private_nh.param("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_, use_servo_cmd_); 50 | if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) 51 | return; 52 | if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) 53 | return; 54 | if (use_servo_cmd_) 55 | { 56 | if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) 57 | return; 58 | if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) 59 | return; 60 | if (!getRequiredParam(nh, "wheelbase", &wheelbase_)) 61 | return; 62 | } 63 | private_nh.param("publish_tf", publish_tf_, publish_tf_); 64 | 65 | // create odom publisher 66 | odom_pub_ = nh.advertise("odom", 10); 67 | 68 | // create tf broadcaster 69 | if (publish_tf_) 70 | { 71 | tf_pub_.reset(new tf::TransformBroadcaster); 72 | } 73 | 74 | // subscribe to vesc state and. optionally, servo command 75 | vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this); 76 | if (use_servo_cmd_) 77 | { 78 | servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, 79 | &VescToOdom::servoCmdCallback, this); 80 | } 81 | } 82 | 83 | void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state) 84 | { 85 | // check that we have a last servo command if we are depending on it for angular velocity 86 | if (use_servo_cmd_ && !last_servo_cmd_) 87 | return; 88 | 89 | // convert to engineering units 90 | double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; 91 | if (std::fabs(current_speed) < 0.05) 92 | { 93 | current_speed = 0.0; 94 | } 95 | double current_steering_angle(0.0), current_angular_velocity(0.0); 96 | if (use_servo_cmd_) 97 | { 98 | current_steering_angle = 99 | (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_; 100 | current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_; 101 | } 102 | 103 | // use current state as last state if this is our first time here 104 | if (!last_state_) 105 | last_state_ = state; 106 | 107 | // calc elapsed time 108 | ros::Duration dt = state->header.stamp - last_state_->header.stamp; 109 | 110 | /** @todo could probably do better propigating odometry, e.g. trapezoidal integration */ 111 | 112 | // propigate odometry 113 | double x_dot = current_speed * cos(yaw_); 114 | double y_dot = current_speed * sin(yaw_); 115 | x_ += x_dot * dt.toSec(); 116 | y_ += y_dot * dt.toSec(); 117 | if (use_servo_cmd_) 118 | yaw_ += current_angular_velocity * dt.toSec(); 119 | 120 | // save state for next time 121 | last_state_ = state; 122 | 123 | // publish odometry message 124 | nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry); 125 | odom->header.frame_id = odom_frame_; 126 | odom->header.stamp = state->header.stamp; 127 | odom->child_frame_id = base_frame_; 128 | 129 | // Position 130 | odom->pose.pose.position.x = x_; 131 | odom->pose.pose.position.y = y_; 132 | odom->pose.pose.orientation.x = 0.0; 133 | odom->pose.pose.orientation.y = 0.0; 134 | odom->pose.pose.orientation.z = sin(yaw_ / 2.0); 135 | odom->pose.pose.orientation.w = cos(yaw_ / 2.0); 136 | 137 | // Position uncertainty 138 | /** @todo Think about position uncertainty, perhaps get from parameters? */ 139 | odom->pose.covariance[0] = 0.2; ///< x 140 | odom->pose.covariance[7] = 0.2; ///< y 141 | odom->pose.covariance[35] = 0.4; ///< yaw 142 | 143 | // Velocity ("in the coordinate frame given by the child_frame_id") 144 | odom->twist.twist.linear.x = current_speed; 145 | odom->twist.twist.linear.y = 0.0; 146 | odom->twist.twist.angular.z = current_angular_velocity; 147 | 148 | // Velocity uncertainty 149 | /** @todo Think about velocity uncertainty */ 150 | 151 | if (publish_tf_) 152 | { 153 | geometry_msgs::TransformStamped tf; 154 | tf.header.frame_id = odom_frame_; 155 | tf.child_frame_id = base_frame_; 156 | tf.header.stamp = ros::Time::now(); 157 | tf.transform.translation.x = x_; 158 | tf.transform.translation.y = y_; 159 | tf.transform.translation.z = 0.0; 160 | tf.transform.rotation = odom->pose.pose.orientation; 161 | if (ros::ok()) 162 | { 163 | tf_pub_->sendTransform(tf); 164 | } 165 | } 166 | 167 | if (ros::ok()) 168 | { 169 | odom_pub_.publish(odom); 170 | } 171 | } 172 | 173 | void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo) 174 | { 175 | last_servo_cmd_ = servo; 176 | } 177 | 178 | template 179 | inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) 180 | { 181 | if (nh.getParam(name, *value)) 182 | return true; 183 | 184 | ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str()); 185 | return false; 186 | } 187 | 188 | } // namespace vesc_ackermann 189 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_packet.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #ifndef VESC_DRIVER_VESC_PACKET_H_ 29 | #define VESC_DRIVER_VESC_PACKET_H_ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #define CRCPP_USE_CPP11 38 | #include "vesc_driver/crc.h" 39 | 40 | namespace vesc_driver 41 | { 42 | 43 | typedef std::vector Buffer; 44 | typedef std::pair BufferRange; 45 | typedef std::pair BufferRangeConst; 46 | 47 | /** The raw frame for communicating with the VESC */ 48 | class VescFrame 49 | { 50 | public: 51 | virtual ~VescFrame() {} 52 | 53 | // getters 54 | virtual const Buffer& frame() const 55 | { 56 | return *frame_; 57 | } 58 | 59 | // VESC packet properties 60 | static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes 61 | static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes 62 | static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes 63 | static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value 64 | static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value 65 | static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value 66 | 67 | /** CRC parameters for the VESC */ 68 | static constexpr CRC::Parameters CRC_TYPE = { 0x1021, 0x0000, 0x0000, false, false }; 69 | 70 | protected: 71 | /** Construct frame with specified payload size. */ 72 | explicit VescFrame(int payload_size); 73 | 74 | std::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy 75 | BufferRange payload_; ///< View into frame's payload section 76 | 77 | private: 78 | /** Construct from buffer. Used by VescPacketFactory factory. */ 79 | VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload); 80 | 81 | /** Give VescPacketFactory access to private constructor. */ 82 | friend class VescPacketFactory; 83 | }; 84 | 85 | /*------------------------------------------------------------------------------------------------*/ 86 | 87 | /** A VescPacket is a VescFrame with a non-zero length payload */ 88 | class VescPacket : public VescFrame 89 | { 90 | public: 91 | virtual ~VescPacket() {} 92 | 93 | virtual const std::string& name() const 94 | { 95 | return name_; 96 | } 97 | 98 | protected: 99 | VescPacket(const std::string& name, int payload_size, int payload_id); 100 | VescPacket(const std::string& name, std::shared_ptr raw); 101 | 102 | private: 103 | std::string name_; 104 | }; 105 | 106 | typedef std::shared_ptr VescPacketPtr; 107 | typedef std::shared_ptr VescPacketConstPtr; 108 | 109 | /*------------------------------------------------------------------------------------------------*/ 110 | 111 | class VescPacketFWVersion : public VescPacket 112 | { 113 | public: 114 | explicit VescPacketFWVersion(std::shared_ptr raw); 115 | 116 | int fwMajor() const; 117 | int fwMinor() const; 118 | }; 119 | 120 | class VescPacketRequestFWVersion : public VescPacket 121 | { 122 | public: 123 | VescPacketRequestFWVersion(); 124 | }; 125 | 126 | /*------------------------------------------------------------------------------------------------*/ 127 | 128 | class VescPacketValues : public VescPacket 129 | { 130 | public: 131 | explicit VescPacketValues(std::shared_ptr raw); 132 | 133 | double v_in() const; 134 | double temp_mos1() const; 135 | double temp_mos2() const; 136 | double temp_mos3() const; 137 | double temp_mos4() const; 138 | double temp_mos5() const; 139 | double temp_mos6() const; 140 | double temp_pcb() const; 141 | double current_motor() const; 142 | double current_in() const; 143 | double rpm() const; 144 | double duty_now() const; 145 | double amp_hours() const; 146 | double amp_hours_charged() const; 147 | double watt_hours() const; 148 | double watt_hours_charged() const; 149 | double tachometer() const; 150 | double tachometer_abs() const; 151 | int fault_code() const; 152 | }; 153 | 154 | class VescPacketRequestValues : public VescPacket 155 | { 156 | public: 157 | VescPacketRequestValues(); 158 | }; 159 | 160 | /*------------------------------------------------------------------------------------------------*/ 161 | 162 | class VescPacketSetDuty : public VescPacket 163 | { 164 | public: 165 | explicit VescPacketSetDuty(double duty); 166 | 167 | // double duty() const; 168 | }; 169 | 170 | /*------------------------------------------------------------------------------------------------*/ 171 | 172 | class VescPacketSetCurrent : public VescPacket 173 | { 174 | public: 175 | explicit VescPacketSetCurrent(double current); 176 | 177 | // double current() const; 178 | }; 179 | 180 | /*------------------------------------------------------------------------------------------------*/ 181 | 182 | class VescPacketSetCurrentBrake : public VescPacket 183 | { 184 | public: 185 | explicit VescPacketSetCurrentBrake(double current_brake); 186 | 187 | // double current_brake() const; 188 | }; 189 | 190 | /*------------------------------------------------------------------------------------------------*/ 191 | 192 | class VescPacketSetRPM : public VescPacket 193 | { 194 | public: 195 | explicit VescPacketSetRPM(double rpm); 196 | 197 | // double rpm() const; 198 | }; 199 | 200 | /*------------------------------------------------------------------------------------------------*/ 201 | 202 | class VescPacketSetPos : public VescPacket 203 | { 204 | public: 205 | explicit VescPacketSetPos(double pos); 206 | 207 | // double pos() const; 208 | }; 209 | 210 | /*------------------------------------------------------------------------------------------------*/ 211 | 212 | class VescPacketSetServoPos : public VescPacket 213 | { 214 | public: 215 | explicit VescPacketSetServoPos(double servo_pos); 216 | 217 | // double servo_pos() const; 218 | }; 219 | 220 | } // namespace vesc_driver 221 | 222 | #endif // VESC_DRIVER_VESC_PACKET_H_ 223 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_interface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #include "vesc_driver/vesc_interface.h" 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include "vesc_driver/vesc_packet_factory.h" 43 | 44 | namespace vesc_driver 45 | { 46 | 47 | class VescInterface::Impl 48 | { 49 | public: 50 | Impl() : 51 | serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100), 52 | serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none) 53 | {} 54 | 55 | void rxThread(); 56 | 57 | std::thread rxThreadHelper() 58 | { 59 | return std::thread(&Impl::rxThread, this); 60 | } 61 | 62 | std::thread rx_thread_; 63 | bool rx_thread_run_; 64 | PacketHandlerFunction packet_handler_; 65 | ErrorHandlerFunction error_handler_; 66 | serial::Serial serial_; 67 | }; 68 | 69 | void VescInterface::Impl::rxThread() 70 | { 71 | Buffer buffer; 72 | buffer.reserve(4096); 73 | 74 | while (rx_thread_run_) 75 | { 76 | int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; 77 | if (!buffer.empty()) 78 | { 79 | // search buffer for valid packet(s) 80 | Buffer::iterator iter(buffer.begin()); 81 | Buffer::iterator iter_begin(buffer.begin()); 82 | while (iter != buffer.end()) 83 | { 84 | // check if valid start-of-frame character 85 | if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || 86 | VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) 87 | { 88 | // good start, now attempt to create packet 89 | std::string error; 90 | VescPacketConstPtr packet = 91 | VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); 92 | if (packet) 93 | { 94 | // good packet, check if we skipped any data 95 | if (std::distance(iter_begin, iter) > 0) 96 | { 97 | std::ostringstream ss; 98 | ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " 99 | << std::distance(iter_begin, iter) << " bytes."; 100 | error_handler_(ss.str()); 101 | } 102 | // call packet handler 103 | packet_handler_(packet); 104 | // update state 105 | iter = iter + packet->frame().size(); 106 | iter_begin = iter; 107 | // continue to look for another frame in buffer 108 | continue; 109 | } 110 | else if (bytes_needed > 0) 111 | { 112 | // need more data, break out of while loop 113 | break; // for (iter_sof... 114 | } 115 | else 116 | { 117 | // else, this was not a packet, move on to next byte 118 | error_handler_(error); 119 | } 120 | } 121 | 122 | iter++; 123 | } 124 | 125 | // if iter is at the end of the buffer, more bytes are needed 126 | if (iter == buffer.end()) 127 | bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; 128 | 129 | // erase "used" buffer 130 | if (std::distance(iter_begin, iter) > 0) 131 | { 132 | std::ostringstream ss; 133 | ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; 134 | error_handler_(ss.str()); 135 | } 136 | buffer.erase(buffer.begin(), iter); 137 | } 138 | 139 | // attempt to read at least bytes_needed bytes from the serial port 140 | int bytes_to_read = 141 | std::max(bytes_needed, std::min(4096, static_cast(serial_.available()))); 142 | int bytes_read = serial_.read(buffer, bytes_to_read); 143 | if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) 144 | { 145 | error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); 146 | } 147 | } 148 | } 149 | 150 | 151 | VescInterface::VescInterface(const std::string& port, 152 | const PacketHandlerFunction& packet_handler, 153 | const ErrorHandlerFunction& error_handler) : 154 | impl_(new Impl()) 155 | { 156 | setPacketHandler(packet_handler); 157 | setErrorHandler(error_handler); 158 | // attempt to conect if the port is specified 159 | if (!port.empty()) 160 | connect(port); 161 | } 162 | 163 | VescInterface::~VescInterface() 164 | { 165 | disconnect(); 166 | } 167 | 168 | void VescInterface::setPacketHandler(const PacketHandlerFunction& handler) 169 | { 170 | // todo - definately need mutex 171 | impl_->packet_handler_ = handler; 172 | } 173 | 174 | void VescInterface::setErrorHandler(const ErrorHandlerFunction& handler) 175 | { 176 | // todo - definately need mutex 177 | impl_->error_handler_ = handler; 178 | } 179 | 180 | void VescInterface::connect(const std::string& port) 181 | { 182 | // todo - mutex? 183 | 184 | if (isConnected()) 185 | { 186 | throw SerialException("Already connected to serial port."); 187 | } 188 | 189 | // connect to serial port 190 | try 191 | { 192 | impl_->serial_.setPort(port); 193 | impl_->serial_.open(); 194 | } 195 | catch (const std::exception& e) 196 | { 197 | std::stringstream ss; 198 | ss << "Failed to open the serial port to the VESC. " << e.what(); 199 | throw SerialException(ss.str().c_str()); 200 | } 201 | 202 | // start up a monitoring thread 203 | impl_->rx_thread_run_ = true; 204 | impl_->rx_thread_ = impl_->rxThreadHelper(); 205 | } 206 | 207 | void VescInterface::disconnect() 208 | { 209 | // todo - mutex? 210 | 211 | if (isConnected()) 212 | { 213 | // bring down read thread 214 | impl_->rx_thread_run_ = false; 215 | impl_->rx_thread_.join(); 216 | impl_->serial_.close(); 217 | } 218 | } 219 | 220 | bool VescInterface::isConnected() const 221 | { 222 | return impl_->serial_.isOpen(); 223 | } 224 | 225 | void VescInterface::send(const VescPacket& packet) 226 | { 227 | size_t written = impl_->serial_.write(packet.frame()); 228 | if (written != packet.frame().size()) 229 | { 230 | std::stringstream ss; 231 | ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; 232 | throw SerialException(ss.str().c_str()); 233 | } 234 | } 235 | 236 | void VescInterface::requestFWVersion() 237 | { 238 | send(VescPacketRequestFWVersion()); 239 | } 240 | 241 | void VescInterface::requestState() 242 | { 243 | send(VescPacketRequestValues()); 244 | } 245 | 246 | void VescInterface::setDutyCycle(double duty_cycle) 247 | { 248 | send(VescPacketSetDuty(duty_cycle)); 249 | } 250 | 251 | void VescInterface::setCurrent(double current) 252 | { 253 | send(VescPacketSetCurrent(current)); 254 | } 255 | 256 | void VescInterface::setBrake(double brake) 257 | { 258 | send(VescPacketSetCurrentBrake(brake)); 259 | } 260 | 261 | void VescInterface::setSpeed(double speed) 262 | { 263 | send(VescPacketSetRPM(speed)); 264 | } 265 | 266 | void VescInterface::setPosition(double position) 267 | { 268 | send(VescPacketSetPos(position)); 269 | } 270 | 271 | void VescInterface::setServo(double servo) 272 | { 273 | send(VescPacketSetServoPos(servo)); 274 | } 275 | 276 | } // namespace vesc_driver 277 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_driver.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #include "vesc_driver/vesc_driver.h" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace vesc_driver 39 | { 40 | 41 | using std::placeholders::_1; 42 | 43 | VescDriver::VescDriver(ros::NodeHandle nh, 44 | ros::NodeHandle private_nh) : 45 | vesc_(std::string(), 46 | std::bind(&VescDriver::vescPacketCallback, this, _1), 47 | std::bind(&VescDriver::vescErrorCallback, this, _1)), 48 | duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"), 49 | brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), 50 | position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), 51 | driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) 52 | { 53 | // get vesc serial port address 54 | std::string port; 55 | if (!private_nh.getParam("port", port)) 56 | { 57 | ROS_FATAL("VESC communication port parameter required."); 58 | ros::shutdown(); 59 | return; 60 | } 61 | 62 | // attempt to connect to the serial port 63 | try 64 | { 65 | vesc_.connect(port); 66 | } 67 | catch (SerialException e) 68 | { 69 | ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); 70 | ros::shutdown(); 71 | return; 72 | } 73 | 74 | // create vesc state (telemetry) publisher 75 | state_pub_ = nh.advertise("sensors/core", 10); 76 | 77 | // since vesc state does not include the servo position, publish the commanded 78 | // servo position as a "sensor" 79 | servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); 80 | 81 | // subscribe to motor and servo command topics 82 | duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, 83 | &VescDriver::dutyCycleCallback, this); 84 | current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); 85 | brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); 86 | speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); 87 | position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); 88 | servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); 89 | 90 | // create a 50Hz timer, used for state machine & polling VESC telemetry 91 | timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); 92 | } 93 | 94 | /* TODO or TO-THINKABOUT LIST 95 | - what should we do on startup? send brake or zero command? 96 | - what to do if the vesc interface gives an error? 97 | - check version number against know compatable? 98 | - should we wait until we receive telemetry before sending commands? 99 | - should we track the last motor command 100 | - what to do if no motor command received recently? 101 | - what to do if no servo command received recently? 102 | - what is the motor safe off state (0 current?) 103 | - what to do if a command parameter is out of range, ignore? 104 | - try to predict vesc bounds (from vesc config) and command detect bounds errors 105 | */ 106 | 107 | void VescDriver::timerCallback(const ros::TimerEvent& event) 108 | { 109 | // VESC interface should not unexpectedly disconnect, but test for it anyway 110 | if (!vesc_.isConnected()) 111 | { 112 | ROS_FATAL("Unexpectedly disconnected from serial port."); 113 | timer_.stop(); 114 | ros::shutdown(); 115 | return; 116 | } 117 | 118 | /* 119 | * Driver state machine, modes: 120 | * INITIALIZING - request and wait for vesc version 121 | * OPERATING - receiving commands from subscriber topics 122 | */ 123 | if (driver_mode_ == MODE_INITIALIZING) 124 | { 125 | // request version number, return packet will update the internal version numbers 126 | vesc_.requestFWVersion(); 127 | if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) 128 | { 129 | ROS_INFO("Connected to VESC with firmware version %d.%d", 130 | fw_version_major_, fw_version_minor_); 131 | driver_mode_ = MODE_OPERATING; 132 | } 133 | } 134 | else if (driver_mode_ == MODE_OPERATING) 135 | { 136 | // poll for vesc state (telemetry) 137 | vesc_.requestState(); 138 | } 139 | else 140 | { 141 | // unknown mode, how did that happen? 142 | assert(false && "unknown driver mode"); 143 | } 144 | } 145 | 146 | void VescDriver::vescPacketCallback(const std::shared_ptr& packet) 147 | { 148 | if (packet->name() == "Values") 149 | { 150 | std::shared_ptr values = 151 | std::dynamic_pointer_cast(packet); 152 | 153 | vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); 154 | state_msg->header.stamp = ros::Time::now(); 155 | state_msg->state.voltage_input = values->v_in(); 156 | state_msg->state.temperature_pcb = values->temp_pcb(); 157 | state_msg->state.current_motor = values->current_motor(); 158 | state_msg->state.current_input = values->current_in(); 159 | state_msg->state.speed = values->rpm(); 160 | state_msg->state.duty_cycle = values->duty_now(); 161 | state_msg->state.charge_drawn = values->amp_hours(); 162 | state_msg->state.charge_regen = values->amp_hours_charged(); 163 | state_msg->state.energy_drawn = values->watt_hours(); 164 | state_msg->state.energy_regen = values->watt_hours_charged(); 165 | state_msg->state.displacement = values->tachometer(); 166 | state_msg->state.distance_traveled = values->tachometer_abs(); 167 | state_msg->state.fault_code = values->fault_code(); 168 | 169 | state_pub_.publish(state_msg); 170 | } 171 | else if (packet->name() == "FWVersion") 172 | { 173 | std::shared_ptr fw_version = 174 | std::dynamic_pointer_cast(packet); 175 | // todo: might need lock here 176 | fw_version_major_ = fw_version->fwMajor(); 177 | fw_version_minor_ = fw_version->fwMinor(); 178 | } 179 | } 180 | 181 | void VescDriver::vescErrorCallback(const std::string& error) 182 | { 183 | ROS_ERROR("%s", error.c_str()); 184 | } 185 | 186 | /** 187 | * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However, 188 | * note that the VESC may impose a more restrictive bounds on the range depending 189 | * on its configuration, e.g. absolute value is between 0.05 and 0.95. 190 | */ 191 | void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) 192 | { 193 | if (driver_mode_ == MODE_OPERATING) 194 | { 195 | vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); 196 | } 197 | } 198 | 199 | /** 200 | * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However, 201 | * note that the VESC may impose a more restrictive bounds on the range depending on 202 | * its configuration. 203 | */ 204 | void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) 205 | { 206 | if (driver_mode_ == MODE_OPERATING) 207 | { 208 | vesc_.setCurrent(current_limit_.clip(current->data)); 209 | } 210 | } 211 | 212 | /** 213 | * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver. 214 | * However, note that the VESC may impose a more restrictive bounds on the range 215 | * depending on its configuration. 216 | */ 217 | void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) 218 | { 219 | if (driver_mode_ == MODE_OPERATING) 220 | { 221 | vesc_.setBrake(brake_limit_.clip(brake->data)); 222 | } 223 | } 224 | 225 | /** 226 | * @param speed Commanded VESC speed in electrical RPM. Electrical RPM is the mechanical RPM 227 | * multiplied by the number of motor poles. Any value is accepted by this 228 | * driver. However, note that the VESC may impose a more restrictive bounds on the 229 | * range depending on its configuration. 230 | */ 231 | void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) 232 | { 233 | if (driver_mode_ == MODE_OPERATING) 234 | { 235 | vesc_.setSpeed(speed_limit_.clip(speed->data)); 236 | } 237 | } 238 | 239 | /** 240 | * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. 241 | * Note that the VESC must be in encoder mode for this command to have an effect. 242 | */ 243 | void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) 244 | { 245 | if (driver_mode_ == MODE_OPERATING) 246 | { 247 | // ROS uses radians but VESC seems to use degrees. Convert to degrees. 248 | double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; 249 | vesc_.setPosition(position_deg); 250 | } 251 | } 252 | 253 | /** 254 | * @param servo Commanded VESC servo output position. Valid range is 0 to 1. 255 | */ 256 | void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) 257 | { 258 | if (driver_mode_ == MODE_OPERATING) 259 | { 260 | double servo_clipped(servo_limit_.clip(servo->data)); 261 | vesc_.setServo(servo_clipped); 262 | // publish clipped servo value as a "sensor" 263 | std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); 264 | servo_sensor_msg->data = servo_clipped; 265 | servo_sensor_pub_.publish(servo_sensor_msg); 266 | } 267 | } 268 | 269 | VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, 270 | const boost::optional& min_lower, 271 | const boost::optional& max_upper) : 272 | name(str) 273 | { 274 | // check if user's minimum value is outside of the range min_lower to max_upper 275 | double param_min; 276 | if (nh.getParam(name + "_min", param_min)) 277 | { 278 | if (min_lower && param_min < *min_lower) 279 | { 280 | lower = *min_lower; 281 | ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << 282 | ") is less than the feasible minimum (" << *min_lower << ")."); 283 | } 284 | else if (max_upper && param_min > *max_upper) 285 | { 286 | lower = *max_upper; 287 | ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << 288 | ") is greater than the feasible maximum (" << *max_upper << ")."); 289 | } 290 | else 291 | { 292 | lower = param_min; 293 | } 294 | } 295 | else if (min_lower) 296 | { 297 | lower = *min_lower; 298 | } 299 | 300 | // check if the uers' maximum value is outside of the range min_lower to max_upper 301 | double param_max; 302 | if (nh.getParam(name + "_max", param_max)) 303 | { 304 | if (min_lower && param_max < *min_lower) 305 | { 306 | upper = *min_lower; 307 | ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << 308 | ") is less than the feasible minimum (" << *min_lower << ")."); 309 | } 310 | else if (max_upper && param_max > *max_upper) 311 | { 312 | upper = *max_upper; 313 | ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << 314 | ") is greater than the feasible maximum (" << *max_upper << ")."); 315 | } 316 | else 317 | { 318 | upper = param_max; 319 | } 320 | } 321 | else if (max_upper) 322 | { 323 | upper = *max_upper; 324 | } 325 | 326 | // check for min > max 327 | if (upper && lower && *lower > *upper) 328 | { 329 | ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper 330 | << ") is less than parameter " << name << "_min (" << *lower << ")."); 331 | double temp(*lower); 332 | lower = *upper; 333 | upper = temp; 334 | } 335 | 336 | std::ostringstream oss; 337 | oss << " " << name << " limit: "; 338 | if (lower) oss << *lower << " "; 339 | else oss << "(none) "; 340 | if (upper) oss << *upper; 341 | else oss << "(none)"; 342 | ROS_DEBUG_STREAM(oss.str()); 343 | } 344 | 345 | double VescDriver::CommandLimit::clip(double value) 346 | { 347 | if (lower && value < lower) 348 | { 349 | ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", 350 | name.c_str(), value, *lower); 351 | return *lower; 352 | } 353 | if (upper && value > upper) 354 | { 355 | ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", 356 | name.c_str(), value, *upper); 357 | return *upper; 358 | } 359 | return value; 360 | } 361 | 362 | 363 | } // namespace vesc_driver 364 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_packet.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 F1TENTH Foundation 2 | // 3 | // Redistribution and use in source and binary forms, with or without modification, are permitted 4 | // provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, this list of conditions 7 | // and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | // of conditions and the following disclaimer in the documentation and/or other materials 11 | // provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors may be used 14 | // to endorse or promote products derived from this software without specific prior 15 | // written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 18 | // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 19 | // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 20 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY 24 | // WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | // -*- mode:c++; fill-column: 100; -*- 27 | 28 | #include "vesc_driver/vesc_packet.h" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "vesc_driver/vesc_packet_factory.h" 36 | #include "vesc_driver/datatypes.h" 37 | 38 | namespace vesc_driver 39 | { 40 | 41 | constexpr CRC::Parameters VescFrame::CRC_TYPE; 42 | 43 | VescFrame::VescFrame(int payload_size) 44 | { 45 | assert(payload_size >= 0 && payload_size <= 1024); 46 | 47 | if (payload_size < 256) 48 | { 49 | // single byte payload size 50 | frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); 51 | *frame_->begin() = 2; 52 | *(frame_->begin() + 1) = payload_size; 53 | payload_.first = frame_->begin() + 2; 54 | } 55 | else 56 | { 57 | // two byte payload size 58 | frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); 59 | *frame_->begin() = 3; 60 | *(frame_->begin() + 1) = payload_size >> 8; 61 | *(frame_->begin() + 2) = payload_size & 0xFF; 62 | payload_.first = frame_->begin() + 3; 63 | } 64 | 65 | payload_.second = payload_.first + payload_size; 66 | *(frame_->end() - 1) = 3; 67 | } 68 | 69 | VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload) 70 | { 71 | /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap 72 | checks anyway */ 73 | assert(std::distance(frame.first, frame.second) >= VESC_MIN_FRAME_SIZE); 74 | assert(std::distance(frame.first, frame.second) <= VESC_MAX_FRAME_SIZE); 75 | assert(std::distance(payload.first, payload.second) <= VESC_MAX_PAYLOAD_SIZE); 76 | assert(std::distance(frame.first, payload.first) > 0 && 77 | std::distance(payload.second, frame.second) > 0); 78 | 79 | frame_.reset(new Buffer(frame.first, frame.second)); 80 | payload_.first = frame_->begin() + std::distance(frame.first, payload.first); 81 | payload_.second = frame_->begin() + std::distance(frame.first, payload.second); 82 | } 83 | 84 | VescPacket::VescPacket(const std::string& name, int payload_size, int payload_id) : 85 | VescFrame(payload_size), name_(name) 86 | { 87 | assert(payload_id >= 0 && payload_id < 256); 88 | assert(std::distance(payload_.first, payload_.second) > 0); 89 | *payload_.first = payload_id; 90 | } 91 | 92 | VescPacket::VescPacket(const std::string& name, std::shared_ptr raw) : 93 | VescFrame(*raw), name_(name) 94 | { 95 | } 96 | 97 | /*------------------------------------------------------------------------------------------------*/ 98 | 99 | VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : 100 | VescPacket("FWVersion", raw) 101 | { 102 | } 103 | 104 | int VescPacketFWVersion::fwMajor() const 105 | { 106 | return *(payload_.first + 1); 107 | } 108 | 109 | int VescPacketFWVersion::fwMinor() const 110 | { 111 | return *(payload_.first + 2); 112 | } 113 | 114 | REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) 115 | 116 | VescPacketRequestFWVersion::VescPacketRequestFWVersion() : 117 | VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) 118 | { 119 | uint16_t crc = CRC::Calculate( 120 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 121 | *(frame_->end() - 3) = static_cast(crc >> 8); 122 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 123 | } 124 | 125 | /*------------------------------------------------------------------------------------------------*/ 126 | 127 | VescPacketValues::VescPacketValues(std::shared_ptr raw) : 128 | VescPacket("Values", raw) 129 | { 130 | } 131 | 132 | double VescPacketValues::temp_mos1() const 133 | { 134 | int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + 135 | static_cast(*(payload_.first + 2))); 136 | return static_cast(v) / 10.0; 137 | } 138 | 139 | double VescPacketValues::temp_mos2() const 140 | { 141 | int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + 142 | static_cast(*(payload_.first + 4))); 143 | return static_cast(v) / 10.0; 144 | } 145 | 146 | double VescPacketValues::current_motor() const 147 | { 148 | int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + 149 | (static_cast(*(payload_.first + 6)) << 16) + 150 | (static_cast(*(payload_.first + 7)) << 8) + 151 | static_cast(*(payload_.first + 8))); 152 | return static_cast(v) / 100.0; 153 | } 154 | 155 | double VescPacketValues::current_in() const 156 | { 157 | int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + 158 | (static_cast(*(payload_.first + 10)) << 16) + 159 | (static_cast(*(payload_.first + 11)) << 8) + 160 | static_cast(*(payload_.first + 12))); 161 | return static_cast(v) / 100.0; 162 | } 163 | 164 | 165 | double VescPacketValues::duty_now() const 166 | { 167 | int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + 168 | static_cast(*(payload_.first + 22))); 169 | return static_cast(v) / 1000.0; 170 | } 171 | 172 | double VescPacketValues::rpm() const 173 | { 174 | int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + 175 | (static_cast(*(payload_.first + 24)) << 16) + 176 | (static_cast(*(payload_.first + 25)) << 8) + 177 | static_cast(*(payload_.first + 26))); 178 | return static_cast(-1 * v); 179 | } 180 | 181 | double VescPacketValues::amp_hours() const 182 | { 183 | int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + 184 | (static_cast(*(payload_.first + 28)) << 16) + 185 | (static_cast(*(payload_.first + 29)) << 8) + 186 | static_cast(*(payload_.first + 30))); 187 | return static_cast(v); 188 | } 189 | 190 | double VescPacketValues::amp_hours_charged() const 191 | { 192 | int32_t v = static_cast((static_cast(*(payload_.first + 31)) << 24) + 193 | (static_cast(*(payload_.first + 32)) << 16) + 194 | (static_cast(*(payload_.first + 33)) << 8) + 195 | static_cast(*(payload_.first + 34))); 196 | return static_cast(v); 197 | } 198 | 199 | double VescPacketValues::tachometer() const 200 | { 201 | int32_t v = static_cast((static_cast(*(payload_.first + 35)) << 24) + 202 | (static_cast(*(payload_.first + 36)) << 16) + 203 | (static_cast(*(payload_.first + 37)) << 8) + 204 | static_cast(*(payload_.first + 38))); 205 | return static_cast(v); 206 | } 207 | 208 | double VescPacketValues::tachometer_abs() const 209 | { 210 | int32_t v = static_cast((static_cast(*(payload_.first + 39)) << 24) + 211 | (static_cast(*(payload_.first + 40)) << 16) + 212 | (static_cast(*(payload_.first + 41)) << 8) + 213 | static_cast(*(payload_.first + 42))); 214 | return static_cast(v); 215 | } 216 | 217 | int VescPacketValues::fault_code() const 218 | { 219 | return static_cast(*(payload_.first + 56)); 220 | } 221 | 222 | double VescPacketValues::v_in() const 223 | { 224 | int32_t v = 0; 225 | return static_cast(v); 226 | } 227 | 228 | double VescPacketValues::temp_pcb() const 229 | { 230 | int32_t v = 0; 231 | return static_cast(v); 232 | } 233 | 234 | double VescPacketValues::watt_hours() const 235 | { 236 | int32_t v = 0; 237 | return static_cast(v); 238 | } 239 | 240 | double VescPacketValues::watt_hours_charged() const 241 | { 242 | int32_t v = 0; 243 | return static_cast(v); 244 | } 245 | 246 | REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) 247 | 248 | VescPacketRequestValues::VescPacketRequestValues() : 249 | VescPacket("RequestValues", 1, COMM_GET_VALUES) 250 | { 251 | uint16_t crc = CRC::Calculate( 252 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 253 | *(frame_->end() - 3) = static_cast(crc >> 8); 254 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 255 | } 256 | 257 | /*------------------------------------------------------------------------------------------------*/ 258 | 259 | 260 | VescPacketSetDuty::VescPacketSetDuty(double duty) : 261 | VescPacket("SetDuty", 5, COMM_SET_DUTY) 262 | { 263 | /** @todo range check duty */ 264 | 265 | int32_t v = static_cast(duty * 100000.0); 266 | 267 | *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); 268 | *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); 269 | *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); 270 | *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); 271 | 272 | uint16_t crc = CRC::Calculate( 273 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 274 | *(frame_->end() - 3) = static_cast(crc >> 8); 275 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 276 | } 277 | 278 | /*------------------------------------------------------------------------------------------------*/ 279 | 280 | VescPacketSetCurrent::VescPacketSetCurrent(double current) : 281 | VescPacket("SetCurrent", 5, COMM_SET_CURRENT) 282 | { 283 | int32_t v = static_cast(current * 1000.0); 284 | 285 | *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); 286 | *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); 287 | *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); 288 | *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); 289 | 290 | uint16_t crc = CRC::Calculate( 291 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 292 | *(frame_->end() - 3) = static_cast(crc >> 8); 293 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 294 | } 295 | 296 | /*------------------------------------------------------------------------------------------------*/ 297 | 298 | VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : 299 | VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) 300 | { 301 | int32_t v = static_cast(current_brake * 1000.0); 302 | 303 | *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); 304 | *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); 305 | *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); 306 | *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); 307 | 308 | uint16_t crc = CRC::Calculate( 309 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 310 | *(frame_->end() - 3) = static_cast(crc >> 8); 311 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 312 | } 313 | 314 | /*------------------------------------------------------------------------------------------------*/ 315 | 316 | VescPacketSetRPM::VescPacketSetRPM(double rpm) : 317 | VescPacket("SetRPM", 5, COMM_SET_RPM) 318 | { 319 | int32_t v = static_cast(rpm); 320 | 321 | *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); 322 | *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); 323 | *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); 324 | *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); 325 | 326 | uint16_t crc = CRC::Calculate( 327 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 328 | *(frame_->end() - 3) = static_cast(crc >> 8); 329 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 330 | } 331 | 332 | /*------------------------------------------------------------------------------------------------*/ 333 | 334 | VescPacketSetPos::VescPacketSetPos(double pos) : 335 | VescPacket("SetPos", 5, COMM_SET_POS) 336 | { 337 | /** @todo range check pos */ 338 | 339 | int32_t v = static_cast(pos * 1000000.0); 340 | 341 | *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); 342 | *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); 343 | *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); 344 | *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); 345 | 346 | uint16_t crc = CRC::Calculate( 347 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 348 | *(frame_->end() - 3) = static_cast(crc >> 8); 349 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 350 | } 351 | 352 | /*------------------------------------------------------------------------------------------------*/ 353 | 354 | VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : 355 | VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) 356 | { 357 | /** @todo range check pos */ 358 | 359 | int16_t v = static_cast(servo_pos * 1000.0); 360 | 361 | *(payload_.first + 1) = static_cast((static_cast(v) >> 8) & 0xFF); 362 | *(payload_.first + 2) = static_cast(static_cast(v) & 0xFF); 363 | 364 | uint16_t crc = CRC::Calculate( 365 | &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); 366 | *(frame_->end() - 3) = static_cast(crc >> 8); 367 | *(frame_->end() - 2) = static_cast(crc & 0xFF); 368 | } 369 | 370 | } // namespace vesc_driver 371 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | @file CRC.h 3 | @author Daniel Bahr 4 | @version 1.0.1.0 5 | @copyright 6 | @parblock 7 | CRC++ 8 | Copyright (c) 2020, Daniel Bahr 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of CRC++ nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | @endparblock 36 | */ 37 | 38 | /* 39 | CRC++ can be configured by setting various #defines before #including this header file: 40 | 41 | #define crcpp_uint8 - Specifies the type used to store CRCs that have a width of 8 bits or less. 42 | This type is not used in CRC calculations. Defaults to ::std::uint8_t. 43 | #define crcpp_uint16 - Specifies the type used to store CRCs that have a width between 9 and 16 bits (inclusive). 44 | This type is not used in CRC calculations. Defaults to ::std::uint16_t. 45 | #define crcpp_uint32 - Specifies the type used to store CRCs that have a width between 17 and 32 bits (inclusive). 46 | This type is not used in CRC calculations. Defaults to ::std::uint32_t. 47 | #define crcpp_uint64 - Specifies the type used to store CRCs that have a width between 33 and 64 bits (inclusive). 48 | This type is not used in CRC calculations. Defaults to ::std::uint64_t. 49 | #define crcpp_size - This type is used for loop iteration and function signatures only. Defaults to ::std::size_t. 50 | #define CRCPP_USE_NAMESPACE - Define to place all CRC++ code within the ::CRCPP namespace. 51 | #define CRCPP_BRANCHLESS - Define to enable a branchless CRC implementation. The branchless implementation uses a single integer 52 | multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation 53 | may be faster on processor architectures which support single-instruction integer multiplication. 54 | #define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). 55 | #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. 56 | */ 57 | 58 | #ifndef VESC_DRIVER_CRC_H_ 59 | #define VESC_DRIVER_CRC_H_ 60 | 61 | #include // Includes CHAR_BIT 62 | #ifdef CRCPP_USE_CPP11 63 | #include // Includes ::std::size_t 64 | #include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t 65 | #else 66 | #include // Includes size_t 67 | #include // Includes uint8_t, uint16_t, uint32_t, uint64_t 68 | #endif 69 | #include // Includes ::std::numeric_limits 70 | #include // Includes ::std::move 71 | 72 | #ifndef crcpp_uint8 73 | # ifdef CRCPP_USE_CPP11 74 | /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. 75 | # define crcpp_uint8 ::std::uint8_t 76 | # else 77 | /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. 78 | # define crcpp_uint8 uint8_t 79 | # endif 80 | #endif 81 | 82 | #ifndef crcpp_uint16 83 | # ifdef CRCPP_USE_CPP11 84 | /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. 85 | # define crcpp_uint16 ::std::uint16_t 86 | # else 87 | /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. 88 | # define crcpp_uint16 uint16_t 89 | # endif 90 | #endif 91 | 92 | #ifndef crcpp_uint32 93 | # ifdef CRCPP_USE_CPP11 94 | /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. 95 | # define crcpp_uint32 ::std::uint32_t 96 | # else 97 | /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. 98 | # define crcpp_uint32 uint32_t 99 | # endif 100 | #endif 101 | 102 | #ifndef crcpp_uint64 103 | # ifdef CRCPP_USE_CPP11 104 | /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. 105 | # define crcpp_uint64 ::std::uint64_t 106 | # else 107 | /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. 108 | # define crcpp_uint64 uint64_t 109 | # endif 110 | #endif 111 | 112 | #ifndef crcpp_size 113 | # ifdef CRCPP_USE_CPP11 114 | /// @brief Unsigned size definition, used for specifying data sizes. 115 | # define crcpp_size ::std::size_t 116 | # else 117 | /// @brief Unsigned size definition, used for specifying data sizes. 118 | # define crcpp_size size_t 119 | # endif 120 | #endif 121 | 122 | #ifdef CRCPP_USE_CPP11 123 | /// @brief Compile-time expression definition. 124 | # define crcpp_constexpr constexpr 125 | #else 126 | /// @brief Compile-time expression definition. 127 | # define crcpp_constexpr const 128 | #endif 129 | 130 | #ifdef CRCPP_USE_NAMESPACE 131 | namespace CRCPP 132 | { 133 | #endif 134 | 135 | /** 136 | @brief Static class for computing CRCs. 137 | @note This class supports computation of full and multi-part CRCs, using a bit-by-bit algorithm or a 138 | byte-by-byte lookup table. The CRCs are calculated using as many optimizations as is reasonable. 139 | If compiling with C++11, the constexpr keyword is used liberally so that many calculations are 140 | performed at compile-time instead of at runtime. 141 | */ 142 | class CRC 143 | { 144 | public: 145 | // Forward declaration 146 | template 147 | struct Table; 148 | 149 | /** 150 | @brief CRC parameters. 151 | */ 152 | template 153 | struct Parameters 154 | { 155 | CRCType polynomial; ///< CRC polynomial 156 | CRCType initialValue; ///< Initial CRC value 157 | CRCType finalXOR; ///< Value to XOR with the final CRC 158 | bool reflectInput; ///< true to reflect all input bytes 159 | bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) 160 | 161 | Table MakeTable() const; 162 | }; 163 | 164 | /** 165 | @brief CRC lookup table. After construction, the CRC parameters are fixed. 166 | @note A CRC table can be used for multiple CRC calculations. 167 | */ 168 | template 169 | struct Table 170 | { 171 | // Constructors are intentionally NOT marked explicit. 172 | Table(const Parameters & parameters); 173 | 174 | #ifdef CRCPP_USE_CPP11 175 | Table(Parameters && parameters); 176 | #endif 177 | 178 | const Parameters & GetParameters() const; 179 | 180 | const CRCType * GetTable() const; 181 | 182 | CRCType operator[](unsigned char index) const; 183 | 184 | private: 185 | void InitTable(); 186 | 187 | Parameters parameters; ///< CRC parameters used to construct the table 188 | CRCType table[1 << CHAR_BIT]; ///< CRC lookup table 189 | }; 190 | 191 | // The number of bits in CRCType must be at least as large as CRCWidth. 192 | // CRCType must be an unsigned integer type or a custom type with operator overloads. 193 | template 194 | static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters); 195 | 196 | template 197 | static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); 198 | 199 | template 200 | static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable); 201 | 202 | template 203 | static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); 204 | 205 | // Common CRCs up to 64 bits. 206 | // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) 207 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 208 | static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); 209 | static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); 210 | static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); 211 | static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); 212 | static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); 213 | static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); 214 | static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); 215 | static const Parameters< crcpp_uint8, 7> & CRC_7(); 216 | #endif 217 | static const Parameters< crcpp_uint8, 8> & CRC_8(); 218 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 219 | static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); 220 | static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); 221 | static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); 222 | static const Parameters & CRC_10(); 223 | static const Parameters & CRC_10_CDMA2000(); 224 | static const Parameters & CRC_11(); 225 | static const Parameters & CRC_12_CDMA2000(); 226 | static const Parameters & CRC_12_DECT(); 227 | static const Parameters & CRC_12_UMTS(); 228 | static const Parameters & CRC_13_BBC(); 229 | static const Parameters & CRC_15(); 230 | static const Parameters & CRC_15_MPT1327(); 231 | #endif 232 | static const Parameters & CRC_16_ARC(); 233 | static const Parameters & CRC_16_BUYPASS(); 234 | static const Parameters & CRC_16_CCITTFALSE(); 235 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 236 | static const Parameters & CRC_16_CDMA2000(); 237 | static const Parameters & CRC_16_CMS(); 238 | static const Parameters & CRC_16_DECTR(); 239 | static const Parameters & CRC_16_DECTX(); 240 | static const Parameters & CRC_16_DNP(); 241 | #endif 242 | static const Parameters & CRC_16_GENIBUS(); 243 | static const Parameters & CRC_16_KERMIT(); 244 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 245 | static const Parameters & CRC_16_MAXIM(); 246 | static const Parameters & CRC_16_MODBUS(); 247 | static const Parameters & CRC_16_T10DIF(); 248 | static const Parameters & CRC_16_USB(); 249 | #endif 250 | static const Parameters & CRC_16_X25(); 251 | static const Parameters & CRC_16_XMODEM(); 252 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 253 | static const Parameters & CRC_17_CAN(); 254 | static const Parameters & CRC_21_CAN(); 255 | static const Parameters & CRC_24(); 256 | static const Parameters & CRC_24_FLEXRAYA(); 257 | static const Parameters & CRC_24_FLEXRAYB(); 258 | static const Parameters & CRC_30(); 259 | #endif 260 | static const Parameters & CRC_32(); 261 | static const Parameters & CRC_32_BZIP2(); 262 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 263 | static const Parameters & CRC_32_C(); 264 | #endif 265 | static const Parameters & CRC_32_MPEG2(); 266 | static const Parameters & CRC_32_POSIX(); 267 | #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS 268 | static const Parameters & CRC_32_Q(); 269 | static const Parameters & CRC_40_GSM(); 270 | static const Parameters & CRC_64(); 271 | #endif 272 | 273 | #ifdef CRCPP_USE_CPP11 274 | CRC() = delete; 275 | CRC(const CRC & other) = delete; 276 | CRC & operator=(const CRC & other) = delete; 277 | CRC(CRC && other) = delete; 278 | CRC & operator=(CRC && other) = delete; 279 | #endif 280 | 281 | private: 282 | #ifndef CRCPP_USE_CPP11 283 | CRC(); 284 | CRC(const CRC & other); 285 | CRC & operator=(const CRC & other); 286 | #endif 287 | 288 | template 289 | static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); 290 | 291 | template 292 | static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); 293 | 294 | template 295 | static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); 296 | 297 | template 298 | static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); 299 | 300 | template 301 | static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); 302 | }; 303 | 304 | /** 305 | @brief Returns a CRC lookup table construct using these CRC parameters. 306 | @note This function primarily exists to allow use of the auto keyword instead of instantiating 307 | a table directly, since template parameters are not inferred in constructors. 308 | @tparam CRCType Integer type for storing the CRC result 309 | @tparam CRCWidth Number of bits in the CRC 310 | @return CRC lookup table 311 | */ 312 | template 313 | inline CRC::Table CRC::Parameters::MakeTable() const 314 | { 315 | // This should take advantage of RVO and optimize out the copy. 316 | return CRC::Table(*this); 317 | } 318 | 319 | /** 320 | @brief Constructs a CRC table from a set of CRC parameters 321 | @param[in] params CRC parameters 322 | @tparam CRCType Integer type for storing the CRC result 323 | @tparam CRCWidth Number of bits in the CRC 324 | */ 325 | template 326 | inline CRC::Table::Table(const Parameters & params) : 327 | parameters(params) 328 | { 329 | InitTable(); 330 | } 331 | 332 | #ifdef CRCPP_USE_CPP11 333 | /** 334 | @brief Constructs a CRC table from a set of CRC parameters 335 | @param[in] params CRC parameters 336 | @tparam CRCType Integer type for storing the CRC result 337 | @tparam CRCWidth Number of bits in the CRC 338 | */ 339 | template 340 | inline CRC::Table::Table(Parameters && params) : 341 | parameters(::std::move(params)) 342 | { 343 | InitTable(); 344 | } 345 | #endif 346 | 347 | /** 348 | @brief Gets the CRC parameters used to construct the CRC table 349 | @tparam CRCType Integer type for storing the CRC result 350 | @tparam CRCWidth Number of bits in the CRC 351 | @return CRC parameters 352 | */ 353 | template 354 | inline const CRC::Parameters & CRC::Table::GetParameters() const 355 | { 356 | return parameters; 357 | } 358 | 359 | /** 360 | @brief Gets the CRC table 361 | @tparam CRCType Integer type for storing the CRC result 362 | @tparam CRCWidth Number of bits in the CRC 363 | @return CRC table 364 | */ 365 | template 366 | inline const CRCType * CRC::Table::GetTable() const 367 | { 368 | return table; 369 | } 370 | 371 | /** 372 | @brief Gets an entry in the CRC table 373 | @param[in] index Index into the CRC table 374 | @tparam CRCType Integer type for storing the CRC result 375 | @tparam CRCWidth Number of bits in the CRC 376 | @return CRC table entry 377 | */ 378 | template 379 | inline CRCType CRC::Table::operator[](unsigned char index) const 380 | { 381 | return table[index]; 382 | } 383 | 384 | /** 385 | @brief Initializes a CRC table. 386 | @tparam CRCType Integer type for storing the CRC result 387 | @tparam CRCWidth Number of bits in the CRC 388 | */ 389 | template 390 | inline void CRC::Table::InitTable() 391 | { 392 | // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) 393 | static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | 394 | ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); 395 | 396 | // The conditional expression is used to avoid a -Wshift-count-overflow warning. 397 | static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); 398 | 399 | CRCType crc; 400 | unsigned char byte = 0; 401 | 402 | // Loop over each dividend (each possible number storable in an unsigned char) 403 | do 404 | { 405 | crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); 406 | 407 | // This mask might not be necessary; all unit tests pass with this line commented out, 408 | // but that might just be a coincidence based on the CRC parameters used for testing. 409 | // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. 410 | crc &= BIT_MASK; 411 | 412 | if (!parameters.reflectInput && CRCWidth < CHAR_BIT) 413 | { 414 | // Undo the special operation at the end of the CalculateRemainder() 415 | // function for non-reflected CRCs < CHAR_BIT. 416 | crc = static_cast(crc << SHIFT); 417 | } 418 | 419 | table[byte] = crc; 420 | } 421 | while (++byte); 422 | } 423 | 424 | /** 425 | @brief Computes a CRC. 426 | @param[in] data Data over which CRC will be computed 427 | @param[in] size Size of the data 428 | @param[in] parameters CRC parameters 429 | @tparam CRCType Integer type for storing the CRC result 430 | @tparam CRCWidth Number of bits in the CRC 431 | @return CRC 432 | */ 433 | template 434 | inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) 435 | { 436 | CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); 437 | 438 | // No need to mask the remainder here; the mask will be applied in the Finalize() function. 439 | 440 | return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); 441 | } 442 | /** 443 | @brief Appends additional data to a previous CRC calculation. 444 | @note This function can be used to compute multi-part CRCs. 445 | @param[in] data Data over which CRC will be computed 446 | @param[in] size Size of the data 447 | @param[in] parameters CRC parameters 448 | @param[in] crc CRC from a previous calculation 449 | @tparam CRCType Integer type for storing the CRC result 450 | @tparam CRCWidth Number of bits in the CRC 451 | @return CRC 452 | */ 453 | template 454 | inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) 455 | { 456 | CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); 457 | 458 | remainder = CalculateRemainder(data, size, parameters, remainder); 459 | 460 | // No need to mask the remainder here; the mask will be applied in the Finalize() function. 461 | 462 | return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); 463 | } 464 | 465 | /** 466 | @brief Computes a CRC via a lookup table. 467 | @param[in] data Data over which CRC will be computed 468 | @param[in] size Size of the data 469 | @param[in] lookupTable CRC lookup table 470 | @tparam CRCType Integer type for storing the CRC result 471 | @tparam CRCWidth Number of bits in the CRC 472 | @return CRC 473 | */ 474 | template 475 | inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) 476 | { 477 | const Parameters & parameters = lookupTable.GetParameters(); 478 | 479 | CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); 480 | 481 | // No need to mask the remainder here; the mask will be applied in the Finalize() function. 482 | 483 | return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); 484 | } 485 | 486 | /** 487 | @brief Appends additional data to a previous CRC calculation using a lookup table. 488 | @note This function can be used to compute multi-part CRCs. 489 | @param[in] data Data over which CRC will be computed 490 | @param[in] size Size of the data 491 | @param[in] lookupTable CRC lookup table 492 | @param[in] crc CRC from a previous calculation 493 | @tparam CRCType Integer type for storing the CRC result 494 | @tparam CRCWidth Number of bits in the CRC 495 | @return CRC 496 | */ 497 | template 498 | inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) 499 | { 500 | const Parameters & parameters = lookupTable.GetParameters(); 501 | 502 | CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); 503 | 504 | remainder = CalculateRemainder(data, size, lookupTable, remainder); 505 | 506 | // No need to mask the remainder here; the mask will be applied in the Finalize() function. 507 | 508 | return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); 509 | } 510 | 511 | /** 512 | @brief Reflects (i.e. reverses the bits within) an integer value. 513 | @param[in] value Value to reflect 514 | @param[in] numBits Number of bits in the integer which will be reflected 515 | @tparam IntegerType Integer type of the value being reflected 516 | @return Reflected value 517 | */ 518 | template 519 | inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) 520 | { 521 | IntegerType reversedValue(0); 522 | 523 | for (crcpp_uint16 i = 0; i < numBits; ++i) 524 | { 525 | reversedValue = static_cast((reversedValue << 1) | (value & 1)); 526 | value = static_cast(value >> 1); 527 | } 528 | 529 | return reversedValue; 530 | } 531 | 532 | /** 533 | @brief Computes the final reflection and XOR of a CRC remainder. 534 | @param[in] remainder CRC remainder to reflect and XOR 535 | @param[in] finalXOR Final value to XOR with the remainder 536 | @param[in] reflectOutput true to reflect each byte of the remainder before the XOR 537 | @tparam CRCType Integer type for storing the CRC result 538 | @tparam CRCWidth Number of bits in the CRC 539 | @return Final CRC 540 | */ 541 | template 542 | inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) 543 | { 544 | // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) 545 | static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | 546 | ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); 547 | 548 | if (reflectOutput) 549 | { 550 | remainder = Reflect(remainder, CRCWidth); 551 | } 552 | 553 | return (remainder ^ finalXOR) & BIT_MASK; 554 | } 555 | 556 | /** 557 | @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. 558 | @note This function allows for computation of multi-part CRCs 559 | @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: 560 | 561 | CRCType x = ...; 562 | CRCType y = Finalize(x, finalXOR, reflectOutput); 563 | CRCType z = UndoFinalize(y, finalXOR, reflectOutput); 564 | assert(x == z); 565 | 566 | @param[in] crc Reflected and XORed CRC 567 | @param[in] finalXOR Final value XORed with the remainder 568 | @param[in] reflectOutput true if the remainder is to be reflected 569 | @tparam CRCType Integer type for storing the CRC result 570 | @tparam CRCWidth Number of bits in the CRC 571 | @return Un-finalized CRC remainder 572 | */ 573 | template 574 | inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) 575 | { 576 | // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) 577 | static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | 578 | ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); 579 | 580 | crc = (crc & BIT_MASK) ^ finalXOR; 581 | 582 | if (reflectOutput) 583 | { 584 | crc = Reflect(crc, CRCWidth); 585 | } 586 | 587 | return crc; 588 | } 589 | 590 | /** 591 | @brief Computes a CRC remainder. 592 | @param[in] data Data over which the remainder will be computed 593 | @param[in] size Size of the data 594 | @param[in] parameters CRC parameters 595 | @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. 596 | @tparam CRCType Integer type for storing the CRC result 597 | @tparam CRCWidth Number of bits in the CRC 598 | @return CRC remainder 599 | */ 600 | template 601 | inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) 602 | { 603 | #ifdef CRCPP_USE_CPP11 604 | // This static_assert is put here because this function will always be compiled in no matter what 605 | // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. 606 | static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); 607 | #else 608 | // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's 609 | // better than nothing. 610 | enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; 611 | #endif 612 | 613 | const unsigned char * current = reinterpret_cast(data); 614 | 615 | // Slightly different implementations based on the parameters. The current implementations try to eliminate as much 616 | // computation from the inner loop (looping over each bit) as possible. 617 | if (parameters.reflectInput) 618 | { 619 | CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); 620 | while (size--) 621 | { 622 | remainder = static_cast(remainder ^ *current++); 623 | 624 | // An optimizing compiler might choose to unroll this loop. 625 | for (crcpp_size i = 0; i < CHAR_BIT; ++i) 626 | { 627 | #ifdef CRCPP_BRANCHLESS 628 | // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: 629 | // if (remainder & 1) 630 | // remainder = (remainder >> 1) ^ polynomial; 631 | // else 632 | // remainder >>= 1; 633 | remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); 634 | #else 635 | remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); 636 | #endif 637 | } 638 | } 639 | } 640 | else if (CRCWidth >= CHAR_BIT) 641 | { 642 | static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); 643 | #ifndef CRCPP_BRANCHLESS 644 | static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); 645 | #endif 646 | // The conditional expression is used to avoid a -Wshift-count-overflow warning. 647 | static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); 648 | 649 | while (size--) 650 | { 651 | remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); 652 | 653 | // An optimizing compiler might choose to unroll this loop. 654 | for (crcpp_size i = 0; i < CHAR_BIT; ++i) 655 | { 656 | #ifdef CRCPP_BRANCHLESS 657 | // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: 658 | // if (remainder & CRC_HIGHEST_BIT_MASK) 659 | // remainder = (remainder << 1) ^ parameters.polynomial; 660 | // else 661 | // remainder <<= 1; 662 | remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); 663 | #else 664 | remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); 665 | #endif 666 | } 667 | } 668 | } 669 | else 670 | { 671 | static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); 672 | #ifndef CRCPP_BRANCHLESS 673 | static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); 674 | #endif 675 | // The conditional expression is used to avoid a -Wshift-count-overflow warning. 676 | static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); 677 | 678 | CRCType polynomial = static_cast(parameters.polynomial << SHIFT); 679 | remainder = static_cast(remainder << SHIFT); 680 | 681 | while (size--) 682 | { 683 | remainder = static_cast(remainder ^ *current++); 684 | 685 | // An optimizing compiler might choose to unroll this loop. 686 | for (crcpp_size i = 0; i < CHAR_BIT; ++i) 687 | { 688 | #ifdef CRCPP_BRANCHLESS 689 | // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: 690 | // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) 691 | // remainder = (remainder << 1) ^ polynomial; 692 | // else 693 | // remainder <<= 1; 694 | remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); 695 | #else 696 | remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); 697 | #endif 698 | } 699 | } 700 | 701 | remainder = static_cast(remainder >> SHIFT); 702 | } 703 | 704 | return remainder; 705 | } 706 | 707 | /** 708 | @brief Computes a CRC remainder using lookup table. 709 | @param[in] data Data over which the remainder will be computed 710 | @param[in] size Size of the data 711 | @param[in] lookupTable CRC lookup table 712 | @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. 713 | @tparam CRCType Integer type for storing the CRC result 714 | @tparam CRCWidth Number of bits in the CRC 715 | @return CRC remainder 716 | */ 717 | template 718 | inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) 719 | { 720 | const unsigned char * current = reinterpret_cast(data); 721 | 722 | if (lookupTable.GetParameters().reflectInput) 723 | { 724 | while (size--) 725 | { 726 | #if defined(WIN32) || defined(_WIN32) || defined(WINCE) 727 | // Disable warning about data loss when doing (remainder >> CHAR_BIT) when 728 | // remainder is one byte long. The algorithm is still correct in this case, 729 | // though it's possible that one additional machine instruction will be executed. 730 | # pragma warning (push) 731 | # pragma warning (disable : 4333) 732 | #endif 733 | remainder = static_cast((remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); 734 | #if defined(WIN32) || defined(_WIN32) || defined(WINCE) 735 | # pragma warning (pop) 736 | #endif 737 | } 738 | } 739 | else if (CRCWidth >= CHAR_BIT) 740 | { 741 | // The conditional expression is used to avoid a -Wshift-count-overflow warning. 742 | static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); 743 | 744 | while (size--) 745 | { 746 | remainder = static_cast((remainder << CHAR_BIT) ^ lookupTable[static_cast