From f075df85f4bcba780b32e38a5d14e050c6c5b5e6 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Sun, 9 Apr 2023 18:47:43 -0400 Subject: [PATCH 01/20] [IMU]: added madgwick imu filter --- README.md | 7 +- vesc.yaml | 42 + vesc/CHANGELOG.rst | 16 - vesc/package.xml | 23 +- vesc_ackermann/CHANGELOG.rst | 22 - vesc_ackermann/CMakeLists.txt | 31 +- .../vesc_ackermann/ackermann_to_vesc.h | 34 +- .../include/vesc_ackermann/vesc_to_odom.h | 40 +- vesc_ackermann/package.xml | 45 +- vesc_ackermann/src/ackermann_to_vesc.cpp | 94 +- vesc_ackermann/src/ackermann_to_vesc_node.cpp | 25 - .../src/ackermann_to_vesc_nodelet.cpp | 37 +- vesc_ackermann/src/vesc_to_odom.cpp | 82 +- vesc_ackermann/src/vesc_to_odom_node.cpp | 25 - vesc_ackermann/src/vesc_to_odom_nodelet.cpp | 37 +- vesc_driver/CHANGELOG.rst | 27 - vesc_driver/CMakeLists.txt | 33 +- vesc_driver/include/vesc_driver/crc.h | 1705 ----------------- vesc_driver/include/vesc_driver/datatypes.h | 345 ++-- vesc_driver/include/vesc_driver/v8stdint.h | 57 + vesc_driver/include/vesc_driver/vesc_driver.h | 42 +- .../include/vesc_driver/vesc_interface.h | 72 +- vesc_driver/include/vesc_driver/vesc_packet.h | 160 +- .../include/vesc_driver/vesc_packet_factory.h | 86 +- vesc_driver/launch/vesc_driver_node.launch | 13 + vesc_driver/package.xml | 37 +- vesc_driver/src/vesc_driver.cpp | 633 +++--- vesc_driver/src/vesc_driver_node.cpp | 25 - vesc_driver/src/vesc_driver_nodelet.cpp | 37 +- vesc_driver/src/vesc_interface.cpp | 120 +- vesc_driver/src/vesc_packet.cpp | 835 +++++--- vesc_driver/src/vesc_packet_factory.cpp | 71 +- vesc_msgs/CHANGELOG.rst | 17 - vesc_msgs/CMakeLists.txt | 7 +- vesc_msgs/msg/VescImu.msg | 6 + vesc_msgs/msg/VescImuStamped.msg | 4 + vesc_msgs/package.xml | 26 +- 37 files changed, 1585 insertions(+), 3333 deletions(-) create mode 100644 vesc.yaml delete mode 100644 vesc/CHANGELOG.rst delete mode 100644 vesc_ackermann/CHANGELOG.rst delete mode 100644 vesc_driver/CHANGELOG.rst delete mode 100644 vesc_driver/include/vesc_driver/crc.h create mode 100644 vesc_driver/include/vesc_driver/v8stdint.h delete mode 100644 vesc_msgs/CHANGELOG.rst create mode 100644 vesc_msgs/msg/VescImu.msg create mode 100644 vesc_msgs/msg/VescImuStamped.msg diff --git a/README.md b/README.md index 959c0dd..dec269c 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,2 @@ -# Veddar VESC Interface - -![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg) - -Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details +# vesc +VESC 6 driver diff --git a/vesc.yaml b/vesc.yaml new file mode 100644 index 0000000..4b8588c --- /dev/null +++ b/vesc.yaml @@ -0,0 +1,42 @@ + +# erpm (electrical rpm) = speed_to_erpm_gain * speed (meters / second) + speed_to_erpm_offset +speed_to_erpm_gain: 5420 #4614 +speed_to_erpm_offset: 0.0 + +# Set gains for converting acceleration to current and brake control values +accel_to_current_gain: 100 +accel_to_brake_gain: -80 + +tachometer_ticks_to_meters_gain: 0.00225 +# servo smoother - limits rotation speed and smooths anything above limit +max_servo_speed: 3.2 # radians/second +servo_smoother_rate: 75.0 # messages/sec + +# servo smoother - limits acceleration and smooths anything above limit +max_acceleration: 2.5 # meters/second^2 +throttle_smoother_rate: 75.0 # messages/sec + +# servo value (0 to 1) = steering_angle_to_servo_gain * steering angle (radians) + steering_angle_to_servo_offset +steering_angle_to_servo_gain: -1.2135 +steering_angle_to_servo_offset: 0.55 #0.5304 + +# publish odom to base link tf +vesc_to_odom/publish_tf: false + +# car wheelbase is about 25cm +wheelbase: .256 + +vesc_driver: + port: /dev/sensors/vesc + duty_cycle_min: 0.0 + duty_cycle_max: 0.0 + current_min: 0.0 + current_max: 100.0 + brake_min: -20000.0 + brake_max: 200000.0 + speed_min: -23250 + speed_max: 23250 + position_min: 0.0 + position_max: 0.0 + servo_min: 0.15 + servo_max: 0.85 diff --git a/vesc/CHANGELOG.rst b/vesc/CHANGELOG.rst deleted file mode 100644 index eb2c307..0000000 --- a/vesc/CHANGELOG.rst +++ /dev/null @@ -1,16 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package vesc -^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.0 (2020-12-12) ------------------- -* Merge pull request `#1 `_ from f1tenth/melodic-devel - Updating for Melodic -* Updating package.xml to format 3 and setting C++ standard to 11. -* Contributors: Joshua Whitley - -1.0.0 (2020-12-02) ------------------- -* Updating maintainers, authors, and URLs. -* added onboard car -* Contributors: Joshua Whitley, billyzheng diff --git a/vesc/package.xml b/vesc/package.xml index e1deeb4..2df63f4 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -1,26 +1,27 @@ - - + vesc - 1.1.0 + 0.0.1 Metapackage for ROS interface to the Vedder VESC open source motor controller. - Johannes Betz + + Michael T. Boulet + Michael T. Boulet BSD + http://www.ros.org/wiki/vesc - https://github.com/f1tenth/vesc - https://github.com/f1tenth/vesc/issues - Michael T. Boulet - Joshua Whitley + https://github.mit.edu/racecar/vesc + https://github.mit.edu/racecar/vesc/issues catkin - vesc_driver - vesc_msgs - vesc_ackermann + vesc_driver + vesc_msgs + vesc_ackermann + diff --git a/vesc_ackermann/CHANGELOG.rst b/vesc_ackermann/CHANGELOG.rst deleted file mode 100644 index bb78e5b..0000000 --- a/vesc_ackermann/CHANGELOG.rst +++ /dev/null @@ -1,22 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package vesc_ackermann -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.0 (2020-12-12) ------------------- -* Merge pull request `#1 `_ from f1tenth/melodic-devel - Updating for Melodic -* Remove unused Boost dependency. -* Replacing boost::shared_ptr with Standard Library equivalent. -* Fixing roslint error. -* Updating package.xml to format 3 and setting C++ standard to 11. -* Contributors: Joshua Whitley - -1.0.0 (2020-12-02) ------------------- -* Applying roslint to vesc_ackerman. -* Adding roslint. -* Adding licenses. -* Updating maintainers, authors, and URLs. -* added onboard car -* Contributors: Joshua Whitley, billyzheng diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 166d201..718a9d9 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -1,38 +1,29 @@ cmake_minimum_required(VERSION 2.8.3) project(vesc_ackermann) -# Setting C++ standard to 11 -if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_STANDARD 11) -endif() - find_package(catkin REQUIRED COMPONENTS - ackermann_msgs - geometry_msgs - nav_msgs nodelet pluginlib roscpp - roslint + nav_msgs std_msgs + geometry_msgs tf + ackermann_msgs vesc_msgs ) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS - ackermann_msgs - geometry_msgs - nav_msgs nodelet pluginlib + roscpp + nav_msgs#!/usr/bin/env python std_msgs + geometry_msgs tf + ackermann_msgs vesc_msgs ) @@ -42,6 +33,7 @@ catkin_package( include_directories( include + ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) @@ -70,9 +62,6 @@ target_link_libraries(vesc_ackermann_nodelet ${catkin_LIBRARIES} ) -set(ROSLINT_CPP_OPTS "--filter=-build/c++11") -roslint_cpp() - ############# ## Install ## ############# @@ -96,6 +85,4 @@ install(DIRECTORY launch/ ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - roslint_add_test() -endif() +# TODO \ No newline at end of file diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h index 2674ab3..3edb06e 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h @@ -1,28 +1,3 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ @@ -37,14 +12,13 @@ namespace vesc_ackermann class AckermannToVesc { public: + AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); private: // ROS parameters // conversion gain and offset - bool previous_mode_speed_ = true; double speed_to_erpm_gain_, speed_to_erpm_offset_; - double accel_to_current_gain_, accel_to_brake_gain_; double steering_to_servo_gain_, steering_to_servo_offset_; /** @todo consider also providing an interpolated look-up table conversion */ @@ -52,14 +26,12 @@ class AckermannToVesc // ROS services ros::Publisher erpm_pub_; ros::Publisher servo_pub_; - ros::Publisher current_pub_; - ros::Publisher brake_pub_; ros::Subscriber ackermann_sub_; // ROS callbacks void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); }; -} // namespace vesc_ackermann +} // namespace vesc_ackermann -#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ +#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h index f864833..7866a5a 100644 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h @@ -1,39 +1,12 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_ #define VESC_ACKERMANN_VESC_TO_ODOM_H_ -#include -#include - #include #include #include +#include #include namespace vesc_ackermann @@ -42,6 +15,7 @@ namespace vesc_ackermann class VescToOdom { public: + VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh); private: @@ -58,20 +32,20 @@ class VescToOdom // odometry state double x_, y_, yaw_; - std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value - vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message + std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value + vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message // ROS services ros::Publisher odom_pub_; ros::Subscriber vesc_state_sub_; ros::Subscriber servo_sub_; - std::shared_ptr tf_pub_; + boost::shared_ptr tf_pub_; // ROS callbacks void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state); void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo); }; -} // namespace vesc_ackermann +} // namespace vesc_ackermann -#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ +#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index 34d1301..ae97b77 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -1,34 +1,43 @@ - - + vesc_ackermann - 1.1.0 + 0.0.1 Translate between VESC messages and ROS ackermann and odometry messages. - Johannes Betz + + Michael T. Boulet + Michael T. Boulet BSD + http://www.ros.org/wiki/vesc_ackermann - https://github.com/f1tenth/vesc - https://github.com/f1tenth/vesc/issues - Michael T. Boulet - Joshua Whitley + https://github.mit.edu/racecar/racecar-iap + https://github.mit.edu/racecar/racecar-iap/issues catkin - roslint + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + geometry_msgs + tf + ackermann_msgs + vesc_msgs - ackermann_msgs - geometry_msgs - nav_msgs - nodelet - pluginlib - roscpp - std_msgs - tf - vesc_msgs + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + geometry_msgs + tf + ackermann_msgs + vesc_msgs + diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index c8c057f..0a00aaa 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -1,35 +1,9 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #include "vesc_ackermann/ackermann_to_vesc.h" #include #include -#include #include @@ -37,28 +11,22 @@ namespace vesc_ackermann { template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value); AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) { // get conversion parameters - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) - return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) - return; - if (!getRequiredParam(nh, "accel_to_current_gain", &accel_to_current_gain_)) + if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_)) return; - if (!getRequiredParam(nh, "accel_to_brake_gain", &accel_to_brake_gain_)) + if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_)) return; - if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) + if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_)) return; - if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) + if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_)) return; // create publishers to vesc electric-RPM (speed) and servo commands erpm_pub_ = nh.advertise("commands/motor/speed", 10); - current_pub_ = nh.advertise("commands/motor/current", 10); - brake_pub_ = nh.advertise("commands/motor/brake", 10); servo_pub_ = nh.advertise("commands/servo/position", 10); // subscribe to ackermann topic @@ -72,67 +40,25 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; - // calc vesc current/brake (acceleration) - bool is_positive_accel = true; - std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); - std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); - current_msg->data = 0; - brake_msg->data = 0; - if (cmd->drive.acceleration < 0) - { - brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; - is_positive_accel = false; - } - else - { - current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; - } - // calc steering angle (servo) std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; // publish - if (ros::ok()) - { - // The below code attempts to stick to the previous mode until a new message forces a mode switch. - if (erpm_msg->data != 0 || previous_mode_speed_) - { - erpm_pub_.publish(erpm_msg); - } - else - { - if (is_positive_accel) - { - current_pub_.publish(current_msg); - } - else - { - brake_pub_.publish(brake_msg); - } - } + if (ros::ok()) { + erpm_pub_.publish(erpm_msg); servo_pub_.publish(servo_msg); } - - // The lines below are to determine which mode we are in so we can hold until new messages force a switch. - if (erpm_msg->data != 0) - { - previous_mode_speed_ = true; - } - else if (current_msg->data != 0 || brake_msg->data != 0) - { - previous_mode_speed_ = false; - } } template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value) { - if (nh.getParam(name, *value)) + if (nh.getParam(name, value)) return true; ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str()); return false; } -} // namespace vesc_ackermann +} // namespace vesc_ackermann diff --git a/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/vesc_ackermann/src/ackermann_to_vesc_node.cpp index d7fcec0..cfe2268 100644 --- a/vesc_ackermann/src/ackermann_to_vesc_node.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc_node.cpp @@ -1,28 +1,3 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - #include #include "vesc_ackermann/ackermann_to_vesc.h" diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp index 5b72582..129df4f 100644 --- a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp @@ -1,34 +1,8 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - +#include #include #include #include -#include - #include "vesc_ackermann/ackermann_to_vesc.h" namespace vesc_ackermann @@ -37,13 +11,16 @@ namespace vesc_ackermann class AckermannToVescNodelet: public nodelet::Nodelet { public: + AckermannToVescNodelet() {} private: + virtual void onInit(void); - std::shared_ptr ackermann_to_vesc_; -}; // class AckermannToVescNodelet + boost::shared_ptr ackermann_to_vesc_; + +}; // class AckermannToVescNodelet void AckermannToVescNodelet::onInit() { @@ -51,6 +28,6 @@ void AckermannToVescNodelet::onInit() ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); } -} // namespace vesc_ackermann +} // namespace vesc_ackermann PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index cca7d11..eedd4c3 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -1,34 +1,8 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #include "vesc_ackermann/vesc_to_odom.h" #include -#include #include #include @@ -37,7 +11,7 @@ namespace vesc_ackermann { template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value); VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : odom_frame_("odom"), base_frame_("base_link"), @@ -47,17 +21,16 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : private_nh.param("odom_frame", odom_frame_, odom_frame_); private_nh.param("base_frame", base_frame_, base_frame_); private_nh.param("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_, use_servo_cmd_); - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) + if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_)) return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) + if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_)) return; - if (use_servo_cmd_) - { - if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) + if (use_servo_cmd_) { + if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_)) return; - if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) + if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_)) return; - if (!getRequiredParam(nh, "wheelbase", &wheelbase_)) + if (!getRequiredParam(nh, "wheelbase", wheelbase_)) return; } private_nh.param("publish_tf", publish_tf_, publish_tf_); @@ -66,15 +39,13 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : odom_pub_ = nh.advertise("odom", 10); // create tf broadcaster - if (publish_tf_) - { + if (publish_tf_) { tf_pub_.reset(new tf::TransformBroadcaster); } // subscribe to vesc state and. optionally, servo command vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this); - if (use_servo_cmd_) - { + if (use_servo_cmd_) { servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, &VescToOdom::servoCmdCallback, this); } @@ -87,16 +58,14 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& return; // convert to engineering units - double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; - if (std::fabs(current_speed) < 0.05) - { + double current_speed = ( -state->state.speed - speed_to_erpm_offset_ ) / speed_to_erpm_gain_; + if (std::fabs(current_speed) < 0.05){ current_speed = 0.0; } double current_steering_angle(0.0), current_angular_velocity(0.0); - if (use_servo_cmd_) - { + if (use_servo_cmd_) { current_steering_angle = - (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_; + ( last_servo_cmd_->data - steering_to_servo_offset_ ) / steering_to_servo_gain_; current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_; } @@ -131,14 +100,14 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& odom->pose.pose.position.y = y_; odom->pose.pose.orientation.x = 0.0; odom->pose.pose.orientation.y = 0.0; - odom->pose.pose.orientation.z = sin(yaw_ / 2.0); - odom->pose.pose.orientation.w = cos(yaw_ / 2.0); + odom->pose.pose.orientation.z = sin(yaw_/2.0); + odom->pose.pose.orientation.w = cos(yaw_/2.0); // Position uncertainty /** @todo Think about position uncertainty, perhaps get from parameters? */ - odom->pose.covariance[0] = 0.2; ///< x - odom->pose.covariance[7] = 0.2; ///< y - odom->pose.covariance[35] = 0.4; ///< yaw + odom->pose.covariance[0] = 0.2; ///< x + odom->pose.covariance[7] = 0.2; ///< y + odom->pose.covariance[35] = 0.4; ///< yaw // Velocity ("in the coordinate frame given by the child_frame_id") odom->twist.twist.linear.x = current_speed; @@ -148,8 +117,7 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& // Velocity uncertainty /** @todo Think about velocity uncertainty */ - if (publish_tf_) - { + if (publish_tf_) { geometry_msgs::TransformStamped tf; tf.header.frame_id = odom_frame_; tf.child_frame_id = base_frame_; @@ -158,14 +126,12 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& tf.transform.translation.y = y_; tf.transform.translation.z = 0.0; tf.transform.rotation = odom->pose.pose.orientation; - if (ros::ok()) - { + if (ros::ok()) { tf_pub_->sendTransform(tf); } } - if (ros::ok()) - { + if (ros::ok()) { odom_pub_.publish(odom); } } @@ -176,13 +142,13 @@ void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo) } template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value) { - if (nh.getParam(name, *value)) + if (nh.getParam(name, value)) return true; ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str()); return false; } -} // namespace vesc_ackermann +} // namespace vesc_ackermann diff --git a/vesc_ackermann/src/vesc_to_odom_node.cpp b/vesc_ackermann/src/vesc_to_odom_node.cpp index caea389..9ce992f 100644 --- a/vesc_ackermann/src/vesc_to_odom_node.cpp +++ b/vesc_ackermann/src/vesc_to_odom_node.cpp @@ -1,28 +1,3 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - #include #include "vesc_ackermann/vesc_to_odom.h" diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp index 5f21c85..3fc37e4 100644 --- a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp +++ b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp @@ -1,34 +1,8 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - +#include #include #include #include -#include - #include "vesc_ackermann/vesc_to_odom.h" namespace vesc_ackermann @@ -37,13 +11,16 @@ namespace vesc_ackermann class VescToOdomNodelet: public nodelet::Nodelet { public: + VescToOdomNodelet() {} private: + virtual void onInit(void); - std::shared_ptr vesc_to_odom_; -}; // class VescToOdomNodelet + boost::shared_ptr vesc_to_odom_; + +}; // class VescToOdomNodelet void VescToOdomNodelet::onInit() { @@ -51,6 +28,6 @@ void VescToOdomNodelet::onInit() vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); } -} // namespace vesc_ackermann +} // namespace vesc_ackermann PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); diff --git a/vesc_driver/CHANGELOG.rst b/vesc_driver/CHANGELOG.rst deleted file mode 100644 index 1b48d3e..0000000 --- a/vesc_driver/CHANGELOG.rst +++ /dev/null @@ -1,27 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package vesc_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.0 (2020-12-12) ------------------- -* Merge pull request `#1 `_ from f1tenth/melodic-devel - Updating for Melodic -* Exclude crc.h from roslint. -* Replacing boost::crc with CRCPP. -* Replacing boost::begin, boost::end, and boost::distance with Standard Library equivalents. -* Replacing boost::bind with Standard Library equivalent. -* Replaing boost::noncopyable with C++ equivalent. -* Replacing boost::function with Standard Library version. -* Replacing Boost smart pointers with Standard Library equivalents. -* Removing unnecessary v8stdint.h. -* Updating package.xml to format 3 and setting C++ standard to 11. -* Contributors: Joshua Whitley - -1.0.0 (2020-12-02) ------------------- -* Applying roslint and replacing registration macro with templated class. -* Adding roslint. -* Adding licenses. -* Updating maintainers, authors, and URLs. -* added onboard car -* Contributors: Joshua Whitley, billyzheng diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 50221d3..ece8de1 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,34 +1,18 @@ cmake_minimum_required(VERSION 2.8.3) project(vesc_driver) -# Set minimum C++ standard to C++11 -if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_STANDARD 11) -endif() - -find_package(Boost REQUIRED) - find_package(catkin REQUIRED COMPONENTS nodelet pluginlib roscpp - roslint - serial std_msgs vesc_msgs + serial ) catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS - nodelet - pluginlib - std_msgs - vesc_msgs serial + CATKIN_DEPENDS nodelet pluginlib roscpp std_msgs vesc_msgs serial ) ########### @@ -63,15 +47,6 @@ target_link_libraries(vesc_driver_nodelet ${catkin_LIBRARIES} ) -file(GLOB_RECURSE ${PROJECT_NAME}_CPP_FILES - RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} - . *.cpp *.h) -list(REMOVE_ITEM ${PROJECT_NAME}_CPP_FILES - include/${PROJECT_NAME}/crc.h) - -set(ROSLINT_CPP_OPTS "--filter=-build/c++11") -roslint_cpp(${${PROJECT_NAME}_CPP_FILES}) - ############# ## Install ## ############# @@ -95,6 +70,4 @@ install(DIRECTORY launch/ ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - roslint_add_test() -endif() +# TODO \ No newline at end of file diff --git a/vesc_driver/include/vesc_driver/crc.h b/vesc_driver/include/vesc_driver/crc.h deleted file mode 100644 index 68e322d..0000000 --- a/vesc_driver/include/vesc_driver/crc.h +++ /dev/null @@ -1,1705 +0,0 @@ -/** - @file CRC.h - @author Daniel Bahr - @version 1.0.1.0 - @copyright - @parblock - CRC++ - Copyright (c) 2020, Daniel Bahr - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of CRC++ nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL 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. - @endparblock -*/ - -/* - CRC++ can be configured by setting various #defines before #including this header file: - - #define crcpp_uint8 - Specifies the type used to store CRCs that have a width of 8 bits or less. - This type is not used in CRC calculations. Defaults to ::std::uint8_t. - #define crcpp_uint16 - Specifies the type used to store CRCs that have a width between 9 and 16 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint16_t. - #define crcpp_uint32 - Specifies the type used to store CRCs that have a width between 17 and 32 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint32_t. - #define crcpp_uint64 - Specifies the type used to store CRCs that have a width between 33 and 64 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint64_t. - #define crcpp_size - This type is used for loop iteration and function signatures only. Defaults to ::std::size_t. - #define CRCPP_USE_NAMESPACE - Define to place all CRC++ code within the ::CRCPP namespace. - #define CRCPP_BRANCHLESS - Define to enable a branchless CRC implementation. The branchless implementation uses a single integer - multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation - may be faster on processor architectures which support single-instruction integer multiplication. - #define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). - #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. -*/ - -#ifndef VESC_DRIVER_CRC_H_ -#define VESC_DRIVER_CRC_H_ - -#include // Includes CHAR_BIT -#ifdef CRCPP_USE_CPP11 -#include // Includes ::std::size_t -#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t -#else -#include // Includes size_t -#include // Includes uint8_t, uint16_t, uint32_t, uint64_t -#endif -#include // Includes ::std::numeric_limits -#include // Includes ::std::move - -#ifndef crcpp_uint8 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint8 ::std::uint8_t -# else - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint8 uint8_t -# endif -#endif - -#ifndef crcpp_uint16 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint16 ::std::uint16_t -# else - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint16 uint16_t -# endif -#endif - -#ifndef crcpp_uint32 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint32 ::std::uint32_t -# else - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint32 uint32_t -# endif -#endif - -#ifndef crcpp_uint64 -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint64 ::std::uint64_t -# else - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. -# define crcpp_uint64 uint64_t -# endif -#endif - -#ifndef crcpp_size -# ifdef CRCPP_USE_CPP11 - /// @brief Unsigned size definition, used for specifying data sizes. -# define crcpp_size ::std::size_t -# else - /// @brief Unsigned size definition, used for specifying data sizes. -# define crcpp_size size_t -# endif -#endif - -#ifdef CRCPP_USE_CPP11 - /// @brief Compile-time expression definition. -# define crcpp_constexpr constexpr -#else - /// @brief Compile-time expression definition. -# define crcpp_constexpr const -#endif - -#ifdef CRCPP_USE_NAMESPACE -namespace CRCPP -{ -#endif - -/** - @brief Static class for computing CRCs. - @note This class supports computation of full and multi-part CRCs, using a bit-by-bit algorithm or a - byte-by-byte lookup table. The CRCs are calculated using as many optimizations as is reasonable. - If compiling with C++11, the constexpr keyword is used liberally so that many calculations are - performed at compile-time instead of at runtime. -*/ -class CRC -{ -public: - // Forward declaration - template - struct Table; - - /** - @brief CRC parameters. - */ - template - struct Parameters - { - CRCType polynomial; ///< CRC polynomial - CRCType initialValue; ///< Initial CRC value - CRCType finalXOR; ///< Value to XOR with the final CRC - bool reflectInput; ///< true to reflect all input bytes - bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) - - Table MakeTable() const; - }; - - /** - @brief CRC lookup table. After construction, the CRC parameters are fixed. - @note A CRC table can be used for multiple CRC calculations. - */ - template - struct Table - { - // Constructors are intentionally NOT marked explicit. - Table(const Parameters & parameters); - -#ifdef CRCPP_USE_CPP11 - Table(Parameters && parameters); -#endif - - const Parameters & GetParameters() const; - - const CRCType * GetTable() const; - - CRCType operator[](unsigned char index) const; - - private: - void InitTable(); - - Parameters parameters; ///< CRC parameters used to construct the table - CRCType table[1 << CHAR_BIT]; ///< CRC lookup table - }; - - // The number of bits in CRCType must be at least as large as CRCWidth. - // CRCType must be an unsigned integer type or a custom type with operator overloads. - template - static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); - - // Common CRCs up to 64 bits. - // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); - static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); - static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); - static const Parameters< crcpp_uint8, 7> & CRC_7(); -#endif - static const Parameters< crcpp_uint8, 8> & CRC_8(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); - static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); - static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); - static const Parameters & CRC_10(); - static const Parameters & CRC_10_CDMA2000(); - static const Parameters & CRC_11(); - static const Parameters & CRC_12_CDMA2000(); - static const Parameters & CRC_12_DECT(); - static const Parameters & CRC_12_UMTS(); - static const Parameters & CRC_13_BBC(); - static const Parameters & CRC_15(); - static const Parameters & CRC_15_MPT1327(); -#endif - static const Parameters & CRC_16_ARC(); - static const Parameters & CRC_16_BUYPASS(); - static const Parameters & CRC_16_CCITTFALSE(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_CDMA2000(); - static const Parameters & CRC_16_CMS(); - static const Parameters & CRC_16_DECTR(); - static const Parameters & CRC_16_DECTX(); - static const Parameters & CRC_16_DNP(); -#endif - static const Parameters & CRC_16_GENIBUS(); - static const Parameters & CRC_16_KERMIT(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_MAXIM(); - static const Parameters & CRC_16_MODBUS(); - static const Parameters & CRC_16_T10DIF(); - static const Parameters & CRC_16_USB(); -#endif - static const Parameters & CRC_16_X25(); - static const Parameters & CRC_16_XMODEM(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_17_CAN(); - static const Parameters & CRC_21_CAN(); - static const Parameters & CRC_24(); - static const Parameters & CRC_24_FLEXRAYA(); - static const Parameters & CRC_24_FLEXRAYB(); - static const Parameters & CRC_30(); -#endif - static const Parameters & CRC_32(); - static const Parameters & CRC_32_BZIP2(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_C(); -#endif - static const Parameters & CRC_32_MPEG2(); - static const Parameters & CRC_32_POSIX(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_Q(); - static const Parameters & CRC_40_GSM(); - static const Parameters & CRC_64(); -#endif - -#ifdef CRCPP_USE_CPP11 - CRC() = delete; - CRC(const CRC & other) = delete; - CRC & operator=(const CRC & other) = delete; - CRC(CRC && other) = delete; - CRC & operator=(CRC && other) = delete; -#endif - -private: -#ifndef CRCPP_USE_CPP11 - CRC(); - CRC(const CRC & other); - CRC & operator=(const CRC & other); -#endif - - template - static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); - - template - static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - - template - static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); - - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); -}; - -/** - @brief Returns a CRC lookup table construct using these CRC parameters. - @note This function primarily exists to allow use of the auto keyword instead of instantiating - a table directly, since template parameters are not inferred in constructors. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC lookup table -*/ -template -inline CRC::Table CRC::Parameters::MakeTable() const -{ - // This should take advantage of RVO and optimize out the copy. - return CRC::Table(*this); -} - -/** - @brief Constructs a CRC table from a set of CRC parameters - @param[in] params CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -template -inline CRC::Table::Table(const Parameters & params) : - parameters(params) -{ - InitTable(); -} - -#ifdef CRCPP_USE_CPP11 -/** - @brief Constructs a CRC table from a set of CRC parameters - @param[in] params CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -template -inline CRC::Table::Table(Parameters && params) : - parameters(::std::move(params)) -{ - InitTable(); -} -#endif - -/** - @brief Gets the CRC parameters used to construct the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC parameters -*/ -template -inline const CRC::Parameters & CRC::Table::GetParameters() const -{ - return parameters; -} - -/** - @brief Gets the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC table -*/ -template -inline const CRCType * CRC::Table::GetTable() const -{ - return table; -} - -/** - @brief Gets an entry in the CRC table - @param[in] index Index into the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC table entry -*/ -template -inline CRCType CRC::Table::operator[](unsigned char index) const -{ - return table[index]; -} - -/** - @brief Initializes a CRC table. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -template -inline void CRC::Table::InitTable() -{ - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); - - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - CRCType crc; - unsigned char byte = 0; - - // Loop over each dividend (each possible number storable in an unsigned char) - do - { - crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); - - // This mask might not be necessary; all unit tests pass with this line commented out, - // but that might just be a coincidence based on the CRC parameters used for testing. - // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. - crc &= BIT_MASK; - - if (!parameters.reflectInput && CRCWidth < CHAR_BIT) - { - // Undo the special operation at the end of the CalculateRemainder() - // function for non-reflected CRCs < CHAR_BIT. - crc = static_cast(crc << SHIFT); - } - - table[byte] = crc; - } - while (++byte); -} - -/** - @brief Computes a CRC. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] parameters CRC parameters - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) -{ - CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} -/** - @brief Appends additional data to a previous CRC calculation. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] parameters CRC parameters - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) -{ - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - remainder = CalculateRemainder(data, size, parameters, remainder); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Computes a CRC via a lookup table. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] lookupTable CRC lookup table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Appends additional data to a previous CRC calculation using a lookup table. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] lookupTable CRC lookup table - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - remainder = CalculateRemainder(data, size, lookupTable, remainder); - - // No need to mask the remainder here; the mask will be applied in the Finalize() function. - - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); -} - -/** - @brief Reflects (i.e. reverses the bits within) an integer value. - @param[in] value Value to reflect - @param[in] numBits Number of bits in the integer which will be reflected - @tparam IntegerType Integer type of the value being reflected - @return Reflected value -*/ -template -inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) -{ - IntegerType reversedValue(0); - - for (crcpp_uint16 i = 0; i < numBits; ++i) - { - reversedValue = static_cast((reversedValue << 1) | (value & 1)); - value = static_cast(value >> 1); - } - - return reversedValue; -} - -/** - @brief Computes the final reflection and XOR of a CRC remainder. - @param[in] remainder CRC remainder to reflect and XOR - @param[in] finalXOR Final value to XOR with the remainder - @param[in] reflectOutput true to reflect each byte of the remainder before the XOR - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return Final CRC -*/ -template -inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) -{ - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - - if (reflectOutput) - { - remainder = Reflect(remainder, CRCWidth); - } - - return (remainder ^ finalXOR) & BIT_MASK; -} - -/** - @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. - @note This function allows for computation of multi-part CRCs - @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: - - CRCType x = ...; - CRCType y = Finalize(x, finalXOR, reflectOutput); - CRCType z = UndoFinalize(y, finalXOR, reflectOutput); - assert(x == z); - - @param[in] crc Reflected and XORed CRC - @param[in] finalXOR Final value XORed with the remainder - @param[in] reflectOutput true if the remainder is to be reflected - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return Un-finalized CRC remainder -*/ -template -inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) -{ - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - - crc = (crc & BIT_MASK) ^ finalXOR; - - if (reflectOutput) - { - crc = Reflect(crc, CRCWidth); - } - - return crc; -} - -/** - @brief Computes a CRC remainder. - @param[in] data Data over which the remainder will be computed - @param[in] size Size of the data - @param[in] parameters CRC parameters - @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC remainder -*/ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) -{ -#ifdef CRCPP_USE_CPP11 - // This static_assert is put here because this function will always be compiled in no matter what - // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. - static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); -#else - // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's - // better than nothing. - enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; -#endif - - const unsigned char * current = reinterpret_cast(data); - - // Slightly different implementations based on the parameters. The current implementations try to eliminate as much - // computation from the inner loop (looping over each bit) as possible. - if (parameters.reflectInput) - { - CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); - while (size--) - { - remainder = static_cast(remainder ^ *current++); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & 1) - // remainder = (remainder >> 1) ^ polynomial; - // else - // remainder >>= 1; - remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); -#else - remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); -#endif - } - } - } - else if (CRCWidth >= CHAR_BIT) - { - static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); -#ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); -#endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - while (size--) - { - remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CRC_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ parameters.polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); -#else - remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); -#endif - } - } - } - else - { - static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); -#ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); -#endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - CRCType polynomial = static_cast(parameters.polynomial << SHIFT); - remainder = static_cast(remainder << SHIFT); - - while (size--) - { - remainder = static_cast(remainder ^ *current++); - - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { -#ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); -#else - remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); -#endif - } - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -/** - @brief Computes a CRC remainder using lookup table. - @param[in] data Data over which the remainder will be computed - @param[in] size Size of the data - @param[in] lookupTable CRC lookup table - @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC remainder -*/ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) -{ - const unsigned char * current = reinterpret_cast(data); - - if (lookupTable.GetParameters().reflectInput) - { - while (size--) - { -#if defined(WIN32) || defined(_WIN32) || defined(WINCE) - // Disable warning about data loss when doing (remainder >> CHAR_BIT) when - // remainder is one byte long. The algorithm is still correct in this case, - // though it's possible that one additional machine instruction will be executed. -# pragma warning (push) -# pragma warning (disable : 4333) -#endif - remainder = static_cast((remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); -#if defined(WIN32) || defined(_WIN32) || defined(WINCE) -# pragma warning (pop) -#endif - } - } - else if (CRCWidth >= CHAR_BIT) - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - while (size--) - { - remainder = static_cast((remainder << CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); - } - } - else - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - - remainder = static_cast(remainder << SHIFT); - - while (size--) - { - // Note: no need to mask here since remainder is guaranteed to fit in a single byte. - remainder = lookupTable[static_cast(remainder ^ *current++)]; - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-4 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-4 ITU has the following parameters and check value: - - polynomial = 0x3 - - initial value = 0x0 - - final XOR = 0x0 - - reflect input = true - - reflect output = true - - check value = 0x7 - @return CRC-4 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_4_ITU() -{ - static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 EPC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 EPC has the following parameters and check value: - - polynomial = 0x09 - - initial value = 0x09 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x00 - @return CRC-5 EPC parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_EPC() -{ - static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 ITU has the following parameters and check value: - - polynomial = 0x15 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x07 - @return CRC-5 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_ITU() -{ - static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 USB. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 USB has the following parameters and check value: - - polynomial = 0x05 - - initial value = 0x1F - - final XOR = 0x1F - - reflect input = true - - reflect output = true - - check value = 0x19 - @return CRC-5 USB parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_USB() -{ - static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 CDMA2000-A. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 CDMA2000-A has the following parameters and check value: - - polynomial = 0x27 - - initial value = 0x3F - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x0D - @return CRC-6 CDMA2000-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() -{ - static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 CDMA2000-B. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 CDMA2000-A has the following parameters and check value: - - polynomial = 0x07 - - initial value = 0x3F - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x3B - @return CRC-6 CDMA2000-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() -{ - static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 ITU has the following parameters and check value: - - polynomial = 0x03 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x06 - @return CRC-6 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_ITU() -{ - static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-7 JEDEC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-7 JEDEC has the following parameters and check value: - - polynomial = 0x09 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x75 - @return CRC-7 JEDEC parameters -*/ -inline const CRC::Parameters & CRC::CRC_7() -{ - static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-8 SMBus. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 SMBus has the following parameters and check value: - - polynomial = 0x07 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0xF4 - @return CRC-8 SMBus parameters -*/ -inline const CRC::Parameters & CRC::CRC_8() -{ - static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-8 EBU (aka CRC-8 AES). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 EBU has the following parameters and check value: - - polynomial = 0x1D - - initial value = 0xFF - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x97 - @return CRC-8 EBU parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_EBU() -{ - static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 MAXIM (aka CRC-8 DOW-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 MAXIM has the following parameters and check value: - - polynomial = 0x31 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0xA1 - @return CRC-8 MAXIM parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_MAXIM() -{ - static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 WCDMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 WCDMA has the following parameters and check value: - - polynomial = 0x9B - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x25 - @return CRC-8 WCDMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_WCDMA() -{ - static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-10 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-10 ITU has the following parameters and check value: - - polynomial = 0x233 - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x199 - @return CRC-10 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_10() -{ - static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-10 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-10 CDMA2000 has the following parameters and check value: - - polynomial = 0x3D9 - - initial value = 0x3FF - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x233 - @return CRC-10 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_10_CDMA2000() -{ - static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-11 FlexRay. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-11 FlexRay has the following parameters and check value: - - polynomial = 0x385 - - initial value = 0x01A - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x5A3 - @return CRC-11 FlexRay parameters -*/ -inline const CRC::Parameters & CRC::CRC_11() -{ - static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 CDMA2000 has the following parameters and check value: - - polynomial = 0xF13 - - initial value = 0xFFF - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0xD4D - @return CRC-12 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_CDMA2000() -{ - static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 DECT (aka CRC-12 X-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 DECT has the following parameters and check value: - - polynomial = 0x80F - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0xF5B - @return CRC-12 DECT parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_DECT() -{ - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 UMTS (aka CRC-12 3GPP). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 UMTS has the following parameters and check value: - - polynomial = 0x80F - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = true - - check value = 0xDAF - @return CRC-12 UMTS parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_UMTS() -{ - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-13 BBC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-13 BBC has the following parameters and check value: - - polynomial = 0x1CF5 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x04FA - @return CRC-13 BBC parameters -*/ -inline const CRC::Parameters & CRC::CRC_13_BBC() -{ - static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-15 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-15 CAN has the following parameters and check value: - - polynomial = 0x4599 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x059E - @return CRC-15 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_15() -{ - static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-15 MPT1327. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-15 MPT1327 has the following parameters and check value: - - polynomial = 0x6815 - - initial value = 0x0000 - - final XOR = 0x0001 - - reflect input = false - - reflect output = false - - check value = 0x2566 - @return CRC-15 MPT1327 parameters -*/ -inline const CRC::Parameters & CRC::CRC_15_MPT1327() -{ - static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 ARC has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0xBB3D - @return CRC-16 ARC parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_ARC() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 BUYPASS (aka CRC-16 VERIFONE, CRC-16 UMTS). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 BUYPASS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xFEE8 - @return CRC-16 BUYPASS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_BUYPASS() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 CCITT FALSE. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CCITT FALSE has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x29B1 - @return CRC-16 CCITT FALSE parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-16 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CDMA2000 has the following parameters and check value: - - polynomial = 0xC867 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x4C06 - @return CRC-16 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CDMA2000() -{ - static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 CMS. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CMS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xAEE7 - @return CRC-16 CMS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CMS() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DECT-R (aka CRC-16 R-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DECT-R has the following parameters and check value: - - polynomial = 0x0589 - - initial value = 0x0000 - - final XOR = 0x0001 - - reflect input = false - - reflect output = false - - check value = 0x007E - @return CRC-16 DECT-R parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DECTR() -{ - static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DECT-X (aka CRC-16 X-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DECT-X has the following parameters and check value: - - polynomial = 0x0589 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x007F - @return CRC-16 DECT-X parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DECTX() -{ - static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DNP. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DNP has the following parameters and check value: - - polynomial = 0x3D65 - - initial value = 0x0000 - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0xEA82 - @return CRC-16 DNP parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DNP() -{ - static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 GENIBUS (aka CRC-16 EPC, CRC-16 I-CODE, CRC-16 DARC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 GENIBUS has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = false - - reflect output = false - - check value = 0xD64E - @return CRC-16 GENIBUS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_GENIBUS() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 KERMIT (aka CRC-16 CCITT, CRC-16 CCITT-TRUE). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 KERMIT has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0x2189 - @return CRC-16 KERMIT parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_KERMIT() -{ - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-16 MAXIM. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 MAXIM has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0x44C2 - @return CRC-16 MAXIM parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_MAXIM() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 MODBUS. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 MODBUS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0x4B37 - @return CRC-16 MODBUS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_MODBUS() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 T10-DIF. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 T10-DIF has the following parameters and check value: - - polynomial = 0x8BB7 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xD0DB - @return CRC-16 T10-DIF parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_T10DIF() -{ - static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 USB. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 USB has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0xB4C8 - @return CRC-16 USB parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_USB() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; - return parameters; -} - -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 X-25 (aka CRC-16 IBM-SDLC, CRC-16 ISO-HDLC, CRC-16 B). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 X-25 has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0x906E - @return CRC-16 X-25 parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_X25() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 XMODEM (aka CRC-16 ZMODEM, CRC-16 ACORN, CRC-16 LTE). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 XMODEM has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x31C3 - @return CRC-16 XMODEM parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_XMODEM() -{ - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-17 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-17 CAN has the following parameters and check value: - - polynomial = 0x1685B - - initial value = 0x00000 - - final XOR = 0x00000 - - reflect input = false - - reflect output = false - - check value = 0x04F03 - @return CRC-17 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_17_CAN() -{ - static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-21 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-21 CAN has the following parameters and check value: - - polynomial = 0x102899 - - initial value = 0x000000 - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x0ED841 - @return CRC-21 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_21_CAN() -{ - static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 OPENPGP. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 OPENPGP has the following parameters and check value: - - polynomial = 0x864CFB - - initial value = 0xB704CE - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x21CF02 - @return CRC-24 OPENPGP parameters -*/ -inline const CRC::Parameters & CRC::CRC_24() -{ - static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 FlexRay-A. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 FlexRay-A has the following parameters and check value: - - polynomial = 0x5D6DCB - - initial value = 0xFEDCBA - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x7979BD - @return CRC-24 FlexRay-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() -{ - static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 FlexRay-B. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 FlexRay-B has the following parameters and check value: - - polynomial = 0x5D6DCB - - initial value = 0xABCDEF - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x1F23B8 - @return CRC-24 FlexRay-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() -{ - static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-30 CDMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-30 CDMA has the following parameters and check value: - - polynomial = 0x2030B9C7 - - initial value = 0x3FFFFFFF - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x3B3CB540 - @return CRC-30 CDMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_30() -{ - static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = true - - reflect output = true - - check value = 0xCBF43926 - @return CRC-32 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-32 BZIP2 (aka CRC-32 AAL5, CRC-32 DECT-B, CRC-32 B-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 BZIP2 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0xFC891918 - @return CRC-32 BZIP2 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_BZIP2() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-32 C (aka CRC-32 ISCSI, CRC-32 Castagnoli, CRC-32 Interlaken). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 C has the following parameters and check value: - - polynomial = 0x1EDC6F41 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = true - - reflect output = true - - check value = 0xE3069283 - @return CRC-32 C parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_C() -{ - static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; -} -#endif - -/** - @brief Returns a set of parameters for CRC-32 MPEG-2. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 MPEG-2 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x0376E6E7 - @return CRC-32 MPEG-2 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_MPEG2() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-32 POSIX. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 POSIX has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0x00000000 - - final XOR = 0xFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0x765E7680 - @return CRC-32 POSIX parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_POSIX() -{ - static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-32 Q. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 Q has the following parameters and check value: - - polynomial = 0x814141AB - - initial value = 0x00000000 - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x3010BF7F - @return CRC-32 Q parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_Q() -{ - static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-40 GSM. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-40 GSM has the following parameters and check value: - - polynomial = 0x0004820009 - - initial value = 0x0000000000 - - final XOR = 0xFFFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0xD4164FC646 - @return CRC-40 GSM parameters -*/ -inline const CRC::Parameters & CRC::CRC_40_GSM() -{ - static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-64 ECMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-64 ECMA has the following parameters and check value: - - polynomial = 0x42F0E1EBA9EA3693 - - initial value = 0x0000000000000000 - - final XOR = 0x0000000000000000 - - reflect input = false - - reflect output = false - - check value = 0x6C40DF5F0B497347 - @return CRC-64 ECMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_64() -{ - static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -#ifdef CRCPP_USE_NAMESPACE -} -#endif - -#endif // CRCPP_CRC_H_ diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.h index 1497a20..994920d 100644 --- a/vesc_driver/include/vesc_driver/datatypes.h +++ b/vesc_driver/include/vesc_driver/datatypes.h @@ -1,30 +1,5 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - /* - Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se + Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se This file is part of VESC Tool. @@ -42,102 +17,98 @@ along with this program. If not, see . */ -#ifndef VESC_DRIVER_DATATYPES_H -#define VESC_DRIVER_DATATYPES_H +#ifndef DATATYPES_H +#define DATATYPES_H -#include +//#include +//#include +//#include +//#include +#include -typedef struct -{ - bool isVesc; -} -VSerialInfo_t; +typedef struct { + bool isVesc; +} VSerialInfo_t; -typedef enum -{ - CFG_T_UNDEFINED = 0, - CFG_T_DOUBLE, - CFG_T_INT, - CFG_T_QSTRING, - CFG_T_ENUM, - CFG_T_BOOL -} -CFG_T; +typedef enum { + CFG_T_UNDEFINED = 0, + CFG_T_DOUBLE, + CFG_T_INT, + CFG_T_QSTRING, + CFG_T_ENUM, + CFG_T_BOOL +} CFG_T; -typedef enum -{ - VESC_TX_UNDEFINED = 0, - VESC_TX_UINT8, - VESC_TX_INT8, - VESC_TX_UINT16, - VESC_TX_INT16, - VESC_TX_UINT32, - VESC_TX_INT32, - VESC_TX_DOUBLE16, - VESC_TX_DOUBLE32, - VESC_TX_DOUBLE32_AUTO -} -VESC_TX_T; +typedef enum { + VESC_TX_UNDEFINED = 0, + VESC_TX_UINT8, + VESC_TX_INT8, + VESC_TX_UINT16, + VESC_TX_INT16, + VESC_TX_UINT32, + VESC_TX_INT32, + VESC_TX_DOUBLE16, + VESC_TX_DOUBLE32, + VESC_TX_DOUBLE32_AUTO +} VESC_TX_T; -typedef enum -{ - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR -} -mc_fault_code; +typedef enum { + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR +} mc_fault_code; + +typedef enum { + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR +} disp_pos_mode; + +struct MC_VALUES { -typedef enum -{ - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR -} -disp_pos_mode; - -struct MC_VALUES -{ public: - double v_in; - double temp_mos; - double temp_motor; - double current_motor; - double current_in; - double id; - double iq; - double rpm; - double duty_now; - double amp_hours; - double amp_hours_charged; - double watt_hours; - double watt_hours_charged; - int tachometer; - int tachometer_abs; - double position; - mc_fault_code fault_code; + double v_in; + double temp_mos; + double temp_motor; + double current_motor; + double current_in; + double id; + double iq; + double rpm; + double duty_now; + double amp_hours; + double amp_hours_charged; + double watt_hours; + double watt_hours_charged; + int tachometer; + int tachometer_abs; + double position; + mc_fault_code fault_code; }; -typedef enum -{ - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES -} -debug_sampling_mode; + +typedef enum { + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES +} debug_sampling_mode; + + +// Communication commands typedef enum { COMM_FW_VERSION = 0, @@ -177,36 +148,130 @@ typedef enum COMM_FORWARD_CAN, COMM_SET_CHUCK_DATA, COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING -} -COMM_PACKET_ID; + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT, + COMM_TERMINAL_CMD_SYNC, + COMM_GET_IMU_DATA, + COMM_BM_CONNECT, + COMM_BM_ERASE_FLASH_ALL, + COMM_BM_WRITE_FLASH, + COMM_BM_REBOOT, + COMM_BM_DISCONNECT, + COMM_BM_MAP_PINS_DEFAULT, + COMM_BM_MAP_PINS_NRF5X, + COMM_ERASE_BOOTLOADER, + COMM_ERASE_BOOTLOADER_ALL_CAN, + COMM_PLOT_INIT, + COMM_PLOT_DATA, + COMM_PLOT_ADD_GRAPH, + COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, + COMM_BM_MEM_READ, + COMM_WRITE_NEW_APP_DATA_LZO, + COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, + COMM_BM_WRITE_FLASH_LZO, + COMM_SET_CURRENT_REL, + COMM_CAN_FWD_FRAME, + COMM_SET_BATTERY_CUT, + COMM_SET_BLE_NAME, + COMM_SET_BLE_PIN, + COMM_SET_CAN_MODE, + COMM_GET_IMU_CALIBRATION +} COMM_PACKET_ID; + + +// typedef enum { +// COMM_FW_VERSION = 0, +// COMM_JUMP_TO_BOOTLOADER, +// COMM_ERASE_NEW_APP, +// COMM_WRITE_NEW_APP_DATA, +// COMM_GET_VALUES, +// COMM_SET_DUTY, +// COMM_SET_CURRENT, +// COMM_SET_CURRENT_BRAKE, +// COMM_SET_RPM, +// COMM_SET_POS, +// COMM_SET_HANDBRAKE, +// COMM_SET_DETECT, +// COMM_SET_SERVO_POS, +// COMM_SET_MCCONF, +// COMM_GET_MCCONF, +// COMM_GET_MCCONF_DEFAULT, +// COMM_SET_APPCONF, +// COMM_GET_APPCONF, +// COMM_GET_APPCONF_DEFAULT, +// COMM_SAMPLE_PRINT, +// COMM_TERMINAL_CMD, +// COMM_PRINT, +// COMM_ROTOR_POSITION, +// COMM_EXPERIMENT_SAMPLE, +// COMM_DETECT_MOTOR_PARAM, +// COMM_DETECT_MOTOR_R_L, +// COMM_DETECT_MOTOR_FLUX_LINKAGE, +// COMM_DETECT_ENCODER, +// COMM_DETECT_HALL_FOC, +// COMM_REBOOT, +// COMM_ALIVE, +// COMM_GET_DECODED_PPM, +// COMM_GET_DECODED_ADC, +// COMM_GET_DECODED_CHUK, +// COMM_FORWARD_CAN, +// COMM_SET_CHUCK_DATA, +// COMM_CUSTOM_APP_DATA, +// COMM_NRF_START_PAIRING, +// COMM_GET_IMU_DATA +// } COMM_PACKET_ID; + +typedef struct { + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; +} chuck_data; + +struct bldc_detect { + -typedef struct -{ - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; -} -chuck_data; - -struct bldc_detect -{ public: - double cycle_int_limit; - double bemf_coupling_k; - int hall_res; + double cycle_int_limit; + double bemf_coupling_k; + int hall_res; }; -typedef enum -{ - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL -} -NRF_PAIR_RES; -#endif // VESC_DRIVER_DATATYPES_H + +typedef enum { + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL +} NRF_PAIR_RES; + +#endif // DATATYPES_H diff --git a/vesc_driver/include/vesc_driver/v8stdint.h b/vesc_driver/include/vesc_driver/v8stdint.h new file mode 100644 index 0000000..f3be96b --- /dev/null +++ b/vesc_driver/include/vesc_driver/v8stdint.h @@ -0,0 +1,57 @@ +// This header is from the v8 google project: +// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h + +// Copyright 2012 the V8 project authors. All rights reserved. +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of Google Inc. nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER 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. + +// Load definitions of standard types. + +#ifndef V8STDINT_H_ +#define V8STDINT_H_ + +#include +#include + +#if defined(_WIN32) && !defined(__MINGW32__) + +typedef signed char int8_t; +typedef unsigned char uint8_t; +typedef short int16_t; // NOLINT +typedef unsigned short uint16_t; // NOLINT +typedef int int32_t; +typedef unsigned int uint32_t; +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; +// intptr_t and friends are defined in crtdefs.h through stdio.h. + +#else + +#include + +#endif + +#endif // V8STDINT_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h index bdc5ffd..8ab1931 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ b/vesc_driver/include/vesc_driver/vesc_driver.h @@ -1,34 +1,8 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_DRIVER_H_ #define VESC_DRIVER_VESC_DRIVER_H_ -#include #include #include @@ -44,13 +18,14 @@ namespace vesc_driver class VescDriver { public: + VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); private: // interface to the VESC VescInterface vesc_; - void vescPacketCallback(const std::shared_ptr& packet); + void vescPacketCallback(const boost::shared_ptr& packet); void vescErrorCallback(const std::string& error); // limits on VESC commands @@ -74,6 +49,9 @@ class VescDriver // ROS services ros::Publisher state_pub_; ros::Publisher servo_sensor_pub_; + ros::Publisher imu_pub_; + // ros::Publisher imu_std_pub_; + ros::Subscriber duty_cycle_sub_; ros::Subscriber current_sub_; ros::Subscriber brake_sub_; @@ -83,12 +61,10 @@ class VescDriver ros::Timer timer_; // driver modes (possible states) - typedef enum - { + typedef enum { MODE_INITIALIZING, MODE_OPERATING - } - driver_mode_t; + } driver_mode_t; // other variables driver_mode_t driver_mode_; ///< driver state machine mode (state) @@ -105,6 +81,6 @@ class VescDriver void servoCallback(const std_msgs::Float64::ConstPtr& servo); }; -} // namespace vesc_driver +} // namespace vesc_driver -#endif // VESC_DRIVER_VESC_DRIVER_H_ +#endif // VESC_DRIVER_VESC_DRIVER_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.h index 558ec2b..b2063e0 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.h @@ -1,39 +1,17 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_INTERFACE_H_ #define VESC_DRIVER_VESC_INTERFACE_H_ -#include -#include -#include +#include #include +#include #include -#include + +#include +#include +#include +#include #include "vesc_driver/vesc_packet.h" @@ -43,11 +21,12 @@ namespace vesc_driver /** * Class providing an interface to the Vedder VESC motor controller via a serial port interface. */ -class VescInterface +class VescInterface : private boost::noncopyable { public: - typedef std::function PacketHandlerFunction; - typedef std::function ErrorHandlerFunction; + + typedef boost::function PacketHandlerFunction; + typedef boost::function ErrorHandlerFunction; /** * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not @@ -64,12 +43,6 @@ class VescInterface const PacketHandlerFunction& packet_handler = PacketHandlerFunction(), const ErrorHandlerFunction& error_handler = ErrorHandlerFunction()); - /** - * Delete copy constructor and equals operator. - */ - VescInterface(const VescInterface &) = delete; - VescInterface & operator=(const VescInterface &) = delete; - /** * VescInterface destructor. */ @@ -119,10 +92,13 @@ class VescInterface void setPosition(double position); void setServo(double servo); + void requestImuData(); + + private: // Pimpl - hide serial port members from class users class Impl; - std::unique_ptr impl_; + boost::scoped_ptr impl_; }; // todo: review @@ -132,20 +108,18 @@ class SerialException : public std::exception SerialException& operator=(const SerialException&); std::string e_what_; public: - explicit SerialException(const char *description) - { - std::stringstream ss; - ss << "SerialException " << description << " failed."; - e_what_ = ss.str(); + SerialException (const char *description) { + std::stringstream ss; + ss << "SerialException " << description << " failed."; + e_what_ = ss.str(); } - SerialException(const SerialException& other) : e_what_(other.e_what_) {} + SerialException (const SerialException& other) : e_what_(other.e_what_) {} virtual ~SerialException() throw() {} - virtual const char* what() const throw() - { + virtual const char* what () const throw () { return e_what_.c_str(); } }; -} // namespace vesc_driver +} // namespace vesc_driver -#endif // VESC_DRIVER_VESC_INTERFACE_H_ +#endif // VESC_DRIVER_VESC_INTERFACE_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index 307e2f5..92d9207 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -1,41 +1,18 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_PACKET_H_ #define VESC_DRIVER_VESC_PACKET_H_ -#include -#include #include #include #include -#define CRCPP_USE_CPP11 -#include "vesc_driver/crc.h" +#include +#include + +// #include "vesc_driver/crc.hpp" + +#include "vesc_driver/v8stdint.h" namespace vesc_driver { @@ -51,28 +28,25 @@ class VescFrame virtual ~VescFrame() {} // getters - virtual const Buffer& frame() const - { - return *frame_; - } + virtual const Buffer& frame() const {return *frame_;} // VESC packet properties - static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes - static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes - static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes - static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value - static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value - static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value + static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes + static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes + static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes + static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value + static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value + static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value /** CRC parameters for the VESC */ - static constexpr CRC::Parameters CRC_TYPE = { 0x1021, 0x0000, 0x0000, false, false }; + typedef boost::crc_optimal<16, 0x1021, 0, 0, false, false> CRC; -protected: + protected: /** Construct frame with specified payload size. */ - explicit VescFrame(int payload_size); + VescFrame(int payload_size); - std::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy - BufferRange payload_; ///< View into frame's payload section + boost::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy + BufferRange payload_; ///< View into frame's payload section private: /** Construct from buffer. Used by VescPacketFactory factory. */ @@ -90,37 +64,36 @@ class VescPacket : public VescFrame public: virtual ~VescPacket() {} - virtual const std::string& name() const - { - return name_; - } + virtual const std::string& name() const {return name_;} protected: VescPacket(const std::string& name, int payload_size, int payload_id); - VescPacket(const std::string& name, std::shared_ptr raw); + VescPacket(const std::string& name, boost::shared_ptr raw); private: std::string name_; }; -typedef std::shared_ptr VescPacketPtr; -typedef std::shared_ptr VescPacketConstPtr; +typedef boost::shared_ptr VescPacketPtr; +typedef boost::shared_ptr VescPacketConstPtr; /*------------------------------------------------------------------------------------------------*/ class VescPacketFWVersion : public VescPacket { public: - explicit VescPacketFWVersion(std::shared_ptr raw); + VescPacketFWVersion(boost::shared_ptr raw); int fwMajor() const; int fwMinor() const; + }; class VescPacketRequestFWVersion : public VescPacket { public: VescPacketRequestFWVersion(); + }; /*------------------------------------------------------------------------------------------------*/ @@ -128,7 +101,7 @@ class VescPacketRequestFWVersion : public VescPacket class VescPacketValues : public VescPacket { public: - explicit VescPacketValues(std::shared_ptr raw); + VescPacketValues(boost::shared_ptr raw); double v_in() const; double temp_mos1() const; @@ -149,6 +122,7 @@ class VescPacketValues : public VescPacket double tachometer() const; double tachometer_abs() const; int fault_code() const; + }; class VescPacketRequestValues : public VescPacket @@ -162,7 +136,7 @@ class VescPacketRequestValues : public VescPacket class VescPacketSetDuty : public VescPacket { public: - explicit VescPacketSetDuty(double duty); + VescPacketSetDuty(double duty); // double duty() const; }; @@ -172,7 +146,7 @@ class VescPacketSetDuty : public VescPacket class VescPacketSetCurrent : public VescPacket { public: - explicit VescPacketSetCurrent(double current); + VescPacketSetCurrent(double current); // double current() const; }; @@ -182,7 +156,7 @@ class VescPacketSetCurrent : public VescPacket class VescPacketSetCurrentBrake : public VescPacket { public: - explicit VescPacketSetCurrentBrake(double current_brake); + VescPacketSetCurrentBrake(double current_brake); // double current_brake() const; }; @@ -192,7 +166,7 @@ class VescPacketSetCurrentBrake : public VescPacket class VescPacketSetRPM : public VescPacket { public: - explicit VescPacketSetRPM(double rpm); + VescPacketSetRPM(double rpm); // double rpm() const; }; @@ -202,7 +176,7 @@ class VescPacketSetRPM : public VescPacket class VescPacketSetPos : public VescPacket { public: - explicit VescPacketSetPos(double pos); + VescPacketSetPos(double pos); // double pos() const; }; @@ -212,11 +186,75 @@ class VescPacketSetPos : public VescPacket class VescPacketSetServoPos : public VescPacket { public: - explicit VescPacketSetServoPos(double servo_pos); + VescPacketSetServoPos(double servo_pos); // double servo_pos() const; }; -} // namespace vesc_driver +/*------------------------------------------------------------------------------------------------*/ +class VescPacketRequestImu : public VescPacket +{ +public: + VescPacketRequestImu(); +}; + +class VescPacketImu : public VescPacket +{ +public: + explicit VescPacketImu(boost::shared_ptr raw); + + int mask() const; + + double yaw() const; + double pitch() const; + double roll() const; + + double acc_x() const; + double acc_y() const; + double acc_z() const; + + double gyr_x() const; + double gyr_y() const; + double gyr_z() const; + + double mag_x() const; + double mag_y() const; + double mag_z() const; + + double q_w() const; + double q_x() const; + double q_y() const; + double q_z() const; + +private: + double getFloat32Auto(uint32_t * pos) const; + + uint32_t mask_; + double roll_; + double pitch_; + double yaw_; + + double acc_x_; + double acc_y_; + double acc_z_; + + double gyr_x_; + double gyr_y_; + double gyr_z_; + + double mag_x_; + double mag_y_; + double mag_z_; + + double q0_; + double q1_; + double q2_; + double q3_; +}; + +/*------------------------------------------------------------------------------------------------*/ + + +} // namespace vesc_driver -#endif // VESC_DRIVER_VESC_PACKET_H_ +#endif // VESC_DRIVER_VESC_PACKET_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/vesc_driver/include/vesc_driver/vesc_packet_factory.h index 643d6f9..f856f44 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.h +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.h @@ -1,40 +1,17 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_ #define VESC_DRIVER_VESC_PACKET_FACTORY_H_ -#include -#include +#include #include -#include #include -#include +#include +#include +#include + +#include "vesc_driver/v8stdint.h" #include "vesc_driver/vesc_packet.h" namespace vesc_driver @@ -43,7 +20,7 @@ namespace vesc_driver /** * Class for creating VESC packets from raw data. */ -class VescPacketFactory +class VescPacketFactory : private boost::noncopyable { public: /** Return the global factory object */ @@ -73,42 +50,33 @@ class VescPacketFactory const Buffer::const_iterator& end, int* num_bytes_needed, std::string* what); - typedef std::function)> CreateFn; + typedef boost::function)> CreateFn; /** Register a packet type with the factory. */ static void registerPacketType(int payload_id, CreateFn fn); - /** - * Delete copy constructor and equals operator. - */ - VescPacketFactory(const VescPacketFactory &) = delete; - VescPacketFactory & operator=(const VescPacketFactory &) = delete; - private: - VescPacketFactory(); + typedef std::map FactoryMap; static FactoryMap* getMap(); }; -template -class PacketFactoryTemplate -{ -public: - explicit PacketFactoryTemplate(int payload_id) - { - VescPacketFactory::registerPacketType(payload_id, &PacketFactoryTemplate::create); - } - - static VescPacketPtr create(std::shared_ptr frame) - { - return VescPacketPtr(new PACKETTYPE(frame)); - } -}; - /** Use this macro to register packets */ -#define REGISTER_PACKET_TYPE(id, klass) \ -static PacketFactoryTemplate global_##klass##Factory((id)); - -} // namespace vesc_driver - -#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ +#define REGISTER_PACKET_TYPE(id, klass) \ +class klass##Factory \ +{ \ +public: \ + klass##Factory() \ + { \ + VescPacketFactory::registerPacketType((id), &klass##Factory::create); \ + } \ + static VescPacketPtr create(boost::shared_ptr frame) \ + { \ + return VescPacketPtr(new klass(frame)); \ + } \ +}; \ +static klass##Factory global_##klass##Factory; + +} // namespace vesc_driver + +#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ diff --git a/vesc_driver/launch/vesc_driver_node.launch b/vesc_driver/launch/vesc_driver_node.launch index f7d4765..07d5e85 100644 --- a/vesc_driver/launch/vesc_driver_node.launch +++ b/vesc_driver/launch/vesc_driver_node.launch @@ -17,4 +17,17 @@ + + + + + + + + + + diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 8b58cc4..2ed45e3 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -1,30 +1,37 @@ - - + vesc_driver - 1.1.0 + 0.0.1 ROS device driver for the Vedder VESC open source motor driver. - Johannes Betz + + Michael T. Boulet + Michael T. Boulet BSD + http://www.ros.org/wiki/vesc_driver - https://github.com/f1tenth/vesc - https://github.com/f1tenth/vesc/issues - Michael T. Boulet - Joshua Whitley + https://github.mit.edu/racecar/vesc + https://github.mit.edu/racecar/vesc/issues catkin - roslint - nodelet - pluginlib - roscpp - serial - std_msgs - vesc_msgs + nodelet + pluginlib + roscpp + std_msgs + vesc_msgs + serial + + nodelet + pluginlib + roscpp + std_msgs + vesc_msgs + serial + diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index ed90113..fa2c864 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -1,363 +1,394 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_driver.h" #include #include -#include #include -#include +#include #include +#include namespace vesc_driver { -using std::placeholders::_1; - -VescDriver::VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh) : - vesc_(std::string(), - std::bind(&VescDriver::vescPacketCallback, this, _1), - std::bind(&VescDriver::vescErrorCallback, this, _1)), - duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"), - brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), - position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), - driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) -{ - // get vesc serial port address - std::string port; - if (!private_nh.getParam("port", port)) + VescDriver::VescDriver(ros::NodeHandle nh, + ros::NodeHandle private_nh) : vesc_(std::string(), + boost::bind(&VescDriver::vescPacketCallback, this, _1), + boost::bind(&VescDriver::vescErrorCallback, this, _1)), + duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"), + brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), + position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), + driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) { - ROS_FATAL("VESC communication port parameter required."); - ros::shutdown(); - return; + // get vesc serial port address + std::string port; + if (!private_nh.getParam("port", port)) + { + ROS_FATAL("VESC communication port parameter required."); + ros::shutdown(); + return; + } + + // attempt to connect to the serial port + try + { + vesc_.connect(port); + } + catch (SerialException e) + { + ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); + ros::shutdown(); + return; + } + + // create vesc state (telemetry) publisher + state_pub_ = nh.advertise("sensors/core", 10); + imu_pub_ = nh.advertise("sensors/imu", 10); + // imu_std_pub_ = nh.advertise("sensors/imu/raw", 10); + + // since vesc state does not include the servo position, publish the commanded + // servo position as a "sensor" + servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); + + // subscribe to motor and servo command topics + duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, + &VescDriver::dutyCycleCallback, this); + current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); + brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); + speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); + position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); + servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); + + // create a 50Hz timer, used for state machine & polling VESC telemetry + timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); } - // attempt to connect to the serial port - try + /* TODO or TO-THINKABOUT LIST + - what should we do on startup? send brake or zero command? + - what to do if the vesc interface gives an error? + - check version number against know compatable? + - should we wait until we receive telemetry before sending commands? + - should we track the last motor command + - what to do if no motor command received recently? + - what to do if no servo command received recently? + - what is the motor safe off state (0 current?) + - what to do if a command parameter is out of range, ignore? + - try to predict vesc bounds (from vesc config) and command detect bounds errors + */ + + void VescDriver::timerCallback(const ros::TimerEvent &event) { - vesc_.connect(port); + // VESC interface should not unexpectedly disconnect, but test for it anyway + if (!vesc_.isConnected()) + { + ROS_FATAL("Unexpectedly disconnected from serial port."); + timer_.stop(); + ros::shutdown(); + return; + } + + /* + * Driver state machine, modes: + * INITIALIZING - request and wait for vesc version + * OPERATING - receiving commands from subscriber topics + */ + if (driver_mode_ == MODE_INITIALIZING) + { + // ROS_INFO_STREAM("driver_mode = MODE_INITIALIZING"); + // request version number, return packet will update the internal version numbers + vesc_.requestFWVersion(); + if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) + { + ROS_INFO("Connected to VESC with firmware version %d.%d", + fw_version_major_, fw_version_minor_); + driver_mode_ = MODE_OPERATING; + } + } + else if (driver_mode_ == MODE_OPERATING) + { + // ROS_INFO_STREAM("driver_mode = MODE_OPERATING"); + // poll for vesc state (telemetry) + vesc_.requestState(); + // poll for vesc imu + vesc_.requestImuData(); + } + else + { + // unknown mode, how did that happen? + assert(false && "unknown driver mode"); + } } - catch (SerialException e) + + void VescDriver::vescPacketCallback(const boost::shared_ptr &packet) { - ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); - ros::shutdown(); - return; + + // ROS_INFO_STREAM(packet->name()); + + if (packet->name() == "Values") + { + boost::shared_ptr values = + boost::dynamic_pointer_cast(packet); + + vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); + state_msg->header.stamp = ros::Time::now(); + state_msg->state.voltage_input = values->v_in(); + state_msg->state.temperature_pcb = values->temp_pcb(); + state_msg->state.current_motor = values->current_motor(); + state_msg->state.current_input = values->current_in(); + state_msg->state.speed = values->rpm(); + state_msg->state.duty_cycle = values->duty_now(); + state_msg->state.charge_drawn = values->amp_hours(); + state_msg->state.charge_regen = values->amp_hours_charged(); + state_msg->state.energy_drawn = values->watt_hours(); + state_msg->state.energy_regen = values->watt_hours_charged(); + state_msg->state.displacement = values->tachometer(); + state_msg->state.distance_traveled = values->tachometer_abs(); + state_msg->state.fault_code = values->fault_code(); + + state_pub_.publish(state_msg); + } + else if (packet->name() == "FWVersion") + { + boost::shared_ptr fw_version = + boost::dynamic_pointer_cast(packet); + // todo: might need lock here + fw_version_major_ = fw_version->fwMajor(); + fw_version_minor_ = fw_version->fwMinor(); + } + else if (packet->name() == "ImuData") + { + + boost::shared_ptr imuData = + boost::dynamic_pointer_cast(packet); + + // vesc_msgs::VescImuStamped::Ptr imu_msg(new vesc_msgs::VescImuStamped); + // vesc_msgs::VescIMUStamped::Ptr std_imu_msg(new vesc_msgs::VescIMUStamped); + + auto imu_msg = new vesc_msgs::VescImuStamped(); + // auto std_imu_msg = Imu(); + + imu_msg->header.stamp = ros::Time::now(); + // std_imu_msg.header.stamp = ros::Time::now(); + + imu_msg->imu.ypr.x = imuData->roll(); + imu_msg->imu.ypr.y = imuData->pitch(); + imu_msg->imu.ypr.z = imuData->yaw(); + + imu_msg->imu.linear_acceleration.x = imuData->acc_x(); + imu_msg->imu.linear_acceleration.y = imuData->acc_y(); + imu_msg->imu.linear_acceleration.z = imuData->acc_z(); + + imu_msg->imu.angular_velocity.x = imuData->gyr_x(); + imu_msg->imu.angular_velocity.y = imuData->gyr_y(); + imu_msg->imu.angular_velocity.z = imuData->gyr_z(); + + imu_msg->imu.compass.x = imuData->mag_x(); + imu_msg->imu.compass.y = imuData->mag_y(); + imu_msg->imu.compass.z = imuData->mag_z(); + + imu_msg->imu.orientation.w = imuData->q_w(); + imu_msg->imu.orientation.x = imuData->q_x(); + imu_msg->imu.orientation.y = imuData->q_y(); + imu_msg->imu.orientation.z = imuData->q_z(); + + // std_imu_msg.linear_acceleration.x = imuData->acc_x(); + // std_imu_msg.linear_acceleration.y = imuData->acc_y(); + // std_imu_msg.linear_acceleration.z = imuData->acc_z(); + + // std_imu_msg.angular_velocity.x = imuData->gyr_x(); + // std_imu_msg.angular_velocity.y = imuData->gyr_y(); + // std_imu_msg.angular_velocity.z = imuData->gyr_z(); + + // std_imu_msg.orientation.w = imuData->q_w(); + // std_imu_msg.orientation.x = imuData->q_x(); + // std_imu_msg.orientation.y = imuData->q_y(); + // std_imu_msg.orientation.z = imuData->q_z(); + + imu_pub_.publish(*imu_msg); + // imu_std_pub_.publish(std_imu_msg); + } } - // create vesc state (telemetry) publisher - state_pub_ = nh.advertise("sensors/core", 10); - - // since vesc state does not include the servo position, publish the commanded - // servo position as a "sensor" - servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); - - // subscribe to motor and servo command topics - duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, - &VescDriver::dutyCycleCallback, this); - current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); - brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); - speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); - position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); - servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); - - // create a 50Hz timer, used for state machine & polling VESC telemetry - timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); -} - -/* TODO or TO-THINKABOUT LIST - - what should we do on startup? send brake or zero command? - - what to do if the vesc interface gives an error? - - check version number against know compatable? - - should we wait until we receive telemetry before sending commands? - - should we track the last motor command - - what to do if no motor command received recently? - - what to do if no servo command received recently? - - what is the motor safe off state (0 current?) - - what to do if a command parameter is out of range, ignore? - - try to predict vesc bounds (from vesc config) and command detect bounds errors -*/ - -void VescDriver::timerCallback(const ros::TimerEvent& event) -{ - // VESC interface should not unexpectedly disconnect, but test for it anyway - if (!vesc_.isConnected()) + void VescDriver::vescErrorCallback(const std::string &error) { - ROS_FATAL("Unexpectedly disconnected from serial port."); - timer_.stop(); - ros::shutdown(); - return; + ROS_ERROR("%s", error.c_str()); } - /* - * Driver state machine, modes: - * INITIALIZING - request and wait for vesc version - * OPERATING - receiving commands from subscriber topics + /** + * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However, + * note that the VESC may impose a more restrictive bounds on the range depending + * on its configuration, e.g. absolute value is between 0.05 and 0.95. */ - if (driver_mode_ == MODE_INITIALIZING) + void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr &duty_cycle) { - // request version number, return packet will update the internal version numbers - vesc_.requestFWVersion(); - if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) + if (driver_mode_ = MODE_OPERATING) { - ROS_INFO("Connected to VESC with firmware version %d.%d", - fw_version_major_, fw_version_minor_); - driver_mode_ = MODE_OPERATING; + vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } } - else if (driver_mode_ == MODE_OPERATING) - { - // poll for vesc state (telemetry) - vesc_.requestState(); - } - else - { - // unknown mode, how did that happen? - assert(false && "unknown driver mode"); - } -} -void VescDriver::vescPacketCallback(const std::shared_ptr& packet) -{ - if (packet->name() == "Values") - { - std::shared_ptr values = - std::dynamic_pointer_cast(packet); - - vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); - state_msg->header.stamp = ros::Time::now(); - state_msg->state.voltage_input = values->v_in(); - state_msg->state.temperature_pcb = values->temp_pcb(); - state_msg->state.current_motor = values->current_motor(); - state_msg->state.current_input = values->current_in(); - state_msg->state.speed = values->rpm(); - state_msg->state.duty_cycle = values->duty_now(); - state_msg->state.charge_drawn = values->amp_hours(); - state_msg->state.charge_regen = values->amp_hours_charged(); - state_msg->state.energy_drawn = values->watt_hours(); - state_msg->state.energy_regen = values->watt_hours_charged(); - state_msg->state.displacement = values->tachometer(); - state_msg->state.distance_traveled = values->tachometer_abs(); - state_msg->state.fault_code = values->fault_code(); - - state_pub_.publish(state_msg); - } - else if (packet->name() == "FWVersion") + /** + * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However, + * note that the VESC may impose a more restrictive bounds on the range depending on + * its configuration. + */ + void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr ¤t) { - std::shared_ptr fw_version = - std::dynamic_pointer_cast(packet); - // todo: might need lock here - fw_version_major_ = fw_version->fwMajor(); - fw_version_minor_ = fw_version->fwMinor(); + if (driver_mode_ = MODE_OPERATING) + { + vesc_.setCurrent(current_limit_.clip(current->data)); + } } -} -void VescDriver::vescErrorCallback(const std::string& error) -{ - ROS_ERROR("%s", error.c_str()); -} - -/** - * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However, - * note that the VESC may impose a more restrictive bounds on the range depending - * on its configuration, e.g. absolute value is between 0.05 and 0.95. - */ -void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) -{ - if (driver_mode_ == MODE_OPERATING) - { - vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); - } -} - -/** - * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However, - * note that the VESC may impose a more restrictive bounds on the range depending on - * its configuration. - */ -void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) -{ - if (driver_mode_ == MODE_OPERATING) - { - vesc_.setCurrent(current_limit_.clip(current->data)); - } -} - -/** - * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver. - * However, note that the VESC may impose a more restrictive bounds on the range - * depending on its configuration. - */ -void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) -{ - if (driver_mode_ == MODE_OPERATING) - { - vesc_.setBrake(brake_limit_.clip(brake->data)); - } -} - -/** - * @param speed Commanded VESC speed in electrical RPM. Electrical RPM is the mechanical RPM - * multiplied by the number of motor poles. Any value is accepted by this - * driver. However, note that the VESC may impose a more restrictive bounds on the - * range depending on its configuration. - */ -void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) -{ - if (driver_mode_ == MODE_OPERATING) + /** + * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver. + * However, note that the VESC may impose a more restrictive bounds on the range + * depending on its configuration. + */ + void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr &brake) { - vesc_.setSpeed(speed_limit_.clip(speed->data)); + if (driver_mode_ = MODE_OPERATING) + { + vesc_.setBrake(brake_limit_.clip(brake->data)); + } } -} -/** - * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. - * Note that the VESC must be in encoder mode for this command to have an effect. - */ -void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) -{ - if (driver_mode_ == MODE_OPERATING) + /** + * @param speed Commanded VESC speed in electrical RPM. Electrical RPM is the mechanical RPM + * multiplied by the number of motor poles. Any value is accepted by this + * driver. However, note that the VESC may impose a more restrictive bounds on the + * range depending on its configuration. + */ + void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr &speed) { - // ROS uses radians but VESC seems to use degrees. Convert to degrees. - double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; - vesc_.setPosition(position_deg); + if (driver_mode_ = MODE_OPERATING) + { + vesc_.setSpeed(speed_limit_.clip(speed->data)); + } } -} -/** - * @param servo Commanded VESC servo output position. Valid range is 0 to 1. - */ -void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) -{ - if (driver_mode_ == MODE_OPERATING) + /** + * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. + * Note that the VESC must be in encoder mode for this command to have an effect. + */ + void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr &position) { - double servo_clipped(servo_limit_.clip(servo->data)); - vesc_.setServo(servo_clipped); - // publish clipped servo value as a "sensor" - std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); - servo_sensor_msg->data = servo_clipped; - servo_sensor_pub_.publish(servo_sensor_msg); + if (driver_mode_ = MODE_OPERATING) + { + // ROS uses radians but VESC seems to use degrees. Convert to degrees. + double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; + vesc_.setPosition(position_deg); + } } -} -VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, - const boost::optional& min_lower, - const boost::optional& max_upper) : - name(str) -{ - // check if user's minimum value is outside of the range min_lower to max_upper - double param_min; - if (nh.getParam(name + "_min", param_min)) + /** + * @param servo Commanded VESC servo output position. Valid range is 0 to 1. + */ + void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr &servo) { - if (min_lower && param_min < *min_lower) + if (driver_mode_ = MODE_OPERATING) { - lower = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << - ") is less than the feasible minimum (" << *min_lower << ")."); + double servo_clipped(servo_limit_.clip(servo->data)); + vesc_.setServo(servo_clipped); + // publish clipped servo value as a "sensor" + std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); + servo_sensor_msg->data = servo_clipped; + servo_sensor_pub_.publish(servo_sensor_msg); } - else if (max_upper && param_min > *max_upper) + } + + VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle &nh, const std::string &str, + const boost::optional &min_lower, + const boost::optional &max_upper) : name(str) + { + // check if user's minimum value is outside of the range min_lower to max_upper + double param_min; + if (nh.getParam(name + "_min", param_min)) { - lower = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << - ") is greater than the feasible maximum (" << *max_upper << ")."); + if (min_lower && param_min < *min_lower) + { + lower = *min_lower; + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is less than the feasible minimum (" << *min_lower << ")."); + } + else if (max_upper && param_min > *max_upper) + { + lower = *max_upper; + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is greater than the feasible maximum (" << *max_upper << ")."); + } + else + { + lower = param_min; + } } - else + else if (min_lower) { - lower = param_min; + lower = *min_lower; } - } - else if (min_lower) - { - lower = *min_lower; - } - // check if the uers' maximum value is outside of the range min_lower to max_upper - double param_max; - if (nh.getParam(name + "_max", param_max)) - { - if (min_lower && param_max < *min_lower) + // check if the uers' maximum value is outside of the range min_lower to max_upper + double param_max; + if (nh.getParam(name + "_max", param_max)) { - upper = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << - ") is less than the feasible minimum (" << *min_lower << ")."); + if (min_lower && param_max < *min_lower) + { + upper = *min_lower; + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is less than the feasible minimum (" << *min_lower << ")."); + } + else if (max_upper && param_max > *max_upper) + { + upper = *max_upper; + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is greater than the feasible maximum (" << *max_upper << ")."); + } + else + { + upper = param_max; + } } - else if (max_upper && param_max > *max_upper) + else if (max_upper) { upper = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << - ") is greater than the feasible maximum (" << *max_upper << ")."); } - else + + // check for min > max + if (upper && lower && *lower > *upper) { - upper = param_max; + ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper + << ") is less than parameter " << name << "_min (" << *lower << ")."); + double temp(*lower); + lower = *upper; + upper = temp; } - } - else if (max_upper) - { - upper = *max_upper; - } - // check for min > max - if (upper && lower && *lower > *upper) - { - ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper - << ") is less than parameter " << name << "_min (" << *lower << ")."); - double temp(*lower); - lower = *upper; - upper = temp; + std::ostringstream oss; + oss << " " << name << " limit: "; + if (lower) + oss << *lower << " "; + else + oss << "(none) "; + if (upper) + oss << *upper; + else + oss << "(none)"; + ROS_DEBUG_STREAM(oss.str()); } - std::ostringstream oss; - oss << " " << name << " limit: "; - if (lower) oss << *lower << " "; - else oss << "(none) "; - if (upper) oss << *upper; - else oss << "(none)"; - ROS_DEBUG_STREAM(oss.str()); -} - -double VescDriver::CommandLimit::clip(double value) -{ - if (lower && value < lower) - { - ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", - name.c_str(), value, *lower); - return *lower; - } - if (upper && value > upper) - { - ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", - name.c_str(), value, *upper); - return *upper; + double VescDriver::CommandLimit::clip(double value) + { // check + if (lower && value < lower) + { + ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", + name.c_str(), value, *lower); + return *lower; + } + if (upper && value > upper) + { + ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", + name.c_str(), value, *upper); + return *upper; + } + return value; } - return value; -} - -} // namespace vesc_driver +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_driver_node.cpp b/vesc_driver/src/vesc_driver_node.cpp index 2c61547..115191e 100644 --- a/vesc_driver/src/vesc_driver_node.cpp +++ b/vesc_driver/src/vesc_driver_node.cpp @@ -1,28 +1,3 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - #include #include "vesc_driver/vesc_driver.h" diff --git a/vesc_driver/src/vesc_driver_nodelet.cpp b/vesc_driver/src/vesc_driver_nodelet.cpp index 34134af..78c8d3b 100644 --- a/vesc_driver/src/vesc_driver_nodelet.cpp +++ b/vesc_driver/src/vesc_driver_nodelet.cpp @@ -1,34 +1,8 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - +#include #include #include #include -#include - #include "vesc_driver/vesc_driver.h" namespace vesc_driver @@ -37,13 +11,16 @@ namespace vesc_driver class VescDriverNodelet: public nodelet::Nodelet { public: + VescDriverNodelet() {} private: + virtual void onInit(void); - std::shared_ptr vesc_driver_; -}; // class VescDriverNodelet + boost::shared_ptr vesc_driver_; + +}; // class VescDriverNodelet void VescDriverNodelet::onInit() { @@ -51,6 +28,6 @@ void VescDriverNodelet::onInit() vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); } -} // namespace vesc_driver +} // namespace vesc_driver PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 618185f..29920e8 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -1,43 +1,16 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_interface.h" #include -#include -#include -#include -#include -#include #include -#include +#include +#include +#include #include +#include #include "vesc_driver/vesc_packet_factory.h" @@ -52,48 +25,47 @@ class VescInterface::Impl serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none) {} - void rxThread(); + void* rxThread(void); - std::thread rxThreadHelper() + static void* rxThreadHelper(void *context) { - return std::thread(&Impl::rxThread, this); + return ((VescInterface::Impl*)context)->rxThread(); } - std::thread rx_thread_; + pthread_t rx_thread_; bool rx_thread_run_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; serial::Serial serial_; + VescFrame::CRC send_crc_; }; -void VescInterface::Impl::rxThread() +void* VescInterface::Impl::rxThread(void) { Buffer buffer; buffer.reserve(4096); - while (rx_thread_run_) - { + while (rx_thread_run_) { + int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) - { + if (!buffer.empty()) { + // search buffer for valid packet(s) Buffer::iterator iter(buffer.begin()); Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) - { + while (iter != buffer.end()) { + // check if valid start-of-frame character if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) - { + VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) { + // good start, now attempt to create packet std::string error; VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); - if (packet) - { + if (packet) { // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) - { + if (std::distance(iter_begin, iter) > 0) { std::ostringstream ss; ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << std::distance(iter_begin, iter) << " bytes."; @@ -107,13 +79,11 @@ void VescInterface::Impl::rxThread() // continue to look for another frame in buffer continue; } - else if (bytes_needed > 0) - { + else if (bytes_needed > 0) { // need more data, break out of while loop - break; // for (iter_sof... + break; // for (iter_sof... } - else - { + else { // else, this was not a packet, move on to next byte error_handler_(error); } @@ -127,8 +97,7 @@ void VescInterface::Impl::rxThread() bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) - { + if (std::distance(iter_begin, iter) > 0) { std::ostringstream ss; ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; error_handler_(ss.str()); @@ -140,10 +109,10 @@ void VescInterface::Impl::rxThread() int bytes_to_read = std::max(bytes_needed, std::min(4096, static_cast(serial_.available()))); int bytes_read = serial_.read(buffer, bytes_to_read); - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) - { + if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); } + } } @@ -181,38 +150,38 @@ void VescInterface::connect(const std::string& port) { // todo - mutex? - if (isConnected()) - { + if (isConnected()) { throw SerialException("Already connected to serial port."); } // connect to serial port - try - { + try { impl_->serial_.setPort(port); impl_->serial_.open(); } - catch (const std::exception& e) - { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); + catch (const std::exception& e) { + std::stringstream ss; + ss << "Failed to open the serial port to the VESC. " << e.what(); + throw SerialException(ss.str().c_str()); } // start up a monitoring thread impl_->rx_thread_run_ = true; - impl_->rx_thread_ = impl_->rxThreadHelper(); + int result = + pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); + assert(0 == result); } void VescInterface::disconnect() { // todo - mutex? - if (isConnected()) - { + if (isConnected()) { // bring down read thread impl_->rx_thread_run_ = false; - impl_->rx_thread_.join(); + int result = pthread_join(impl_->rx_thread_, NULL); + assert(0 == result); + impl_->serial_.close(); } } @@ -225,8 +194,7 @@ bool VescInterface::isConnected() const void VescInterface::send(const VescPacket& packet) { size_t written = impl_->serial_.write(packet.frame()); - if (written != packet.frame().size()) - { + if (written != packet.frame().size()) { std::stringstream ss; ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; throw SerialException(ss.str().c_str()); @@ -273,4 +241,10 @@ void VescInterface::setServo(double servo) send(VescPacketSetServoPos(servo)); } -} // namespace vesc_driver +void VescInterface::requestImuData() +{ + send(VescPacketRequestImu()); +} + + +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 8e96365..1adc3cb 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -1,370 +1,607 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_packet.h" #include #include -#include -#include + +#include +#include +#include #include "vesc_driver/vesc_packet_factory.h" #include "vesc_driver/datatypes.h" +#include + namespace vesc_driver { -constexpr CRC::Parameters VescFrame::CRC_TYPE; + VescFrame::VescFrame(int payload_size) + { + assert(payload_size >= 0 && payload_size <= 1024); + + if (payload_size < 256) + { + // single byte payload size + frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); + *frame_->begin() = 2; + *(frame_->begin() + 1) = payload_size; + payload_.first = frame_->begin() + 2; + } + else + { + // two byte payload size + frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); + *frame_->begin() = 3; + *(frame_->begin() + 1) = payload_size >> 8; + *(frame_->begin() + 2) = payload_size & 0xFF; + payload_.first = frame_->begin() + 3; + } + + payload_.second = payload_.first + payload_size; + *(frame_->end() - 1) = 3; + } -VescFrame::VescFrame(int payload_size) -{ - assert(payload_size >= 0 && payload_size <= 1024); + VescFrame::VescFrame(const BufferRangeConst &frame, const BufferRangeConst &payload) + { + /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap + checks anyway */ + assert(boost::distance(frame) >= VESC_MIN_FRAME_SIZE); + assert(boost::distance(frame) <= VESC_MAX_FRAME_SIZE); + assert(boost::distance(payload) <= VESC_MAX_PAYLOAD_SIZE); + assert(std::distance(frame.first, payload.first) > 0 && + std::distance(payload.second, frame.second) > 0); + + frame_.reset(new Buffer(boost::begin(frame), boost::end(frame))); + payload_.first = frame_->begin() + std::distance(frame.first, payload.first); + payload_.second = frame_->begin() + std::distance(frame.first, payload.second); + } - if (payload_size < 256) + VescPacket::VescPacket(const std::string &name, int payload_size, int payload_id) : VescFrame(payload_size), name_(name) { - // single byte payload size - frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); - *frame_->begin() = 2; - *(frame_->begin() + 1) = payload_size; - payload_.first = frame_->begin() + 2; + assert(payload_id >= 0 && payload_id < 256); + assert(boost::distance(payload_) > 0); + *payload_.first = payload_id; } - else + + VescPacket::VescPacket(const std::string &name, boost::shared_ptr raw) : VescFrame(*raw), name_(name) { - // two byte payload size - frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); - *frame_->begin() = 3; - *(frame_->begin() + 1) = payload_size >> 8; - *(frame_->begin() + 2) = payload_size & 0xFF; - payload_.first = frame_->begin() + 3; } - payload_.second = payload_.first + payload_size; - *(frame_->end() - 1) = 3; -} + /*------------------------------------------------------------------------------------------------*/ -VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload) -{ - /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap - checks anyway */ - assert(std::distance(frame.first, frame.second) >= VESC_MIN_FRAME_SIZE); - assert(std::distance(frame.first, frame.second) <= VESC_MAX_FRAME_SIZE); - assert(std::distance(payload.first, payload.second) <= VESC_MAX_PAYLOAD_SIZE); - assert(std::distance(frame.first, payload.first) > 0 && - std::distance(payload.second, frame.second) > 0); - - frame_.reset(new Buffer(frame.first, frame.second)); - payload_.first = frame_->begin() + std::distance(frame.first, payload.first); - payload_.second = frame_->begin() + std::distance(frame.first, payload.second); -} - -VescPacket::VescPacket(const std::string& name, int payload_size, int payload_id) : - VescFrame(payload_size), name_(name) -{ - assert(payload_id >= 0 && payload_id < 256); - assert(std::distance(payload_.first, payload_.second) > 0); - *payload_.first = payload_id; -} + VescPacketFWVersion::VescPacketFWVersion(boost::shared_ptr raw) : VescPacket("FWVersion", raw) + { + } -VescPacket::VescPacket(const std::string& name, std::shared_ptr raw) : - VescFrame(*raw), name_(name) -{ -} + int VescPacketFWVersion::fwMajor() const + { + return *(payload_.first + 1); + } -/*------------------------------------------------------------------------------------------------*/ + int VescPacketFWVersion::fwMinor() const + { + return *(payload_.first + 2); + } -VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : - VescPacket("FWVersion", raw) -{ -} + REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) -int VescPacketFWVersion::fwMajor() const -{ - return *(payload_.first + 1); -} + VescPacketRequestFWVersion::VescPacketRequestFWVersion() : VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) + { + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } -int VescPacketFWVersion::fwMinor() const -{ - return *(payload_.first + 2); -} + /*------------------------------------------------------------------------------------------------*/ -REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) + VescPacketValues::VescPacketValues(boost::shared_ptr raw) : VescPacket("Values", raw) + { + } -VescPacketRequestFWVersion::VescPacketRequestFWVersion() : - VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) -{ - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + // double VescPacketValues::temp_mos1() const + // { + // int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + + // static_cast(*(payload_.first + 2))); + // return static_cast(v) / 10.0; + // } + + // double VescPacketValues::temp_mos2() const + // { + // int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + + // static_cast(*(payload_.first + 4))); + // return static_cast(v) / 10.0; + // } + + double VescPacketValues::temp_mos1() const + { + int16_t v = static_cast( + (static_cast(*(payload_.first + 59)) << 8) + + (static_cast(*(payload_.first + 60)))); + return static_cast(v) / 10.0; + } -/*------------------------------------------------------------------------------------------------*/ + double VescPacketValues::temp_mos2() const + { + int16_t v = static_cast( + (static_cast(*(payload_.first + 61)) << 8) + + (static_cast(*(payload_.first + 62)))); + return static_cast(v) / 10.0; + } -VescPacketValues::VescPacketValues(std::shared_ptr raw) : - VescPacket("Values", raw) -{ -} + double VescPacketValues::temp_mos3() const + { + int16_t v = static_cast( + (static_cast(*(payload_.first + 63)) << 8) + + (static_cast(*(payload_.first + 64)))); + return static_cast(v) / 10.0; + } + double VescPacketValues::current_motor() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + static_cast(*(payload_.first + 8))); + return static_cast(v) / 100.0; + } -double VescPacketValues::temp_mos1() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); - return static_cast(v) / 10.0; -} + double VescPacketValues::current_in() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + static_cast(*(payload_.first + 12))); + return static_cast(v) / 100.0; + } -double VescPacketValues::temp_mos2() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); - return static_cast(v) / 10.0; -} + double VescPacketValues::duty_now() const + { + int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + + static_cast(*(payload_.first + 22))); + return static_cast(v) / 1000.0; + } -double VescPacketValues::current_motor() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + - (static_cast(*(payload_.first + 6)) << 16) + - (static_cast(*(payload_.first + 7)) << 8) + - static_cast(*(payload_.first + 8))); - return static_cast(v) / 100.0; -} - -double VescPacketValues::current_in() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + - (static_cast(*(payload_.first + 10)) << 16) + - (static_cast(*(payload_.first + 11)) << 8) + - static_cast(*(payload_.first + 12))); - return static_cast(v) / 100.0; -} + double VescPacketValues::rpm() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + static_cast(*(payload_.first + 26))); + return static_cast(-1 * v); + } + double VescPacketValues::amp_hours() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + + (static_cast(*(payload_.first + 28)) << 16) + + (static_cast(*(payload_.first + 29)) << 8) + + static_cast(*(payload_.first + 30))); + return static_cast(v); + } -double VescPacketValues::duty_now() const -{ - int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); - return static_cast(v) / 1000.0; -} + double VescPacketValues::amp_hours_charged() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 31)) << 24) + + (static_cast(*(payload_.first + 32)) << 16) + + (static_cast(*(payload_.first + 33)) << 8) + + static_cast(*(payload_.first + 34))); + return static_cast(v); + } -double VescPacketValues::rpm() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26))); - return static_cast(-1 * v); -} - -double VescPacketValues::amp_hours() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + - (static_cast(*(payload_.first + 28)) << 16) + - (static_cast(*(payload_.first + 29)) << 8) + - static_cast(*(payload_.first + 30))); - return static_cast(v); -} - -double VescPacketValues::amp_hours_charged() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 31)) << 24) + - (static_cast(*(payload_.first + 32)) << 16) + - (static_cast(*(payload_.first + 33)) << 8) + - static_cast(*(payload_.first + 34))); - return static_cast(v); -} - -double VescPacketValues::tachometer() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 35)) << 24) + - (static_cast(*(payload_.first + 36)) << 16) + - (static_cast(*(payload_.first + 37)) << 8) + - static_cast(*(payload_.first + 38))); - return static_cast(v); -} - -double VescPacketValues::tachometer_abs() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 39)) << 24) + - (static_cast(*(payload_.first + 40)) << 16) + - (static_cast(*(payload_.first + 41)) << 8) + - static_cast(*(payload_.first + 42))); - return static_cast(v); -} - -int VescPacketValues::fault_code() const -{ - return static_cast(*(payload_.first + 56)); -} + double VescPacketValues::tachometer() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 35)) << 24) + + (static_cast(*(payload_.first + 36)) << 16) + + (static_cast(*(payload_.first + 37)) << 8) + + static_cast(*(payload_.first + 38))); + return static_cast(v); + } -double VescPacketValues::v_in() const -{ - int32_t v = 0; - return static_cast(v); -} + double VescPacketValues::tachometer_abs() const + { + int32_t v = static_cast((static_cast(*(payload_.first + 39)) << 24) + + (static_cast(*(payload_.first + 40)) << 16) + + (static_cast(*(payload_.first + 41)) << 8) + + static_cast(*(payload_.first + 42))); + return static_cast(v); + } -double VescPacketValues::temp_pcb() const -{ - int32_t v = 0; - return static_cast(v); -} + int VescPacketValues::fault_code() const + { + return static_cast(*(payload_.first + 56)); + } -double VescPacketValues::watt_hours() const -{ - int32_t v = 0; - return static_cast(v); -} + double VescPacketValues::v_in() const + { + int32_t v = 0; + return static_cast(v); + } -double VescPacketValues::watt_hours_charged() const -{ - int32_t v = 0; - return static_cast(v); -} + double VescPacketValues::temp_pcb() const + { + int32_t v = 0; + return static_cast(v); + } -REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) + double VescPacketValues::watt_hours() const + { + int32_t v = 0; + return static_cast(v); + } -VescPacketRequestValues::VescPacketRequestValues() : - VescPacket("RequestValues", 1, COMM_GET_VALUES) -{ - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + double VescPacketValues::watt_hours_charged() const + { + int32_t v = 0; + return static_cast(v); + } -/*------------------------------------------------------------------------------------------------*/ + REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) + VescPacketRequestValues::VescPacketRequestValues() : VescPacket("RequestValues", 1, COMM_GET_VALUES) + { + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } -VescPacketSetDuty::VescPacketSetDuty(double duty) : - VescPacket("SetDuty", 5, COMM_SET_DUTY) -{ - /** @todo range check duty */ + /*------------------------------------------------------------------------------------------------*/ - int32_t v = static_cast(duty * 100000.0); + VescPacketSetDuty::VescPacketSetDuty(double duty) : VescPacket("SetDuty", 5, COMM_SET_DUTY) + { + /** @todo range check duty */ - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + int32_t v = static_cast(duty * 100000.0); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); -/*------------------------------------------------------------------------------------------------*/ + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } -VescPacketSetCurrent::VescPacketSetCurrent(double current) : - VescPacket("SetCurrent", 5, COMM_SET_CURRENT) -{ - int32_t v = static_cast(current * 1000.0); + /*------------------------------------------------------------------------------------------------*/ - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + VescPacketSetCurrent::VescPacketSetCurrent(double current) : VescPacket("SetCurrent", 5, COMM_SET_CURRENT) + { + int32_t v = static_cast(current * 1000.0); + + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + /*------------------------------------------------------------------------------------------------*/ -/*------------------------------------------------------------------------------------------------*/ + VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) + { + int32_t v = static_cast(current_brake * 1000.0); + + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } -VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : - VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) -{ - int32_t v = static_cast(current_brake * 1000.0); + /*------------------------------------------------------------------------------------------------*/ - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + VescPacketSetRPM::VescPacketSetRPM(double rpm) : VescPacket("SetRPM", 5, COMM_SET_RPM) + { + int32_t v = static_cast(rpm); + + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + /*------------------------------------------------------------------------------------------------*/ -/*------------------------------------------------------------------------------------------------*/ + VescPacketSetPos::VescPacketSetPos(double pos) : VescPacket("SetPos", 5, COMM_SET_POS) + { + /** @todo range check pos */ -VescPacketSetRPM::VescPacketSetRPM(double rpm) : - VescPacket("SetRPM", 5, COMM_SET_RPM) -{ - int32_t v = static_cast(rpm); + int32_t v = static_cast(pos * 1000000.0); - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } -/*------------------------------------------------------------------------------------------------*/ + /*------------------------------------------------------------------------------------------------*/ -VescPacketSetPos::VescPacketSetPos(double pos) : - VescPacket("SetPos", 5, COMM_SET_POS) -{ - /** @todo range check pos */ + VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) + { + /** @todo range check pos */ + + int16_t v = static_cast(servo_pos * 1000.0); + + *(payload_.first + 1) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 2) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } - int32_t v = static_cast(pos * 1000000.0); + // VescPacketImu::VescPacketImu(boost::shared_ptr raw) + // : VescPacket("ImuData", raw) //check + // { + // uint32_t ind = 1; + // mask_ = static_cast( + // (static_cast(*(payload_.first + ind )) << 8) + + // static_cast(*(payload_.first + ind + 1 )) + // ); + // ind += 2; + + // if (mask_ & ((uint32_t)1 << 0)) {roll_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 1)) {pitch_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 2)) {yaw_ = getFloat32Auto(&ind);} + + // if (mask_ & ((uint32_t)1 << 3)) {acc_x_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 4)) {acc_y_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 5)) {acc_z_ = getFloat32Auto(&ind);} + + // if (mask_ & ((uint32_t)1 << 6)) {gyr_x_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 7)) {gyr_y_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 8)) {gyr_z_ = getFloat32Auto(&ind);} + + // if (mask_ & ((uint32_t)1 << 9)) {mag_x_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 10)) {mag_y_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 11)) {mag_z_ = getFloat32Auto(&ind);} + + // if (mask_ & ((uint32_t)1 << 12)) {q0_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 13)) {q1_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 14)) {q2_ = getFloat32Auto(&ind);} + // if (mask_ & ((uint32_t)1 << 15)) {q3_ = getFloat32Auto(&ind);} + // } + + VescPacketImu::VescPacketImu(boost::shared_ptr raw) + : VescPacket("ImuData", raw) + { + uint32_t ind = 1; + mask_ = static_cast( + (static_cast(*(payload_.first + ind)) << 8) + + static_cast(*(payload_.first + ind + 1))); + ind += 2; + + if (mask_ & ((uint32_t)1 << 0)) + { + roll_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 1)) + { + pitch_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 2)) + { + yaw_ = getFloat32Auto(&ind); + } + + if (mask_ & ((uint32_t)1 << 3)) + { + acc_x_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 4)) + { + acc_y_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 5)) + { + acc_z_ = getFloat32Auto(&ind); + } + + if (mask_ & ((uint32_t)1 << 6)) + { + gyr_x_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 7)) + { + gyr_y_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 8)) + { + gyr_z_ = getFloat32Auto(&ind); + } + + if (mask_ & ((uint32_t)1 << 9)) + { + mag_x_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 10)) + { + mag_y_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 11)) + { + mag_z_ = getFloat32Auto(&ind); + } + + if (mask_ & ((uint32_t)1 << 12)) + { + q0_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 13)) + { + q1_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 14)) + { + q2_ = getFloat32Auto(&ind); + } + if (mask_ & ((uint32_t)1 << 15)) + { + q3_ = getFloat32Auto(&ind); + } + } - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + int VescPacketImu::mask() const + { + return mask_; + } - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + double VescPacketImu::getFloat32Auto(uint32_t *idx) const + { + int pos = *idx; + uint32_t res = static_cast( + (static_cast(*(payload_.first + (pos))) << 24) + + (static_cast(*(payload_.first + (pos + 1))) << 16) + + (static_cast(*(payload_.first + (pos + 2))) << 8) + + static_cast(*(payload_.first + (pos + 3)))); + + *idx += 4; + + int e = (res >> 23) & 0xFF; + int fr = res & 0x7FFFFF; + bool negative = res & (1u << 31); + + float f = 0.0; + if (e != 0 || fr != 0) + { + f = static_cast(fr) / (8388608.0 * 2.0) + 0.5; + e -= 126; + } + + if (negative) + { + f = -f; + } + return ldexpf(f, e); + } -/*------------------------------------------------------------------------------------------------*/ + double VescPacketImu::roll() const + { + return roll_ * 180 / M_PI; // da rad a gradi per debug deg + } -VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : - VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) -{ - /** @todo range check pos */ + double VescPacketImu::pitch() const + { + return pitch_ * 180 / M_PI; + } + + double VescPacketImu::yaw() const + { + return yaw_ * 180 / M_PI; + } + + double VescPacketImu::acc_x() const + { + return acc_x_; // g/s + } + + double VescPacketImu::acc_y() const + { + return acc_y_; + } + + double VescPacketImu::acc_z() const + { + return acc_z_; + } + + double VescPacketImu::gyr_x() const + { + return gyr_x_; // deg/s + } + + double VescPacketImu::gyr_y() const + { + return gyr_y_; + } + + double VescPacketImu::gyr_z() const + { + return gyr_z_; + } + + double VescPacketImu::mag_x() const + { + return mag_x_; + } + + double VescPacketImu::mag_y() const + { + return mag_y_; + } - int16_t v = static_cast(servo_pos * 1000.0); + double VescPacketImu::mag_z() const + { + return mag_z_; + } + + double VescPacketImu::q_w() const + { + return q0_; + } - *(payload_.first + 1) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 2) = static_cast(static_cast(v) & 0xFF); + double VescPacketImu::q_x() const + { + return q1_; + } - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); -} + double VescPacketImu::q_y() const + { + return q2_; + } + + double VescPacketImu::q_z() const + { + return q3_; + } + + REGISTER_PACKET_TYPE(COMM_GET_IMU_DATA, VescPacketImu) + + VescPacketRequestImu::VescPacketRequestImu() + : VescPacket("RequestImuData", 3, COMM_GET_IMU_DATA) + { + *(payload_.first + 1) = static_cast(0xFF); + *(payload_.first + 2) = static_cast(0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + + uint16_t crc = crc_calc.checksum(); + + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); + } + /*------------------------------------------------------------------------------------------------*/ -} // namespace vesc_driver +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index 328401e..470f9c5 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -1,36 +1,13 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 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. -// -// 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. -// -// 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. - // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_packet_factory.h" #include #include -#include -#include + +#include +#include +#include #include "vesc_driver/vesc_packet.h" @@ -61,8 +38,8 @@ VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, } VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, - int* num_bytes_needed, std::string* what) + const Buffer::const_iterator& end, + int* num_bytes_needed, std::string* what) { // initialize output variables if (num_bytes_needed != NULL) *num_bytes_needed = 0; @@ -81,14 +58,12 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi // get a view of the payload BufferRangeConst view_payload; - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) - { + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) { // payload size field is one byte view_payload.first = begin + 2; view_payload.second = view_payload.first + *(begin + 1); } - else - { + else { assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin); // payload size field is two bytes view_payload.first = begin + 3; @@ -96,7 +71,7 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi } // check length - if (std::distance(view_payload.first, view_payload.second) > VescFrame::VESC_MAX_PAYLOAD_SIZE) + if (boost::distance(view_payload) > VescFrame::VESC_MAX_PAYLOAD_SIZE) return createFailed(num_bytes_needed, what, "Invalid payload length"); // get iterators to crc field, end-of-frame field, and a view of the whole frame @@ -105,7 +80,7 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi BufferRangeConst view_frame(begin, iter_eof + 1); // do we have enough data in the buffer to complete the frame? - int frame_size = std::distance(view_frame.first, view_frame.second); + int frame_size = boost::distance(view_frame); if (buffer_size < frame_size) return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", frame_size - buffer_size); @@ -115,35 +90,35 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi return createFailed(num_bytes_needed, what, "Invalid end-of-frame character"); // is the crc valid? - uint16_t crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); - if (crc != CRC::Calculate( - &(*view_payload.first), std::distance(view_payload.first, view_payload.second), VescFrame::CRC_TYPE)) + unsigned short crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*view_payload.first), boost::distance(view_payload)); + if (crc != crc_calc.checksum()) return createFailed(num_bytes_needed, what, "Invalid checksum"); // frame looks good, construct the raw frame - std::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); + boost::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); // if the packet has a payload, construct the corresponding subclass - if (std::distance(view_payload.first, view_payload.second) > 0) - { + if (boost::distance(view_payload) > 0) { + // get constructor function from payload id FactoryMap* p_map(getMap()); FactoryMap::const_iterator search(p_map->find(*view_payload.first)); - if (search != p_map->end()) - { + if (search != p_map->end()) { return search->second(raw_frame); } - else - { + else { // no subclass constructor for this packet return createFailed(num_bytes_needed, what, "Unkown payload type."); } + } - else - { + else { // no payload return createFailed(num_bytes_needed, what, "Frame does not have a payload"); } } -} // namespace vesc_driver + +} // namesapce vesc_driver diff --git a/vesc_msgs/CHANGELOG.rst b/vesc_msgs/CHANGELOG.rst deleted file mode 100644 index f639fd9..0000000 --- a/vesc_msgs/CHANGELOG.rst +++ /dev/null @@ -1,17 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package vesc_msgs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.0 (2020-12-12) ------------------- -* Merge pull request `#1 `_ from f1tenth/melodic-devel - Updating for Melodic -* Updating package.xml to format 3 and setting C++ standard to 11. -* Contributors: Joshua Whitley - -1.0.0 (2020-12-02) ------------------- -* Adding roslint. -* Updating maintainers, authors, and URLs. -* added onboard car -* Contributors: Joshua Whitley, billyzheng diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index d1365bd..0fa1a3d 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -4,6 +4,7 @@ project(vesc_msgs) find_package(catkin REQUIRED COMPONENTS std_msgs message_generation + geometry_msgs ) add_message_files( @@ -11,12 +12,16 @@ add_message_files( FILES VescState.msg VescStateStamped.msg + VescImu.msg + VescImuStamped.msg ) + generate_messages( DEPENDENCIES std_msgs + geometry_msgs ) catkin_package( - CATKIN_DEPENDS std_msgs message_runtime + CATKIN_DEPENDS std_msgs message_runtime geometry_msgs ) diff --git a/vesc_msgs/msg/VescImu.msg b/vesc_msgs/msg/VescImu.msg new file mode 100644 index 0000000..09bccd1 --- /dev/null +++ b/vesc_msgs/msg/VescImu.msg @@ -0,0 +1,6 @@ +geometry_msgs/Vector3 ypr +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Vector3 angular_velocity + +geometry_msgs/Vector3 compass +geometry_msgs/Quaternion orientation diff --git a/vesc_msgs/msg/VescImuStamped.msg b/vesc_msgs/msg/VescImuStamped.msg new file mode 100644 index 0000000..c8915b1 --- /dev/null +++ b/vesc_msgs/msg/VescImuStamped.msg @@ -0,0 +1,4 @@ + + +std_msgs/Header header +VescImu imu \ No newline at end of file diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index dc14307..4db6960 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -1,23 +1,29 @@ - - + vesc_msgs - 1.1.0 + 0.0.1 ROS message definitions for the Vedder VESC open source motor controller. - Johannes Betz + + Michael T. Boulet + Michael T. Boulet BSD + http://ros.org/wiki/vesc_msgs - https://github.com/f1tenth/vesc - https://github.com/f1tenth/vesc/issues - Michael T. Boulet - Joshua Whitley + https://github.mit.edu/racecar/vesc + https://github.mit.edu/racecar/vesc/issues catkin + + std_msgs message_generation + geometry_msgs + imu_tools - std_msgs + std_msgs + message_runtime + geometry_msgs + imu_tools - message_runtime From dee9a729029ba7fd3a275a8a7e9fda8745f75956 Mon Sep 17 00:00:00 2001 From: Shreyas Date: Sun, 9 Apr 2023 19:14:30 -0400 Subject: [PATCH 02/20] added car.launch --- car.launch | 11 +++++++++++ vesc_driver/launch/vesc_driver_node.launch | 16 ++++++++-------- 2 files changed, 19 insertions(+), 8 deletions(-) create mode 100644 car.launch diff --git a/car.launch b/car.launch new file mode 100644 index 0000000..9f2a1c9 --- /dev/null +++ b/car.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/vesc_driver/launch/vesc_driver_node.launch b/vesc_driver/launch/vesc_driver_node.launch index 07d5e85..93f4451 100644 --- a/vesc_driver/launch/vesc_driver_node.launch +++ b/vesc_driver/launch/vesc_driver_node.launch @@ -4,18 +4,18 @@ - - - + + + - + - - + --> - - + + From fc021e2ae23086a2e77259da1565afeebbf82587 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Mon, 10 Apr 2023 01:56:13 -0400 Subject: [PATCH 03/20] [Madgwick Fusion]: Added a new ros packages that convers VESC IMU message to sensor messages IMU messages to beused by the madgwick fusion ros package --- vesc_imu_msg_parser/CMakeLists.txt | 212 ++++++++++++++++++ .../vesc_imu_msg_parser/imu_parser.hpp | 19 ++ vesc_imu_msg_parser/launch/imu_parser.launch | 19 ++ vesc_imu_msg_parser/package.xml | 70 ++++++ vesc_imu_msg_parser/src/imu_parser.cpp | 65 ++++++ 5 files changed, 385 insertions(+) create mode 100644 vesc_imu_msg_parser/CMakeLists.txt create mode 100644 vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp create mode 100644 vesc_imu_msg_parser/launch/imu_parser.launch create mode 100644 vesc_imu_msg_parser/package.xml create mode 100644 vesc_imu_msg_parser/src/imu_parser.cpp diff --git a/vesc_imu_msg_parser/CMakeLists.txt b/vesc_imu_msg_parser/CMakeLists.txt new file mode 100644 index 0000000..4116645 --- /dev/null +++ b/vesc_imu_msg_parser/CMakeLists.txt @@ -0,0 +1,212 @@ +cmake_minimum_required(VERSION 3.0.2) +project(vesc_imu_msg_parser) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + vesc_msgs + # imu_tools +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES vesc_imu_msg_parser +# CATKIN_DEPENDS imu_tools roscpp std_msgs vesc_msgs + CATKIN_DEPENDS roscpp std_msgs vesc_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/imu_parser.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(vesc_imu_msg_parser_node src/imu_parser.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(vesc_imu_msg_parser_node ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(vesc_imu_msg_parser_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +# ## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.hpp" +# PATTERN ".svn" EXCLUDE +# ) + +# install(DIRECTORY launch/ +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) + +# ## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + + + + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_vesc_imu_msg_parser.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp b/vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp new file mode 100644 index 0000000..3f7fc42 --- /dev/null +++ b/vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp @@ -0,0 +1,19 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "vesc_msgs/VescImuStamped.h" + +namespace vesc_imu_parser +{ +class ImuParser +{ +public: + ImuParser(ros::NodeHandle& nh); + +private: + ros::Publisher imu_data_pub; + ros::Publisher imu_mag_pub; + ros::Subscriber imu_data_sub; + + void imuDataCallback(const vesc_msgs::VescImuStamped& imu_msg); +}; +} // namespace vesc_imu_parser \ No newline at end of file diff --git a/vesc_imu_msg_parser/launch/imu_parser.launch b/vesc_imu_msg_parser/launch/imu_parser.launch new file mode 100644 index 0000000..3a0571e --- /dev/null +++ b/vesc_imu_msg_parser/launch/imu_parser.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/vesc_imu_msg_parser/package.xml b/vesc_imu_msg_parser/package.xml new file mode 100644 index 0000000..e901559 --- /dev/null +++ b/vesc_imu_msg_parser/package.xml @@ -0,0 +1,70 @@ + + + vesc_imu_msg_parser + 0.0.0 + The vesc_imu_msg_parser package + + + + + root + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + imu_tools + roscpp + std_msgs + vesc_msgs + imu_tools + roscpp + std_msgs + vesc_msgs + imu_tools + roscpp + std_msgs + vesc_msgs + + + + + + + diff --git a/vesc_imu_msg_parser/src/imu_parser.cpp b/vesc_imu_msg_parser/src/imu_parser.cpp new file mode 100644 index 0000000..85443b5 --- /dev/null +++ b/vesc_imu_msg_parser/src/imu_parser.cpp @@ -0,0 +1,65 @@ +#include "vesc_imu_msg_parser/imu_parser.hpp" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/MagneticField.h" +// #include "vesc_msgs/VescImuStamped.h" +namespace vesc_imu_parser +{ + +ImuParser::ImuParser(ros::NodeHandle& nh) +{ + ImuParser::imu_data_pub = nh.advertise("/imu/data_raw", 10); + ImuParser::imu_mag_pub = nh.advertise("/imu/mag", 10); + ImuParser::imu_data_sub = nh.subscribe("/sensors/imu", 10, &ImuParser::imuDataCallback, this); +} + +void ImuParser::imuDataCallback(const vesc_msgs::VescImuStamped& imu_msg) +{ + auto parsed_imu_msg = new sensor_msgs::Imu(); + auto parsed_imu_mag_msg = new sensor_msgs::MagneticField(); + + // imu data message + parsed_imu_mag_msg->header.stamp = ros::Time::now(); + parsed_imu_msg->header.stamp = ros::Time::now(); + + parsed_imu_msg->orientation.x = imu_msg.imu.orientation.x; + parsed_imu_msg->orientation.y = imu_msg.imu.orientation.y; + parsed_imu_msg->orientation.z = imu_msg.imu.orientation.z; + parsed_imu_msg->orientation.w = imu_msg.imu.orientation.w; + + parsed_imu_msg->linear_acceleration.x = imu_msg.imu.linear_acceleration.x; + parsed_imu_msg->linear_acceleration.y = imu_msg.imu.linear_acceleration.y; + parsed_imu_msg->linear_acceleration.z = imu_msg.imu.linear_acceleration.z; + + parsed_imu_msg->angular_velocity.x = imu_msg.imu.angular_velocity.x; + parsed_imu_msg->angular_velocity.y = imu_msg.imu.angular_velocity.y; + parsed_imu_msg->angular_velocity.z = imu_msg.imu.angular_velocity.z; + + // magnetic field message + parsed_imu_mag_msg->magnetic_field.x = imu_msg.imu.compass.x; + parsed_imu_mag_msg->magnetic_field.y = imu_msg.imu.compass.y; + parsed_imu_mag_msg->magnetic_field.z = imu_msg.imu.compass.z; + + imu_data_pub.publish(*parsed_imu_msg); + imu_mag_pub.publish(*parsed_imu_mag_msg); +} + +} // namespace vesc_imu_parser + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "vesc_imu_msg_parser_node"); + ROS_INFO("VESC IMU Message Parser Node Started"); + ros::NodeHandle n; + + vesc_imu_parser::ImuParser obj = vesc_imu_parser::ImuParser(n); + + ros::Rate loop_rate(10); + + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + + return 0; +} \ No newline at end of file From c2ac08964cbbabf3af5d559d6358e8a77f127844 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Mon, 10 Apr 2023 11:53:14 -0400 Subject: [PATCH 04/20] [IMU Filter]: added remap params to launch file | added frame_id to published message --- .gitignore | 1 + vesc_imu_msg_parser/launch/imu_parser.launch | 30 ++++++++++++++------ vesc_imu_msg_parser/src/imu_parser.cpp | 5 +++- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/.gitignore b/.gitignore index 1377554..3a25c0a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ *.swp +.vscode/ diff --git a/vesc_imu_msg_parser/launch/imu_parser.launch b/vesc_imu_msg_parser/launch/imu_parser.launch index 3a0571e..f4f9dd7 100644 --- a/vesc_imu_msg_parser/launch/imu_parser.launch +++ b/vesc_imu_msg_parser/launch/imu_parser.launch @@ -1,18 +1,32 @@ - - + + + - - + + + + + + + + + + + + - - - + + + + + + diff --git a/vesc_imu_msg_parser/src/imu_parser.cpp b/vesc_imu_msg_parser/src/imu_parser.cpp index 85443b5..639d86d 100644 --- a/vesc_imu_msg_parser/src/imu_parser.cpp +++ b/vesc_imu_msg_parser/src/imu_parser.cpp @@ -17,6 +17,9 @@ void ImuParser::imuDataCallback(const vesc_msgs::VescImuStamped& imu_msg) auto parsed_imu_msg = new sensor_msgs::Imu(); auto parsed_imu_mag_msg = new sensor_msgs::MagneticField(); + // parsed_imu_msg->header.frame_id = "imu"; + // parsed_imu_mag_msg->header.frame_id = "imu"; + // imu data message parsed_imu_mag_msg->header.stamp = ros::Time::now(); parsed_imu_msg->header.stamp = ros::Time::now(); @@ -53,7 +56,7 @@ int main(int argc, char** argv) vesc_imu_parser::ImuParser obj = vesc_imu_parser::ImuParser(n); - ros::Rate loop_rate(10); + ros::Rate loop_rate(60); while (ros::ok()) { From 5d140e799ce677f5cb46c87b8e829b7eadb620a7 Mon Sep 17 00:00:00 2001 From: Vineet <52542471+VineetTambe@users.noreply.github.com> Date: Mon, 10 Apr 2023 11:57:51 -0400 Subject: [PATCH 05/20] Update README.md --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index dec269c..2513da3 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,6 @@ # vesc -VESC 6 driver +# ROS1 VESC Package + +This is the fork of the f1tenth/vesc package - The original repop did not have updated ROS1 drivers. + +The ```noetic``` branch of this fork contains the ROS2 branch back ported to ROS1 such that it is compatible with VESC (tested on the VESC EDU) From 1817c25186983d720fe7b765aa326e07159708ce Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Mon, 10 Apr 2023 13:31:01 -0400 Subject: [PATCH 06/20] [IMU parser] updated frame ids for transforms --- vesc_imu_msg_parser/launch/imu_parser.launch | 4 ++-- vesc_imu_msg_parser/src/imu_parser.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/vesc_imu_msg_parser/launch/imu_parser.launch b/vesc_imu_msg_parser/launch/imu_parser.launch index f4f9dd7..be1ae72 100644 --- a/vesc_imu_msg_parser/launch/imu_parser.launch +++ b/vesc_imu_msg_parser/launch/imu_parser.launch @@ -20,8 +20,8 @@ - + + diff --git a/vesc_imu_msg_parser/src/imu_parser.cpp b/vesc_imu_msg_parser/src/imu_parser.cpp index 639d86d..71b648e 100644 --- a/vesc_imu_msg_parser/src/imu_parser.cpp +++ b/vesc_imu_msg_parser/src/imu_parser.cpp @@ -17,8 +17,8 @@ void ImuParser::imuDataCallback(const vesc_msgs::VescImuStamped& imu_msg) auto parsed_imu_msg = new sensor_msgs::Imu(); auto parsed_imu_mag_msg = new sensor_msgs::MagneticField(); - // parsed_imu_msg->header.frame_id = "imu"; - // parsed_imu_mag_msg->header.frame_id = "imu"; + parsed_imu_msg->header.frame_id = "imu"; + parsed_imu_mag_msg->header.frame_id = "imu"; // imu data message parsed_imu_mag_msg->header.stamp = ros::Time::now(); From c04814e3e403ee740e46abc302454c0d97701f03 Mon Sep 17 00:00:00 2001 From: Vineet <52542471+VineetTambe@users.noreply.github.com> Date: Mon, 10 Apr 2023 15:52:01 -0400 Subject: [PATCH 07/20] Update README.md --- README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 2513da3..10390ed 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ -# vesc +# Veddar VESC Interface # ROS1 VESC Package -This is the fork of the f1tenth/vesc package - The original repop did not have updated ROS1 drivers. +![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg) -The ```noetic``` branch of this fork contains the ROS2 branch back ported to ROS1 such that it is compatible with VESC (tested on the VESC EDU) +Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details + +The ```noetic``` branch contains the ROS2 branch back ported to ROS1 such that it is compatible with VESC (tested on the VESC EDU) From e13d3215a85d41fc199a82de436e62303bb02328 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Mon, 10 Apr 2023 15:56:14 -0400 Subject: [PATCH 08/20] removed car.launch, which was used for testing purposes --- car.launch | 11 ----------- 1 file changed, 11 deletions(-) delete mode 100644 car.launch diff --git a/car.launch b/car.launch deleted file mode 100644 index 9f2a1c9..0000000 --- a/car.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - From b2ae04797aa835ab37f2baf656c96d00b326d0ad Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 15:44:13 -0400 Subject: [PATCH 09/20] removed vesc_imu_msg_parser package and dependency on imu_tools --- vesc_imu_msg_parser/CMakeLists.txt | 212 ------------------ .../vesc_imu_msg_parser/imu_parser.hpp | 19 -- vesc_imu_msg_parser/launch/imu_parser.launch | 33 --- vesc_imu_msg_parser/package.xml | 70 ------ vesc_imu_msg_parser/src/imu_parser.cpp | 68 ------ 5 files changed, 402 deletions(-) delete mode 100644 vesc_imu_msg_parser/CMakeLists.txt delete mode 100644 vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp delete mode 100644 vesc_imu_msg_parser/launch/imu_parser.launch delete mode 100644 vesc_imu_msg_parser/package.xml delete mode 100644 vesc_imu_msg_parser/src/imu_parser.cpp diff --git a/vesc_imu_msg_parser/CMakeLists.txt b/vesc_imu_msg_parser/CMakeLists.txt deleted file mode 100644 index 4116645..0000000 --- a/vesc_imu_msg_parser/CMakeLists.txt +++ /dev/null @@ -1,212 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(vesc_imu_msg_parser) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - vesc_msgs - # imu_tools -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES vesc_imu_msg_parser -# CATKIN_DEPENDS imu_tools roscpp std_msgs vesc_msgs - CATKIN_DEPENDS roscpp std_msgs vesc_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/imu_parser.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(vesc_imu_msg_parser_node src/imu_parser.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(vesc_imu_msg_parser_node ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(vesc_imu_msg_parser_node - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -# ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.hpp" -# PATTERN ".svn" EXCLUDE -# ) - -# install(DIRECTORY launch/ -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - -# ## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - - - - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_vesc_imu_msg_parser.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp b/vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp deleted file mode 100644 index 3f7fc42..0000000 --- a/vesc_imu_msg_parser/include/vesc_imu_msg_parser/imu_parser.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "vesc_msgs/VescImuStamped.h" - -namespace vesc_imu_parser -{ -class ImuParser -{ -public: - ImuParser(ros::NodeHandle& nh); - -private: - ros::Publisher imu_data_pub; - ros::Publisher imu_mag_pub; - ros::Subscriber imu_data_sub; - - void imuDataCallback(const vesc_msgs::VescImuStamped& imu_msg); -}; -} // namespace vesc_imu_parser \ No newline at end of file diff --git a/vesc_imu_msg_parser/launch/imu_parser.launch b/vesc_imu_msg_parser/launch/imu_parser.launch deleted file mode 100644 index be1ae72..0000000 --- a/vesc_imu_msg_parser/launch/imu_parser.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/vesc_imu_msg_parser/package.xml b/vesc_imu_msg_parser/package.xml deleted file mode 100644 index e901559..0000000 --- a/vesc_imu_msg_parser/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - vesc_imu_msg_parser - 0.0.0 - The vesc_imu_msg_parser package - - - - - root - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - imu_tools - roscpp - std_msgs - vesc_msgs - imu_tools - roscpp - std_msgs - vesc_msgs - imu_tools - roscpp - std_msgs - vesc_msgs - - - - - - - diff --git a/vesc_imu_msg_parser/src/imu_parser.cpp b/vesc_imu_msg_parser/src/imu_parser.cpp deleted file mode 100644 index 71b648e..0000000 --- a/vesc_imu_msg_parser/src/imu_parser.cpp +++ /dev/null @@ -1,68 +0,0 @@ -#include "vesc_imu_msg_parser/imu_parser.hpp" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/MagneticField.h" -// #include "vesc_msgs/VescImuStamped.h" -namespace vesc_imu_parser -{ - -ImuParser::ImuParser(ros::NodeHandle& nh) -{ - ImuParser::imu_data_pub = nh.advertise("/imu/data_raw", 10); - ImuParser::imu_mag_pub = nh.advertise("/imu/mag", 10); - ImuParser::imu_data_sub = nh.subscribe("/sensors/imu", 10, &ImuParser::imuDataCallback, this); -} - -void ImuParser::imuDataCallback(const vesc_msgs::VescImuStamped& imu_msg) -{ - auto parsed_imu_msg = new sensor_msgs::Imu(); - auto parsed_imu_mag_msg = new sensor_msgs::MagneticField(); - - parsed_imu_msg->header.frame_id = "imu"; - parsed_imu_mag_msg->header.frame_id = "imu"; - - // imu data message - parsed_imu_mag_msg->header.stamp = ros::Time::now(); - parsed_imu_msg->header.stamp = ros::Time::now(); - - parsed_imu_msg->orientation.x = imu_msg.imu.orientation.x; - parsed_imu_msg->orientation.y = imu_msg.imu.orientation.y; - parsed_imu_msg->orientation.z = imu_msg.imu.orientation.z; - parsed_imu_msg->orientation.w = imu_msg.imu.orientation.w; - - parsed_imu_msg->linear_acceleration.x = imu_msg.imu.linear_acceleration.x; - parsed_imu_msg->linear_acceleration.y = imu_msg.imu.linear_acceleration.y; - parsed_imu_msg->linear_acceleration.z = imu_msg.imu.linear_acceleration.z; - - parsed_imu_msg->angular_velocity.x = imu_msg.imu.angular_velocity.x; - parsed_imu_msg->angular_velocity.y = imu_msg.imu.angular_velocity.y; - parsed_imu_msg->angular_velocity.z = imu_msg.imu.angular_velocity.z; - - // magnetic field message - parsed_imu_mag_msg->magnetic_field.x = imu_msg.imu.compass.x; - parsed_imu_mag_msg->magnetic_field.y = imu_msg.imu.compass.y; - parsed_imu_mag_msg->magnetic_field.z = imu_msg.imu.compass.z; - - imu_data_pub.publish(*parsed_imu_msg); - imu_mag_pub.publish(*parsed_imu_mag_msg); -} - -} // namespace vesc_imu_parser - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "vesc_imu_msg_parser_node"); - ROS_INFO("VESC IMU Message Parser Node Started"); - ros::NodeHandle n; - - vesc_imu_parser::ImuParser obj = vesc_imu_parser::ImuParser(n); - - ros::Rate loop_rate(60); - - while (ros::ok()) - { - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} \ No newline at end of file From ed42c659a13ded7d6aea11ffce2b094a9b554dbd Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 15:44:52 -0400 Subject: [PATCH 10/20] removed imu_tools from package.xml --- vesc_msgs/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index 4db6960..608d416 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -19,11 +19,9 @@ std_msgs message_generation geometry_msgs - imu_tools std_msgs message_runtime geometry_msgs - imu_tools From 5f37bb24198c6745e7a620c1ea36353f8583799b Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 16:35:25 -0400 Subject: [PATCH 11/20] restored package.xml to the version 1.1.0 --- vesc/package.xml | 20 ++++++++++++-------- vesc_ackermann/package.xml | 34 +++++++++++++++++----------------- vesc_driver/package.xml | 28 ++++++++++++++-------------- vesc_msgs/package.xml | 22 +++++++++++----------- 4 files changed, 54 insertions(+), 50 deletions(-) diff --git a/vesc/package.xml b/vesc/package.xml index 2df63f4..ad873c9 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -1,14 +1,18 @@ - + + vesc - 0.0.1 + 1.1.0 Metapackage for ROS interface to the Vedder VESC open source motor controller. - - Michael T. Boulet - Michael T. Boulet + Johannes Betz BSD + http://www.ros.org/wiki/vesc + https://github.com/f1tenth/vesc + https://github.com/f1tenth/vesc/issues + Michael T. Boulet + Joshua Whitley http://www.ros.org/wiki/vesc https://github.mit.edu/racecar/vesc @@ -16,9 +20,9 @@ catkin - vesc_driver - vesc_msgs - vesc_ackermann + vesc_driver + vesc_msgs + vesc_ackermann diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index ae97b77..c051543 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -1,18 +1,18 @@ - + + vesc_ackermann - 0.0.1 + 1.1.0 Translate between VESC messages and ROS ackermann and odometry messages. - - Michael T. Boulet - Michael T. Boulet + Johannes Betz BSD - http://www.ros.org/wiki/vesc_ackermann - https://github.mit.edu/racecar/racecar-iap - https://github.mit.edu/racecar/racecar-iap/issues + https://github.com/f1tenth/vesc + https://github.com/f1tenth/vesc/issues + Michael T. Boulet + Joshua Whitley catkin @@ -26,15 +26,15 @@ ackermann_msgs vesc_msgs - nodelet - pluginlib - roscpp - nav_msgs - std_msgs - geometry_msgs - tf - ackermann_msgs - vesc_msgs + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + geometry_msgs + tf + ackermann_msgs + vesc_msgs diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 2ed45e3..9109bff 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -1,18 +1,18 @@ - + + vesc_driver - 0.0.1 + 1.1.0 ROS device driver for the Vedder VESC open source motor driver. - - Michael T. Boulet - Michael T. Boulet + Johannes Betz BSD - http://www.ros.org/wiki/vesc_driver - https://github.mit.edu/racecar/vesc - https://github.mit.edu/racecar/vesc/issues + https://github.com/f1tenth/vesc + https://github.com/f1tenth/vesc/issues + Michael T. Boulet + Joshua Whitley catkin @@ -23,12 +23,12 @@ vesc_msgs serial - nodelet - pluginlib - roscpp - std_msgs - vesc_msgs - serial + nodelet + pluginlib + roscpp + std_msgs + vesc_msgs + serial diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index 608d416..bc2b8c2 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -1,18 +1,18 @@ - + + vesc_msgs - 0.0.1 + 1.1.0 ROS message definitions for the Vedder VESC open source motor controller. - - Michael T. Boulet - Michael T. Boulet + Johannes Betz BSD - http://ros.org/wiki/vesc_msgs - https://github.mit.edu/racecar/vesc - https://github.mit.edu/racecar/vesc/issues + https://github.com/f1tenth/vesc + https://github.com/f1tenth/vesc/issues + Michael T. Boulet + Joshua Whitley catkin @@ -20,8 +20,8 @@ message_generation geometry_msgs - std_msgs - message_runtime - geometry_msgs + std_msgs + message_runtime + geometry_msgs From 42ae666cbf3faf65f4c2e54af4ca9bb0b3a4404b Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 16:35:40 -0400 Subject: [PATCH 12/20] restored CHANGELOG.rst --- vesc/CHANGELOG.rst | 16 ++++++++++++++++ vesc_ackermann/CHANGELOG.rst | 22 ++++++++++++++++++++++ vesc_driver/CHANGELOG.rst | 27 +++++++++++++++++++++++++++ vesc_msgs/CHANGELOG.rst | 17 +++++++++++++++++ 4 files changed, 82 insertions(+) create mode 100644 vesc/CHANGELOG.rst create mode 100644 vesc_ackermann/CHANGELOG.rst create mode 100644 vesc_driver/CHANGELOG.rst create mode 100644 vesc_msgs/CHANGELOG.rst diff --git a/vesc/CHANGELOG.rst b/vesc/CHANGELOG.rst new file mode 100644 index 0000000..eb2c307 --- /dev/null +++ b/vesc/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package vesc +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2020-12-12) +------------------ +* Merge pull request `#1 `_ from f1tenth/melodic-devel + Updating for Melodic +* Updating package.xml to format 3 and setting C++ standard to 11. +* Contributors: Joshua Whitley + +1.0.0 (2020-12-02) +------------------ +* Updating maintainers, authors, and URLs. +* added onboard car +* Contributors: Joshua Whitley, billyzheng diff --git a/vesc_ackermann/CHANGELOG.rst b/vesc_ackermann/CHANGELOG.rst new file mode 100644 index 0000000..bb78e5b --- /dev/null +++ b/vesc_ackermann/CHANGELOG.rst @@ -0,0 +1,22 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package vesc_ackermann +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2020-12-12) +------------------ +* Merge pull request `#1 `_ from f1tenth/melodic-devel + Updating for Melodic +* Remove unused Boost dependency. +* Replacing boost::shared_ptr with Standard Library equivalent. +* Fixing roslint error. +* Updating package.xml to format 3 and setting C++ standard to 11. +* Contributors: Joshua Whitley + +1.0.0 (2020-12-02) +------------------ +* Applying roslint to vesc_ackerman. +* Adding roslint. +* Adding licenses. +* Updating maintainers, authors, and URLs. +* added onboard car +* Contributors: Joshua Whitley, billyzheng diff --git a/vesc_driver/CHANGELOG.rst b/vesc_driver/CHANGELOG.rst new file mode 100644 index 0000000..c505185 --- /dev/null +++ b/vesc_driver/CHANGELOG.rst @@ -0,0 +1,27 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package vesc_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2020-12-12) +------------------ +* Merge pull request `#1 `_ from f1tenth/melodic-devel + Updating for Melodic +* Exclude crc.h from roslint. +* Replacing boost::crc with CRCPP. +* Replacing boost::begin, boost::end, and boost::distance with Standard Library equivalents. +* Replacing boost::bind with Standards Library equivalent. +* Replaing boost::noncopyable with C++ equivalent. +* Replacing boost::function with Standard Library version. +* Replacing Boost smart pointers with Standard Library equivalents. +* Removing unnecessary v8stdint.h. +* Updating package.xml to format 3 and setting C++ standard to 11. +* Contributors: Joshua Whitley + +1.0.0 (2020-12-02) +------------------ +* Applying roslint and replacing registration macro with templated class. +* Adding roslint. +* Adding licenses. +* Updating maintainers, authors, and URLs. +* added onboard car +* Contributors: Joshua Whitley, billyzheng diff --git a/vesc_msgs/CHANGELOG.rst b/vesc_msgs/CHANGELOG.rst new file mode 100644 index 0000000..f639fd9 --- /dev/null +++ b/vesc_msgs/CHANGELOG.rst @@ -0,0 +1,17 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package vesc_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2020-12-12) +------------------ +* Merge pull request `#1 `_ from f1tenth/melodic-devel + Updating for Melodic +* Updating package.xml to format 3 and setting C++ standard to 11. +* Contributors: Joshua Whitley + +1.0.0 (2020-12-02) +------------------ +* Adding roslint. +* Updating maintainers, authors, and URLs. +* added onboard car +* Contributors: Joshua Whitley, billyzheng From 6f1ea6bed23051977b302edaf5a7ed286ed93710 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 16:41:53 -0400 Subject: [PATCH 13/20] reverted ackerman msgs CmakeList --- vesc_ackermann/CMakeLists.txt | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 718a9d9..166d201 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -1,29 +1,38 @@ cmake_minimum_required(VERSION 2.8.3) project(vesc_ackermann) +# Setting C++ standard to 11 +if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") + message(STATUS "Changing CXX_STANDARD from C++98 to C++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") + message(STATUS "Changing CXX_STANDARD from C++98 to C++11") + set(CMAKE_CXX_STANDARD 11) +endif() + find_package(catkin REQUIRED COMPONENTS + ackermann_msgs + geometry_msgs + nav_msgs nodelet pluginlib roscpp - nav_msgs + roslint std_msgs - geometry_msgs tf - ackermann_msgs vesc_msgs ) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS + ackermann_msgs + geometry_msgs + nav_msgs nodelet pluginlib - roscpp - nav_msgs#!/usr/bin/env python std_msgs - geometry_msgs tf - ackermann_msgs vesc_msgs ) @@ -33,7 +42,6 @@ catkin_package( include_directories( include - ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) @@ -62,6 +70,9 @@ target_link_libraries(vesc_ackermann_nodelet ${catkin_LIBRARIES} ) +set(ROSLINT_CPP_OPTS "--filter=-build/c++11") +roslint_cpp() + ############# ## Install ## ############# @@ -85,4 +96,6 @@ install(DIRECTORY launch/ ## Testing ## ############# -# TODO \ No newline at end of file +if(CATKIN_ENABLE_TESTING) + roslint_add_test() +endif() From de3c916b7a4defa1e79454aa30c45da1568899a8 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 16:44:23 -0400 Subject: [PATCH 14/20] restored ros_lint build dependency for vesc_ackermann --- vesc_ackermann/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index c051543..8cf64ab 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -16,6 +16,7 @@ catkin + roslint nodelet pluginlib roscpp From c78c44f514f285c41731acf8750a818a30734055 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 16:48:57 -0400 Subject: [PATCH 15/20] autoformatted code --- .../vesc_ackermann/ackermann_to_vesc.h | 5 +- .../include/vesc_ackermann/vesc_to_odom.h | 9 +- vesc_ackermann/src/ackermann_to_vesc.cpp | 5 +- .../src/ackermann_to_vesc_nodelet.cpp | 12 +- vesc_ackermann/src/vesc_to_odom.cpp | 49 +- vesc_ackermann/src/vesc_to_odom_nodelet.cpp | 12 +- vesc_driver/include/vesc_driver/datatypes.h | 187 ++-- vesc_driver/include/vesc_driver/v8stdint.h | 2 +- vesc_driver/include/vesc_driver/vesc_driver.h | 17 +- .../include/vesc_driver/vesc_interface.h | 31 +- vesc_driver/include/vesc_driver/vesc_packet.h | 52 +- .../include/vesc_driver/vesc_packet_factory.h | 38 +- vesc_driver/src/vesc_driver.cpp | 637 ++++++------ vesc_driver/src/vesc_driver_nodelet.cpp | 12 +- vesc_driver/src/vesc_interface.cpp | 90 +- vesc_driver/src/vesc_packet.cpp | 960 +++++++++--------- vesc_driver/src/vesc_packet_factory.cpp | 48 +- 17 files changed, 1087 insertions(+), 1079 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h index 3edb06e..5aa6e65 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h @@ -12,7 +12,6 @@ namespace vesc_ackermann class AckermannToVesc { public: - AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); private: @@ -32,6 +31,6 @@ class AckermannToVesc void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); }; -} // namespace vesc_ackermann +} // namespace vesc_ackermann -#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ +#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h index 7866a5a..fbc610b 100644 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h @@ -15,7 +15,6 @@ namespace vesc_ackermann class VescToOdom { public: - VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh); private: @@ -32,8 +31,8 @@ class VescToOdom // odometry state double x_, y_, yaw_; - std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value - vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message + std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value + vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message // ROS services ros::Publisher odom_pub_; @@ -46,6 +45,6 @@ class VescToOdom void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo); }; -} // namespace vesc_ackermann +} // namespace vesc_ackermann -#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ +#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 0a00aaa..17e846d 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -45,7 +45,8 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; // publish - if (ros::ok()) { + if (ros::ok()) + { erpm_pub_.publish(erpm_msg); servo_pub_.publish(servo_msg); } @@ -61,4 +62,4 @@ inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& val return false; } -} // namespace vesc_ackermann +} // namespace vesc_ackermann diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp index 129df4f..766db51 100644 --- a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp @@ -8,19 +8,19 @@ namespace vesc_ackermann { -class AckermannToVescNodelet: public nodelet::Nodelet +class AckermannToVescNodelet : public nodelet::Nodelet { public: - - AckermannToVescNodelet() {} + AckermannToVescNodelet() + { + } private: - virtual void onInit(void); boost::shared_ptr ackermann_to_vesc_; -}; // class AckermannToVescNodelet +}; // class AckermannToVescNodelet void AckermannToVescNodelet::onInit() { @@ -28,6 +28,6 @@ void AckermannToVescNodelet::onInit() ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); } -} // namespace vesc_ackermann +} // namespace vesc_ackermann PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index eedd4c3..cfce965 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -13,9 +13,8 @@ namespace vesc_ackermann template inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value); -VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : - odom_frame_("odom"), base_frame_("base_link"), - use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0) +VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) + : odom_frame_("odom"), base_frame_("base_link"), use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0) { // get ROS parameters private_nh.param("odom_frame", odom_frame_, odom_frame_); @@ -25,7 +24,8 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : return; if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_)) return; - if (use_servo_cmd_) { + if (use_servo_cmd_) + { if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_)) return; if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_)) @@ -39,15 +39,16 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : odom_pub_ = nh.advertise("odom", 10); // create tf broadcaster - if (publish_tf_) { + if (publish_tf_) + { tf_pub_.reset(new tf::TransformBroadcaster); } // subscribe to vesc state and. optionally, servo command vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this); - if (use_servo_cmd_) { - servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, - &VescToOdom::servoCmdCallback, this); + if (use_servo_cmd_) + { + servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, &VescToOdom::servoCmdCallback, this); } } @@ -58,14 +59,15 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& return; // convert to engineering units - double current_speed = ( -state->state.speed - speed_to_erpm_offset_ ) / speed_to_erpm_gain_; - if (std::fabs(current_speed) < 0.05){ + double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; + if (std::fabs(current_speed) < 0.05) + { current_speed = 0.0; } double current_steering_angle(0.0), current_angular_velocity(0.0); - if (use_servo_cmd_) { - current_steering_angle = - ( last_servo_cmd_->data - steering_to_servo_offset_ ) / steering_to_servo_gain_; + if (use_servo_cmd_) + { + current_steering_angle = (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_; current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_; } @@ -100,14 +102,14 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& odom->pose.pose.position.y = y_; odom->pose.pose.orientation.x = 0.0; odom->pose.pose.orientation.y = 0.0; - odom->pose.pose.orientation.z = sin(yaw_/2.0); - odom->pose.pose.orientation.w = cos(yaw_/2.0); + odom->pose.pose.orientation.z = sin(yaw_ / 2.0); + odom->pose.pose.orientation.w = cos(yaw_ / 2.0); // Position uncertainty /** @todo Think about position uncertainty, perhaps get from parameters? */ - odom->pose.covariance[0] = 0.2; ///< x - odom->pose.covariance[7] = 0.2; ///< y - odom->pose.covariance[35] = 0.4; ///< yaw + odom->pose.covariance[0] = 0.2; ///< x + odom->pose.covariance[7] = 0.2; ///< y + odom->pose.covariance[35] = 0.4; ///< yaw // Velocity ("in the coordinate frame given by the child_frame_id") odom->twist.twist.linear.x = current_speed; @@ -117,7 +119,8 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& // Velocity uncertainty /** @todo Think about velocity uncertainty */ - if (publish_tf_) { + if (publish_tf_) + { geometry_msgs::TransformStamped tf; tf.header.frame_id = odom_frame_; tf.child_frame_id = base_frame_; @@ -126,12 +129,14 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& tf.transform.translation.y = y_; tf.transform.translation.z = 0.0; tf.transform.rotation = odom->pose.pose.orientation; - if (ros::ok()) { + if (ros::ok()) + { tf_pub_->sendTransform(tf); } } - if (ros::ok()) { + if (ros::ok()) + { odom_pub_.publish(odom); } } @@ -151,4 +156,4 @@ inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& val return false; } -} // namespace vesc_ackermann +} // namespace vesc_ackermann diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp index 3fc37e4..2a7bd0e 100644 --- a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp +++ b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp @@ -8,19 +8,19 @@ namespace vesc_ackermann { -class VescToOdomNodelet: public nodelet::Nodelet +class VescToOdomNodelet : public nodelet::Nodelet { public: - - VescToOdomNodelet() {} + VescToOdomNodelet() + { + } private: - virtual void onInit(void); boost::shared_ptr vesc_to_odom_; -}; // class VescToOdomNodelet +}; // class VescToOdomNodelet void VescToOdomNodelet::onInit() { @@ -28,6 +28,6 @@ void VescToOdomNodelet::onInit() vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); } -} // namespace vesc_ackermann +} // namespace vesc_ackermann PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.h index 994920d..616dcb7 100644 --- a/vesc_driver/include/vesc_driver/datatypes.h +++ b/vesc_driver/include/vesc_driver/datatypes.h @@ -20,94 +20,97 @@ #ifndef DATATYPES_H #define DATATYPES_H -//#include -//#include -//#include -//#include +// #include +// #include +// #include +// #include #include -typedef struct { - bool isVesc; +typedef struct +{ + bool isVesc; } VSerialInfo_t; -typedef enum { - CFG_T_UNDEFINED = 0, - CFG_T_DOUBLE, - CFG_T_INT, - CFG_T_QSTRING, - CFG_T_ENUM, - CFG_T_BOOL +typedef enum +{ + CFG_T_UNDEFINED = 0, + CFG_T_DOUBLE, + CFG_T_INT, + CFG_T_QSTRING, + CFG_T_ENUM, + CFG_T_BOOL } CFG_T; -typedef enum { - VESC_TX_UNDEFINED = 0, - VESC_TX_UINT8, - VESC_TX_INT8, - VESC_TX_UINT16, - VESC_TX_INT16, - VESC_TX_UINT32, - VESC_TX_INT32, - VESC_TX_DOUBLE16, - VESC_TX_DOUBLE32, - VESC_TX_DOUBLE32_AUTO +typedef enum +{ + VESC_TX_UNDEFINED = 0, + VESC_TX_UINT8, + VESC_TX_INT8, + VESC_TX_UINT16, + VESC_TX_INT16, + VESC_TX_UINT32, + VESC_TX_INT32, + VESC_TX_DOUBLE16, + VESC_TX_DOUBLE32, + VESC_TX_DOUBLE32_AUTO } VESC_TX_T; -typedef enum { - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR +typedef enum +{ + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR } mc_fault_code; -typedef enum { - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR +typedef enum +{ + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR } disp_pos_mode; -struct MC_VALUES { - +struct MC_VALUES +{ public: - double v_in; - double temp_mos; - double temp_motor; - double current_motor; - double current_in; - double id; - double iq; - double rpm; - double duty_now; - double amp_hours; - double amp_hours_charged; - double watt_hours; - double watt_hours_charged; - int tachometer; - int tachometer_abs; - double position; - mc_fault_code fault_code; + double v_in; + double temp_mos; + double temp_motor; + double current_motor; + double current_in; + double id; + double iq; + double rpm; + double duty_now; + double amp_hours; + double amp_hours_charged; + double watt_hours; + double watt_hours_charged; + int tachometer; + int tachometer_abs; + double position; + mc_fault_code fault_code; }; - - -typedef enum { - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES +typedef enum +{ + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES } debug_sampling_mode; - // Communication commands typedef enum { @@ -204,7 +207,6 @@ typedef enum COMM_GET_IMU_CALIBRATION } COMM_PACKET_ID; - // typedef enum { // COMM_FW_VERSION = 0, // COMM_JUMP_TO_BOOTLOADER, @@ -247,31 +249,30 @@ typedef enum // COMM_GET_IMU_DATA // } COMM_PACKET_ID; -typedef struct { - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; +typedef struct +{ + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; } chuck_data; -struct bldc_detect { - - +struct bldc_detect +{ public: - double cycle_int_limit; - double bemf_coupling_k; - int hall_res; + double cycle_int_limit; + double bemf_coupling_k; + int hall_res; }; - - -typedef enum { - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL +typedef enum +{ + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL } NRF_PAIR_RES; -#endif // DATATYPES_H +#endif // DATATYPES_H diff --git a/vesc_driver/include/vesc_driver/v8stdint.h b/vesc_driver/include/vesc_driver/v8stdint.h index f3be96b..082e1ec 100644 --- a/vesc_driver/include/vesc_driver/v8stdint.h +++ b/vesc_driver/include/vesc_driver/v8stdint.h @@ -40,7 +40,7 @@ typedef signed char int8_t; typedef unsigned char uint8_t; -typedef short int16_t; // NOLINT +typedef short int16_t; // NOLINT typedef unsigned short uint16_t; // NOLINT typedef int int32_t; typedef unsigned int uint32_t; diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h index 8ab1931..b723cbb 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ b/vesc_driver/include/vesc_driver/vesc_driver.h @@ -18,9 +18,7 @@ namespace vesc_driver class VescDriver { public: - - VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh); + VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); private: // interface to the VESC @@ -61,15 +59,16 @@ class VescDriver ros::Timer timer_; // driver modes (possible states) - typedef enum { + typedef enum + { MODE_INITIALIZING, MODE_OPERATING } driver_mode_t; // other variables - driver_mode_t driver_mode_; ///< driver state machine mode (state) - int fw_version_major_; ///< firmware major version reported by vesc - int fw_version_minor_; ///< firmware minor version reported by vesc + driver_mode_t driver_mode_; ///< driver state machine mode (state) + int fw_version_major_; ///< firmware major version reported by vesc + int fw_version_minor_; ///< firmware minor version reported by vesc // ROS callbacks void timerCallback(const ros::TimerEvent& event); @@ -81,6 +80,6 @@ class VescDriver void servoCallback(const std_msgs::Float64::ConstPtr& servo); }; -} // namespace vesc_driver +} // namespace vesc_driver -#endif // VESC_DRIVER_VESC_DRIVER_H_ +#endif // VESC_DRIVER_VESC_DRIVER_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.h index b2063e0..1ea1e91 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.h @@ -24,9 +24,8 @@ namespace vesc_driver class VescInterface : private boost::noncopyable { public: - - typedef boost::function PacketHandlerFunction; - typedef boost::function ErrorHandlerFunction; + typedef boost::function PacketHandlerFunction; + typedef boost::function ErrorHandlerFunction; /** * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not @@ -94,7 +93,6 @@ class VescInterface : private boost::noncopyable void requestImuData(); - private: // Pimpl - hide serial port members from class users class Impl; @@ -107,19 +105,26 @@ class SerialException : public std::exception // Disable copy constructors SerialException& operator=(const SerialException&); std::string e_what_; + public: - SerialException (const char *description) { - std::stringstream ss; - ss << "SerialException " << description << " failed."; - e_what_ = ss.str(); + SerialException(const char* description) + { + std::stringstream ss; + ss << "SerialException " << description << " failed."; + e_what_ = ss.str(); + } + SerialException(const SerialException& other) : e_what_(other.e_what_) + { + } + virtual ~SerialException() throw() + { } - SerialException (const SerialException& other) : e_what_(other.e_what_) {} - virtual ~SerialException() throw() {} - virtual const char* what () const throw () { + virtual const char* what() const throw() + { return e_what_.c_str(); } }; -} // namespace vesc_driver +} // namespace vesc_driver -#endif // VESC_DRIVER_VESC_INTERFACE_H_ +#endif // VESC_DRIVER_VESC_INTERFACE_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index 92d9207..d4e4ba0 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -25,28 +25,33 @@ typedef std::pair BufferRangeCon class VescFrame { public: - virtual ~VescFrame() {} + virtual ~VescFrame() + { + } // getters - virtual const Buffer& frame() const {return *frame_;} + virtual const Buffer& frame() const + { + return *frame_; + } // VESC packet properties - static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes - static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes - static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes - static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value - static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value - static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value + static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes + static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes + static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes + static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value + static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value + static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value /** CRC parameters for the VESC */ typedef boost::crc_optimal<16, 0x1021, 0, 0, false, false> CRC; - protected: +protected: /** Construct frame with specified payload size. */ VescFrame(int payload_size); - boost::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy - BufferRange payload_; ///< View into frame's payload section + boost::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy + BufferRange payload_; ///< View into frame's payload section private: /** Construct from buffer. Used by VescPacketFactory factory. */ @@ -62,9 +67,14 @@ class VescFrame class VescPacket : public VescFrame { public: - virtual ~VescPacket() {} + virtual ~VescPacket() + { + } - virtual const std::string& name() const {return name_;} + virtual const std::string& name() const + { + return name_; + } protected: VescPacket(const std::string& name, int payload_size, int payload_id); @@ -86,14 +96,12 @@ class VescPacketFWVersion : public VescPacket int fwMajor() const; int fwMinor() const; - }; class VescPacketRequestFWVersion : public VescPacket { public: VescPacketRequestFWVersion(); - }; /*------------------------------------------------------------------------------------------------*/ @@ -122,7 +130,6 @@ class VescPacketValues : public VescPacket double tachometer() const; double tachometer_abs() const; int fault_code() const; - }; class VescPacketRequestValues : public VescPacket @@ -203,11 +210,11 @@ class VescPacketImu : public VescPacket public: explicit VescPacketImu(boost::shared_ptr raw); - int mask() const; + int mask() const; - double yaw() const; + double yaw() const; double pitch() const; - double roll() const; + double roll() const; double acc_x() const; double acc_y() const; @@ -227,7 +234,7 @@ class VescPacketImu : public VescPacket double q_z() const; private: - double getFloat32Auto(uint32_t * pos) const; + double getFloat32Auto(uint32_t* pos) const; uint32_t mask_; double roll_; @@ -254,7 +261,6 @@ class VescPacketImu : public VescPacket /*------------------------------------------------------------------------------------------------*/ +} // namespace vesc_driver -} // namespace vesc_driver - -#endif // VESC_DRIVER_VESC_PACKET_H_ +#endif // VESC_DRIVER_VESC_PACKET_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/vesc_driver/include/vesc_driver/vesc_packet_factory.h index f856f44..d05e796 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.h +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.h @@ -46,8 +46,7 @@ class VescPacketFactory : private boost::noncopyable * * @return Pointer to a valid VescPacket if successful. Otherwise, an empty pointer. */ - static VescPacketPtr createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, + static VescPacketPtr createPacket(const Buffer::const_iterator& begin, const Buffer::const_iterator& end, int* num_bytes_needed, std::string* what); typedef boost::function)> CreateFn; @@ -56,27 +55,26 @@ class VescPacketFactory : private boost::noncopyable static void registerPacketType(int payload_id, CreateFn fn); private: - - typedef std::map FactoryMap; + typedef std::map FactoryMap; static FactoryMap* getMap(); }; /** Use this macro to register packets */ -#define REGISTER_PACKET_TYPE(id, klass) \ -class klass##Factory \ -{ \ -public: \ - klass##Factory() \ - { \ - VescPacketFactory::registerPacketType((id), &klass##Factory::create); \ - } \ - static VescPacketPtr create(boost::shared_ptr frame) \ - { \ - return VescPacketPtr(new klass(frame)); \ - } \ -}; \ -static klass##Factory global_##klass##Factory; +#define REGISTER_PACKET_TYPE(id, klass) \ + class klass##Factory \ + { \ + public: \ + klass##Factory() \ + { \ + VescPacketFactory::registerPacketType((id), &klass##Factory::create); \ + } \ + static VescPacketPtr create(boost::shared_ptr frame) \ + { \ + return VescPacketPtr(new klass(frame)); \ + } \ + }; \ + static klass##Factory global_##klass##Factory; -} // namespace vesc_driver +} // namespace vesc_driver -#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ +#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index fa2c864..9695a57 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -13,382 +13,383 @@ namespace vesc_driver { - VescDriver::VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh) : vesc_(std::string(), - boost::bind(&VescDriver::vescPacketCallback, this, _1), - boost::bind(&VescDriver::vescErrorCallback, this, _1)), - duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"), - brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), - position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), - driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) +VescDriver::VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) + : vesc_(std::string(), boost::bind(&VescDriver::vescPacketCallback, this, _1), + boost::bind(&VescDriver::vescErrorCallback, this, _1)) + , duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0) + , current_limit_(private_nh, "current") + , brake_limit_(private_nh, "brake") + , speed_limit_(private_nh, "speed") + , position_limit_(private_nh, "position") + , servo_limit_(private_nh, "servo", 0.0, 1.0) + , driver_mode_(MODE_INITIALIZING) + , fw_version_major_(-1) + , fw_version_minor_(-1) +{ + // get vesc serial port address + std::string port; + if (!private_nh.getParam("port", port)) { - // get vesc serial port address - std::string port; - if (!private_nh.getParam("port", port)) - { - ROS_FATAL("VESC communication port parameter required."); - ros::shutdown(); - return; - } - - // attempt to connect to the serial port - try - { - vesc_.connect(port); - } - catch (SerialException e) - { - ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); - ros::shutdown(); - return; - } + ROS_FATAL("VESC communication port parameter required."); + ros::shutdown(); + return; + } - // create vesc state (telemetry) publisher - state_pub_ = nh.advertise("sensors/core", 10); - imu_pub_ = nh.advertise("sensors/imu", 10); - // imu_std_pub_ = nh.advertise("sensors/imu/raw", 10); - - // since vesc state does not include the servo position, publish the commanded - // servo position as a "sensor" - servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); - - // subscribe to motor and servo command topics - duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, - &VescDriver::dutyCycleCallback, this); - current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); - brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); - speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); - position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); - servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); - - // create a 50Hz timer, used for state machine & polling VESC telemetry - timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); + // attempt to connect to the serial port + try + { + vesc_.connect(port); + } + catch (SerialException e) + { + ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); + ros::shutdown(); + return; } - /* TODO or TO-THINKABOUT LIST - - what should we do on startup? send brake or zero command? - - what to do if the vesc interface gives an error? - - check version number against know compatable? - - should we wait until we receive telemetry before sending commands? - - should we track the last motor command - - what to do if no motor command received recently? - - what to do if no servo command received recently? - - what is the motor safe off state (0 current?) - - what to do if a command parameter is out of range, ignore? - - try to predict vesc bounds (from vesc config) and command detect bounds errors - */ - - void VescDriver::timerCallback(const ros::TimerEvent &event) + // create vesc state (telemetry) publisher + state_pub_ = nh.advertise("sensors/core", 10); + imu_pub_ = nh.advertise("sensors/imu", 10); + // imu_std_pub_ = nh.advertise("sensors/imu/raw", 10); + + // since vesc state does not include the servo position, publish the commanded + // servo position as a "sensor" + servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); + + // subscribe to motor and servo command topics + duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, &VescDriver::dutyCycleCallback, this); + current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); + brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); + speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); + position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); + servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); + + // create a 50Hz timer, used for state machine & polling VESC telemetry + timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); +} + +/* TODO or TO-THINKABOUT LIST + - what should we do on startup? send brake or zero command? + - what to do if the vesc interface gives an error? + - check version number against know compatable? + - should we wait until we receive telemetry before sending commands? + - should we track the last motor command + - what to do if no motor command received recently? + - what to do if no servo command received recently? + - what is the motor safe off state (0 current?) + - what to do if a command parameter is out of range, ignore? + - try to predict vesc bounds (from vesc config) and command detect bounds errors +*/ + +void VescDriver::timerCallback(const ros::TimerEvent& event) +{ + // VESC interface should not unexpectedly disconnect, but test for it anyway + if (!vesc_.isConnected()) { - // VESC interface should not unexpectedly disconnect, but test for it anyway - if (!vesc_.isConnected()) - { - ROS_FATAL("Unexpectedly disconnected from serial port."); - timer_.stop(); - ros::shutdown(); - return; - } + ROS_FATAL("Unexpectedly disconnected from serial port."); + timer_.stop(); + ros::shutdown(); + return; + } - /* - * Driver state machine, modes: - * INITIALIZING - request and wait for vesc version - * OPERATING - receiving commands from subscriber topics - */ - if (driver_mode_ == MODE_INITIALIZING) - { - // ROS_INFO_STREAM("driver_mode = MODE_INITIALIZING"); - // request version number, return packet will update the internal version numbers - vesc_.requestFWVersion(); - if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) - { - ROS_INFO("Connected to VESC with firmware version %d.%d", - fw_version_major_, fw_version_minor_); - driver_mode_ = MODE_OPERATING; - } - } - else if (driver_mode_ == MODE_OPERATING) - { - // ROS_INFO_STREAM("driver_mode = MODE_OPERATING"); - // poll for vesc state (telemetry) - vesc_.requestState(); - // poll for vesc imu - vesc_.requestImuData(); - } - else + /* + * Driver state machine, modes: + * INITIALIZING - request and wait for vesc version + * OPERATING - receiving commands from subscriber topics + */ + if (driver_mode_ == MODE_INITIALIZING) + { + // ROS_INFO_STREAM("driver_mode = MODE_INITIALIZING"); + // request version number, return packet will update the internal version numbers + vesc_.requestFWVersion(); + if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) { - // unknown mode, how did that happen? - assert(false && "unknown driver mode"); + ROS_INFO("Connected to VESC with firmware version %d.%d", fw_version_major_, fw_version_minor_); + driver_mode_ = MODE_OPERATING; } } - - void VescDriver::vescPacketCallback(const boost::shared_ptr &packet) + else if (driver_mode_ == MODE_OPERATING) { + // ROS_INFO_STREAM("driver_mode = MODE_OPERATING"); + // poll for vesc state (telemetry) + vesc_.requestState(); + // poll for vesc imu + vesc_.requestImuData(); + } + else + { + // unknown mode, how did that happen? + assert(false && "unknown driver mode"); + } +} - // ROS_INFO_STREAM(packet->name()); - - if (packet->name() == "Values") - { - boost::shared_ptr values = - boost::dynamic_pointer_cast(packet); - - vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); - state_msg->header.stamp = ros::Time::now(); - state_msg->state.voltage_input = values->v_in(); - state_msg->state.temperature_pcb = values->temp_pcb(); - state_msg->state.current_motor = values->current_motor(); - state_msg->state.current_input = values->current_in(); - state_msg->state.speed = values->rpm(); - state_msg->state.duty_cycle = values->duty_now(); - state_msg->state.charge_drawn = values->amp_hours(); - state_msg->state.charge_regen = values->amp_hours_charged(); - state_msg->state.energy_drawn = values->watt_hours(); - state_msg->state.energy_regen = values->watt_hours_charged(); - state_msg->state.displacement = values->tachometer(); - state_msg->state.distance_traveled = values->tachometer_abs(); - state_msg->state.fault_code = values->fault_code(); - - state_pub_.publish(state_msg); - } - else if (packet->name() == "FWVersion") - { - boost::shared_ptr fw_version = - boost::dynamic_pointer_cast(packet); - // todo: might need lock here - fw_version_major_ = fw_version->fwMajor(); - fw_version_minor_ = fw_version->fwMinor(); - } - else if (packet->name() == "ImuData") - { +void VescDriver::vescPacketCallback(const boost::shared_ptr& packet) +{ + // ROS_INFO_STREAM(packet->name()); - boost::shared_ptr imuData = - boost::dynamic_pointer_cast(packet); + if (packet->name() == "Values") + { + boost::shared_ptr values = boost::dynamic_pointer_cast(packet); + + vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); + state_msg->header.stamp = ros::Time::now(); + state_msg->state.voltage_input = values->v_in(); + state_msg->state.temperature_pcb = values->temp_pcb(); + state_msg->state.current_motor = values->current_motor(); + state_msg->state.current_input = values->current_in(); + state_msg->state.speed = values->rpm(); + state_msg->state.duty_cycle = values->duty_now(); + state_msg->state.charge_drawn = values->amp_hours(); + state_msg->state.charge_regen = values->amp_hours_charged(); + state_msg->state.energy_drawn = values->watt_hours(); + state_msg->state.energy_regen = values->watt_hours_charged(); + state_msg->state.displacement = values->tachometer(); + state_msg->state.distance_traveled = values->tachometer_abs(); + state_msg->state.fault_code = values->fault_code(); + + state_pub_.publish(state_msg); + } + else if (packet->name() == "FWVersion") + { + boost::shared_ptr fw_version = + boost::dynamic_pointer_cast(packet); + // todo: might need lock here + fw_version_major_ = fw_version->fwMajor(); + fw_version_minor_ = fw_version->fwMinor(); + } + else if (packet->name() == "ImuData") + { + boost::shared_ptr imuData = boost::dynamic_pointer_cast(packet); - // vesc_msgs::VescImuStamped::Ptr imu_msg(new vesc_msgs::VescImuStamped); - // vesc_msgs::VescIMUStamped::Ptr std_imu_msg(new vesc_msgs::VescIMUStamped); + // vesc_msgs::VescImuStamped::Ptr imu_msg(new vesc_msgs::VescImuStamped); + // vesc_msgs::VescIMUStamped::Ptr std_imu_msg(new vesc_msgs::VescIMUStamped); - auto imu_msg = new vesc_msgs::VescImuStamped(); - // auto std_imu_msg = Imu(); + auto imu_msg = new vesc_msgs::VescImuStamped(); + // auto std_imu_msg = Imu(); - imu_msg->header.stamp = ros::Time::now(); - // std_imu_msg.header.stamp = ros::Time::now(); + imu_msg->header.stamp = ros::Time::now(); + // std_imu_msg.header.stamp = ros::Time::now(); - imu_msg->imu.ypr.x = imuData->roll(); - imu_msg->imu.ypr.y = imuData->pitch(); - imu_msg->imu.ypr.z = imuData->yaw(); + imu_msg->imu.ypr.x = imuData->roll(); + imu_msg->imu.ypr.y = imuData->pitch(); + imu_msg->imu.ypr.z = imuData->yaw(); - imu_msg->imu.linear_acceleration.x = imuData->acc_x(); - imu_msg->imu.linear_acceleration.y = imuData->acc_y(); - imu_msg->imu.linear_acceleration.z = imuData->acc_z(); + imu_msg->imu.linear_acceleration.x = imuData->acc_x(); + imu_msg->imu.linear_acceleration.y = imuData->acc_y(); + imu_msg->imu.linear_acceleration.z = imuData->acc_z(); - imu_msg->imu.angular_velocity.x = imuData->gyr_x(); - imu_msg->imu.angular_velocity.y = imuData->gyr_y(); - imu_msg->imu.angular_velocity.z = imuData->gyr_z(); + imu_msg->imu.angular_velocity.x = imuData->gyr_x(); + imu_msg->imu.angular_velocity.y = imuData->gyr_y(); + imu_msg->imu.angular_velocity.z = imuData->gyr_z(); - imu_msg->imu.compass.x = imuData->mag_x(); - imu_msg->imu.compass.y = imuData->mag_y(); - imu_msg->imu.compass.z = imuData->mag_z(); + imu_msg->imu.compass.x = imuData->mag_x(); + imu_msg->imu.compass.y = imuData->mag_y(); + imu_msg->imu.compass.z = imuData->mag_z(); - imu_msg->imu.orientation.w = imuData->q_w(); - imu_msg->imu.orientation.x = imuData->q_x(); - imu_msg->imu.orientation.y = imuData->q_y(); - imu_msg->imu.orientation.z = imuData->q_z(); + imu_msg->imu.orientation.w = imuData->q_w(); + imu_msg->imu.orientation.x = imuData->q_x(); + imu_msg->imu.orientation.y = imuData->q_y(); + imu_msg->imu.orientation.z = imuData->q_z(); - // std_imu_msg.linear_acceleration.x = imuData->acc_x(); - // std_imu_msg.linear_acceleration.y = imuData->acc_y(); - // std_imu_msg.linear_acceleration.z = imuData->acc_z(); + // std_imu_msg.linear_acceleration.x = imuData->acc_x(); + // std_imu_msg.linear_acceleration.y = imuData->acc_y(); + // std_imu_msg.linear_acceleration.z = imuData->acc_z(); - // std_imu_msg.angular_velocity.x = imuData->gyr_x(); - // std_imu_msg.angular_velocity.y = imuData->gyr_y(); - // std_imu_msg.angular_velocity.z = imuData->gyr_z(); + // std_imu_msg.angular_velocity.x = imuData->gyr_x(); + // std_imu_msg.angular_velocity.y = imuData->gyr_y(); + // std_imu_msg.angular_velocity.z = imuData->gyr_z(); - // std_imu_msg.orientation.w = imuData->q_w(); - // std_imu_msg.orientation.x = imuData->q_x(); - // std_imu_msg.orientation.y = imuData->q_y(); - // std_imu_msg.orientation.z = imuData->q_z(); + // std_imu_msg.orientation.w = imuData->q_w(); + // std_imu_msg.orientation.x = imuData->q_x(); + // std_imu_msg.orientation.y = imuData->q_y(); + // std_imu_msg.orientation.z = imuData->q_z(); - imu_pub_.publish(*imu_msg); - // imu_std_pub_.publish(std_imu_msg); - } + imu_pub_.publish(*imu_msg); + // imu_std_pub_.publish(std_imu_msg); } +} - void VescDriver::vescErrorCallback(const std::string &error) +void VescDriver::vescErrorCallback(const std::string& error) +{ + ROS_ERROR("%s", error.c_str()); +} + +/** + * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However, + * note that the VESC may impose a more restrictive bounds on the range depending + * on its configuration, e.g. absolute value is between 0.05 and 0.95. + */ +void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) +{ + if (driver_mode_ = MODE_OPERATING) { - ROS_ERROR("%s", error.c_str()); + vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } - - /** - * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However, - * note that the VESC may impose a more restrictive bounds on the range depending - * on its configuration, e.g. absolute value is between 0.05 and 0.95. - */ - void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr &duty_cycle) +} + +/** + * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However, + * note that the VESC may impose a more restrictive bounds on the range depending on + * its configuration. + */ +void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) +{ + if (driver_mode_ = MODE_OPERATING) { - if (driver_mode_ = MODE_OPERATING) - { - vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); - } + vesc_.setCurrent(current_limit_.clip(current->data)); } - - /** - * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However, - * note that the VESC may impose a more restrictive bounds on the range depending on - * its configuration. - */ - void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr ¤t) +} + +/** + * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver. + * However, note that the VESC may impose a more restrictive bounds on the range + * depending on its configuration. + */ +void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) +{ + if (driver_mode_ = MODE_OPERATING) { - if (driver_mode_ = MODE_OPERATING) - { - vesc_.setCurrent(current_limit_.clip(current->data)); - } + vesc_.setBrake(brake_limit_.clip(brake->data)); } - - /** - * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver. - * However, note that the VESC may impose a more restrictive bounds on the range - * depending on its configuration. - */ - void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr &brake) +} + +/** + * @param speed Commanded VESC speed in electrical RPM. Electrical RPM is the mechanical RPM + * multiplied by the number of motor poles. Any value is accepted by this + * driver. However, note that the VESC may impose a more restrictive bounds on the + * range depending on its configuration. + */ +void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) +{ + if (driver_mode_ = MODE_OPERATING) { - if (driver_mode_ = MODE_OPERATING) - { - vesc_.setBrake(brake_limit_.clip(brake->data)); - } + vesc_.setSpeed(speed_limit_.clip(speed->data)); } +} - /** - * @param speed Commanded VESC speed in electrical RPM. Electrical RPM is the mechanical RPM - * multiplied by the number of motor poles. Any value is accepted by this - * driver. However, note that the VESC may impose a more restrictive bounds on the - * range depending on its configuration. - */ - void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr &speed) +/** + * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. + * Note that the VESC must be in encoder mode for this command to have an effect. + */ +void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) +{ + if (driver_mode_ = MODE_OPERATING) { - if (driver_mode_ = MODE_OPERATING) - { - vesc_.setSpeed(speed_limit_.clip(speed->data)); - } + // ROS uses radians but VESC seems to use degrees. Convert to degrees. + double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; + vesc_.setPosition(position_deg); } +} - /** - * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. - * Note that the VESC must be in encoder mode for this command to have an effect. - */ - void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr &position) +/** + * @param servo Commanded VESC servo output position. Valid range is 0 to 1. + */ +void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) +{ + if (driver_mode_ = MODE_OPERATING) { - if (driver_mode_ = MODE_OPERATING) - { - // ROS uses radians but VESC seems to use degrees. Convert to degrees. - double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; - vesc_.setPosition(position_deg); - } + double servo_clipped(servo_limit_.clip(servo->data)); + vesc_.setServo(servo_clipped); + // publish clipped servo value as a "sensor" + std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); + servo_sensor_msg->data = servo_clipped; + servo_sensor_pub_.publish(servo_sensor_msg); } +} - /** - * @param servo Commanded VESC servo output position. Valid range is 0 to 1. - */ - void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr &servo) +VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, + const boost::optional& min_lower, + const boost::optional& max_upper) + : name(str) +{ + // check if user's minimum value is outside of the range min_lower to max_upper + double param_min; + if (nh.getParam(name + "_min", param_min)) { - if (driver_mode_ = MODE_OPERATING) + if (min_lower && param_min < *min_lower) { - double servo_clipped(servo_limit_.clip(servo->data)); - vesc_.setServo(servo_clipped); - // publish clipped servo value as a "sensor" - std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); - servo_sensor_msg->data = servo_clipped; - servo_sensor_pub_.publish(servo_sensor_msg); + lower = *min_lower; + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is less than the feasible minimum (" + << *min_lower << ")."); } - } - - VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle &nh, const std::string &str, - const boost::optional &min_lower, - const boost::optional &max_upper) : name(str) - { - // check if user's minimum value is outside of the range min_lower to max_upper - double param_min; - if (nh.getParam(name + "_min", param_min)) + else if (max_upper && param_min > *max_upper) { - if (min_lower && param_min < *min_lower) - { - lower = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is less than the feasible minimum (" << *min_lower << ")."); - } - else if (max_upper && param_min > *max_upper) - { - lower = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is greater than the feasible maximum (" << *max_upper << ")."); - } - else - { - lower = param_min; - } + lower = *max_upper; + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is greater than the feasible maximum (" + << *max_upper << ")."); } - else if (min_lower) + else { - lower = *min_lower; + lower = param_min; } + } + else if (min_lower) + { + lower = *min_lower; + } - // check if the uers' maximum value is outside of the range min_lower to max_upper - double param_max; - if (nh.getParam(name + "_max", param_max)) + // check if the uers' maximum value is outside of the range min_lower to max_upper + double param_max; + if (nh.getParam(name + "_max", param_max)) + { + if (min_lower && param_max < *min_lower) { - if (min_lower && param_max < *min_lower) - { - upper = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is less than the feasible minimum (" << *min_lower << ")."); - } - else if (max_upper && param_max > *max_upper) - { - upper = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is greater than the feasible maximum (" << *max_upper << ")."); - } - else - { - upper = param_max; - } + upper = *min_lower; + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is less than the feasible minimum (" + << *min_lower << ")."); } - else if (max_upper) + else if (max_upper && param_max > *max_upper) { upper = *max_upper; + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is greater than the feasible maximum (" + << *max_upper << ")."); } - - // check for min > max - if (upper && lower && *lower > *upper) + else { - ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper - << ") is less than parameter " << name << "_min (" << *lower << ")."); - double temp(*lower); - lower = *upper; - upper = temp; + upper = param_max; } + } + else if (max_upper) + { + upper = *max_upper; + } - std::ostringstream oss; - oss << " " << name << " limit: "; - if (lower) - oss << *lower << " "; - else - oss << "(none) "; - if (upper) - oss << *upper; - else - oss << "(none)"; - ROS_DEBUG_STREAM(oss.str()); + // check for min > max + if (upper && lower && *lower > *upper) + { + ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper << ") is less than parameter " << name << "_min (" + << *lower << ")."); + double temp(*lower); + lower = *upper; + upper = temp; } - double VescDriver::CommandLimit::clip(double value) - { // check - if (lower && value < lower) - { - ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", - name.c_str(), value, *lower); - return *lower; - } - if (upper && value > upper) - { - ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", - name.c_str(), value, *upper); - return *upper; - } - return value; + std::ostringstream oss; + oss << " " << name << " limit: "; + if (lower) + oss << *lower << " "; + else + oss << "(none) "; + if (upper) + oss << *upper; + else + oss << "(none)"; + ROS_DEBUG_STREAM(oss.str()); +} + +double VescDriver::CommandLimit::clip(double value) +{ // check + if (lower && value < lower) + { + ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", name.c_str(), value, *lower); + return *lower; + } + if (upper && value > upper) + { + ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", name.c_str(), value, *upper); + return *upper; } + return value; +} -} // namespace vesc_driver +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_driver_nodelet.cpp b/vesc_driver/src/vesc_driver_nodelet.cpp index 78c8d3b..59ef279 100644 --- a/vesc_driver/src/vesc_driver_nodelet.cpp +++ b/vesc_driver/src/vesc_driver_nodelet.cpp @@ -8,19 +8,19 @@ namespace vesc_driver { -class VescDriverNodelet: public nodelet::Nodelet +class VescDriverNodelet : public nodelet::Nodelet { public: - - VescDriverNodelet() {} + VescDriverNodelet() + { + } private: - virtual void onInit(void); boost::shared_ptr vesc_driver_; -}; // class VescDriverNodelet +}; // class VescDriverNodelet void VescDriverNodelet::onInit() { @@ -28,6 +28,6 @@ void VescDriverNodelet::onInit() vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); } -} // namespace vesc_driver +} // namespace vesc_driver PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 29920e8..57536ea 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -20,14 +20,15 @@ namespace vesc_driver class VescInterface::Impl { public: - Impl() : - serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100), - serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none) - {} + Impl() + : serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100), serial::eightbits, serial::parity_none, + serial::stopbits_one, serial::flowcontrol_none) + { + } void* rxThread(void); - static void* rxThreadHelper(void *context) + static void* rxThreadHelper(void* context) { return ((VescInterface::Impl*)context)->rxThread(); } @@ -45,27 +46,27 @@ void* VescInterface::Impl::rxThread(void) Buffer buffer; buffer.reserve(4096); - while (rx_thread_run_) { - + while (rx_thread_run_) + { int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) { - + if (!buffer.empty()) + { // search buffer for valid packet(s) Buffer::iterator iter(buffer.begin()); Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) { - + while (iter != buffer.end()) + { // check if valid start-of-frame character - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) { - + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) + { // good start, now attempt to create packet std::string error; - VescPacketConstPtr packet = - VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); - if (packet) { + VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); + if (packet) + { // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) { + if (std::distance(iter_begin, iter) > 0) + { std::ostringstream ss; ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << std::distance(iter_begin, iter) << " bytes."; @@ -79,11 +80,13 @@ void* VescInterface::Impl::rxThread(void) // continue to look for another frame in buffer continue; } - else if (bytes_needed > 0) { + else if (bytes_needed > 0) + { // need more data, break out of while loop - break; // for (iter_sof... + break; // for (iter_sof... } - else { + else + { // else, this was not a packet, move on to next byte error_handler_(error); } @@ -97,7 +100,8 @@ void* VescInterface::Impl::rxThread(void) bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) { + if (std::distance(iter_begin, iter) > 0) + { std::ostringstream ss; ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; error_handler_(ss.str()); @@ -106,21 +110,18 @@ void* VescInterface::Impl::rxThread(void) } // attempt to read at least bytes_needed bytes from the serial port - int bytes_to_read = - std::max(bytes_needed, std::min(4096, static_cast(serial_.available()))); + int bytes_to_read = std::max(bytes_needed, std::min(4096, static_cast(serial_.available()))); int bytes_read = serial_.read(buffer, bytes_to_read); - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { + if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) + { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); } - } } - -VescInterface::VescInterface(const std::string& port, - const PacketHandlerFunction& packet_handler, - const ErrorHandlerFunction& error_handler) : - impl_(new Impl()) +VescInterface::VescInterface(const std::string& port, const PacketHandlerFunction& packet_handler, + const ErrorHandlerFunction& error_handler) + : impl_(new Impl()) { setPacketHandler(packet_handler); setErrorHandler(error_handler); @@ -150,25 +151,27 @@ void VescInterface::connect(const std::string& port) { // todo - mutex? - if (isConnected()) { + if (isConnected()) + { throw SerialException("Already connected to serial port."); } // connect to serial port - try { + try + { impl_->serial_.setPort(port); impl_->serial_.open(); } - catch (const std::exception& e) { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); + catch (const std::exception& e) + { + std::stringstream ss; + ss << "Failed to open the serial port to the VESC. " << e.what(); + throw SerialException(ss.str().c_str()); } // start up a monitoring thread impl_->rx_thread_run_ = true; - int result = - pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); + int result = pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); assert(0 == result); } @@ -176,7 +179,8 @@ void VescInterface::disconnect() { // todo - mutex? - if (isConnected()) { + if (isConnected()) + { // bring down read thread impl_->rx_thread_run_ = false; int result = pthread_join(impl_->rx_thread_, NULL); @@ -194,7 +198,8 @@ bool VescInterface::isConnected() const void VescInterface::send(const VescPacket& packet) { size_t written = impl_->serial_.write(packet.frame()); - if (written != packet.frame().size()) { + if (written != packet.frame().size()) + { std::stringstream ss; ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; throw SerialException(ss.str().c_str()); @@ -246,5 +251,4 @@ void VescInterface::requestImuData() send(VescPacketRequestImu()); } - -} // namespace vesc_driver +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 1adc3cb..e42abfc 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -17,591 +17,577 @@ namespace vesc_driver { - VescFrame::VescFrame(int payload_size) - { - assert(payload_size >= 0 && payload_size <= 1024); - - if (payload_size < 256) - { - // single byte payload size - frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); - *frame_->begin() = 2; - *(frame_->begin() + 1) = payload_size; - payload_.first = frame_->begin() + 2; - } - else - { - // two byte payload size - frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); - *frame_->begin() = 3; - *(frame_->begin() + 1) = payload_size >> 8; - *(frame_->begin() + 2) = payload_size & 0xFF; - payload_.first = frame_->begin() + 3; - } - - payload_.second = payload_.first + payload_size; - *(frame_->end() - 1) = 3; - } +VescFrame::VescFrame(int payload_size) +{ + assert(payload_size >= 0 && payload_size <= 1024); - VescFrame::VescFrame(const BufferRangeConst &frame, const BufferRangeConst &payload) + if (payload_size < 256) { - /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap - checks anyway */ - assert(boost::distance(frame) >= VESC_MIN_FRAME_SIZE); - assert(boost::distance(frame) <= VESC_MAX_FRAME_SIZE); - assert(boost::distance(payload) <= VESC_MAX_PAYLOAD_SIZE); - assert(std::distance(frame.first, payload.first) > 0 && - std::distance(payload.second, frame.second) > 0); - - frame_.reset(new Buffer(boost::begin(frame), boost::end(frame))); - payload_.first = frame_->begin() + std::distance(frame.first, payload.first); - payload_.second = frame_->begin() + std::distance(frame.first, payload.second); + // single byte payload size + frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); + *frame_->begin() = 2; + *(frame_->begin() + 1) = payload_size; + payload_.first = frame_->begin() + 2; } - - VescPacket::VescPacket(const std::string &name, int payload_size, int payload_id) : VescFrame(payload_size), name_(name) + else { - assert(payload_id >= 0 && payload_id < 256); - assert(boost::distance(payload_) > 0); - *payload_.first = payload_id; + // two byte payload size + frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); + *frame_->begin() = 3; + *(frame_->begin() + 1) = payload_size >> 8; + *(frame_->begin() + 2) = payload_size & 0xFF; + payload_.first = frame_->begin() + 3; } - VescPacket::VescPacket(const std::string &name, boost::shared_ptr raw) : VescFrame(*raw), name_(name) - { - } + payload_.second = payload_.first + payload_size; + *(frame_->end() - 1) = 3; +} - /*------------------------------------------------------------------------------------------------*/ +VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload) +{ + /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap + checks anyway */ + assert(boost::distance(frame) >= VESC_MIN_FRAME_SIZE); + assert(boost::distance(frame) <= VESC_MAX_FRAME_SIZE); + assert(boost::distance(payload) <= VESC_MAX_PAYLOAD_SIZE); + assert(std::distance(frame.first, payload.first) > 0 && std::distance(payload.second, frame.second) > 0); + + frame_.reset(new Buffer(boost::begin(frame), boost::end(frame))); + payload_.first = frame_->begin() + std::distance(frame.first, payload.first); + payload_.second = frame_->begin() + std::distance(frame.first, payload.second); +} + +VescPacket::VescPacket(const std::string& name, int payload_size, int payload_id) : VescFrame(payload_size), name_(name) +{ + assert(payload_id >= 0 && payload_id < 256); + assert(boost::distance(payload_) > 0); + *payload_.first = payload_id; +} - VescPacketFWVersion::VescPacketFWVersion(boost::shared_ptr raw) : VescPacket("FWVersion", raw) - { - } +VescPacket::VescPacket(const std::string& name, boost::shared_ptr raw) : VescFrame(*raw), name_(name) +{ +} - int VescPacketFWVersion::fwMajor() const - { - return *(payload_.first + 1); - } +/*------------------------------------------------------------------------------------------------*/ - int VescPacketFWVersion::fwMinor() const - { - return *(payload_.first + 2); - } +VescPacketFWVersion::VescPacketFWVersion(boost::shared_ptr raw) : VescPacket("FWVersion", raw) +{ +} - REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) +int VescPacketFWVersion::fwMajor() const +{ + return *(payload_.first + 1); +} - VescPacketRequestFWVersion::VescPacketRequestFWVersion() : VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) - { - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } +int VescPacketFWVersion::fwMinor() const +{ + return *(payload_.first + 2); +} - /*------------------------------------------------------------------------------------------------*/ +REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) - VescPacketValues::VescPacketValues(boost::shared_ptr raw) : VescPacket("Values", raw) - { - } +VescPacketRequestFWVersion::VescPacketRequestFWVersion() : VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) +{ + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - // double VescPacketValues::temp_mos1() const - // { - // int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - // static_cast(*(payload_.first + 2))); - // return static_cast(v) / 10.0; - // } - - // double VescPacketValues::temp_mos2() const - // { - // int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - // static_cast(*(payload_.first + 4))); - // return static_cast(v) / 10.0; - // } - - double VescPacketValues::temp_mos1() const - { - int16_t v = static_cast( - (static_cast(*(payload_.first + 59)) << 8) + - (static_cast(*(payload_.first + 60)))); - return static_cast(v) / 10.0; - } +/*------------------------------------------------------------------------------------------------*/ - double VescPacketValues::temp_mos2() const - { - int16_t v = static_cast( - (static_cast(*(payload_.first + 61)) << 8) + - (static_cast(*(payload_.first + 62)))); - return static_cast(v) / 10.0; - } +VescPacketValues::VescPacketValues(boost::shared_ptr raw) : VescPacket("Values", raw) +{ +} + +// double VescPacketValues::temp_mos1() const +// { +// int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + +// static_cast(*(payload_.first + 2))); +// return static_cast(v) / 10.0; +// } + +// double VescPacketValues::temp_mos2() const +// { +// int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + +// static_cast(*(payload_.first + 4))); +// return static_cast(v) / 10.0; +// } + +double VescPacketValues::temp_mos1() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + + (static_cast(*(payload_.first + 60)))); + return static_cast(v) / 10.0; +} - double VescPacketValues::temp_mos3() const - { - int16_t v = static_cast( - (static_cast(*(payload_.first + 63)) << 8) + - (static_cast(*(payload_.first + 64)))); - return static_cast(v) / 10.0; - } - double VescPacketValues::current_motor() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + - (static_cast(*(payload_.first + 6)) << 16) + - (static_cast(*(payload_.first + 7)) << 8) + - static_cast(*(payload_.first + 8))); - return static_cast(v) / 100.0; - } +double VescPacketValues::temp_mos2() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 61)) << 8) + + (static_cast(*(payload_.first + 62)))); + return static_cast(v) / 10.0; +} - double VescPacketValues::current_in() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + - (static_cast(*(payload_.first + 10)) << 16) + - (static_cast(*(payload_.first + 11)) << 8) + - static_cast(*(payload_.first + 12))); - return static_cast(v) / 100.0; - } +double VescPacketValues::temp_mos3() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 63)) << 8) + + (static_cast(*(payload_.first + 64)))); + return static_cast(v) / 10.0; +} +double VescPacketValues::current_motor() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 5)) << 24) + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + static_cast(*(payload_.first + 8))); + return static_cast(v) / 100.0; +} - double VescPacketValues::duty_now() const - { - int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); - return static_cast(v) / 1000.0; - } +double VescPacketValues::current_in() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 9)) << 24) + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + static_cast(*(payload_.first + 12))); + return static_cast(v) / 100.0; +} - double VescPacketValues::rpm() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26))); - return static_cast(-1 * v); - } +double VescPacketValues::duty_now() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + + static_cast(*(payload_.first + 22))); + return static_cast(v) / 1000.0; +} - double VescPacketValues::amp_hours() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + - (static_cast(*(payload_.first + 28)) << 16) + - (static_cast(*(payload_.first + 29)) << 8) + - static_cast(*(payload_.first + 30))); - return static_cast(v); - } +double VescPacketValues::rpm() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 23)) << 24) + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + static_cast(*(payload_.first + 26))); + return static_cast(-1 * v); +} - double VescPacketValues::amp_hours_charged() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 31)) << 24) + - (static_cast(*(payload_.first + 32)) << 16) + - (static_cast(*(payload_.first + 33)) << 8) + - static_cast(*(payload_.first + 34))); - return static_cast(v); - } +double VescPacketValues::amp_hours() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 27)) << 24) + (static_cast(*(payload_.first + 28)) << 16) + + (static_cast(*(payload_.first + 29)) << 8) + static_cast(*(payload_.first + 30))); + return static_cast(v); +} - double VescPacketValues::tachometer() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 35)) << 24) + - (static_cast(*(payload_.first + 36)) << 16) + - (static_cast(*(payload_.first + 37)) << 8) + - static_cast(*(payload_.first + 38))); - return static_cast(v); - } +double VescPacketValues::amp_hours_charged() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 31)) << 24) + (static_cast(*(payload_.first + 32)) << 16) + + (static_cast(*(payload_.first + 33)) << 8) + static_cast(*(payload_.first + 34))); + return static_cast(v); +} - double VescPacketValues::tachometer_abs() const - { - int32_t v = static_cast((static_cast(*(payload_.first + 39)) << 24) + - (static_cast(*(payload_.first + 40)) << 16) + - (static_cast(*(payload_.first + 41)) << 8) + - static_cast(*(payload_.first + 42))); - return static_cast(v); - } +double VescPacketValues::tachometer() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 35)) << 24) + (static_cast(*(payload_.first + 36)) << 16) + + (static_cast(*(payload_.first + 37)) << 8) + static_cast(*(payload_.first + 38))); + return static_cast(v); +} - int VescPacketValues::fault_code() const - { - return static_cast(*(payload_.first + 56)); - } +double VescPacketValues::tachometer_abs() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 39)) << 24) + (static_cast(*(payload_.first + 40)) << 16) + + (static_cast(*(payload_.first + 41)) << 8) + static_cast(*(payload_.first + 42))); + return static_cast(v); +} - double VescPacketValues::v_in() const - { - int32_t v = 0; - return static_cast(v); - } +int VescPacketValues::fault_code() const +{ + return static_cast(*(payload_.first + 56)); +} - double VescPacketValues::temp_pcb() const - { - int32_t v = 0; - return static_cast(v); - } +double VescPacketValues::v_in() const +{ + int32_t v = 0; + return static_cast(v); +} - double VescPacketValues::watt_hours() const - { - int32_t v = 0; - return static_cast(v); - } +double VescPacketValues::temp_pcb() const +{ + int32_t v = 0; + return static_cast(v); +} - double VescPacketValues::watt_hours_charged() const - { - int32_t v = 0; - return static_cast(v); - } +double VescPacketValues::watt_hours() const +{ + int32_t v = 0; + return static_cast(v); +} - REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) +double VescPacketValues::watt_hours_charged() const +{ + int32_t v = 0; + return static_cast(v); +} - VescPacketRequestValues::VescPacketRequestValues() : VescPacket("RequestValues", 1, COMM_GET_VALUES) - { - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } +REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) - /*------------------------------------------------------------------------------------------------*/ +VescPacketRequestValues::VescPacketRequestValues() : VescPacket("RequestValues", 1, COMM_GET_VALUES) +{ + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - VescPacketSetDuty::VescPacketSetDuty(double duty) : VescPacket("SetDuty", 5, COMM_SET_DUTY) - { - /** @todo range check duty */ +/*------------------------------------------------------------------------------------------------*/ - int32_t v = static_cast(duty * 100000.0); +VescPacketSetDuty::VescPacketSetDuty(double duty) : VescPacket("SetDuty", 5, COMM_SET_DUTY) +{ + /** @todo range check duty */ - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + int32_t v = static_cast(duty * 100000.0); - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - /*------------------------------------------------------------------------------------------------*/ + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - VescPacketSetCurrent::VescPacketSetCurrent(double current) : VescPacket("SetCurrent", 5, COMM_SET_CURRENT) - { - int32_t v = static_cast(current * 1000.0); - - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } +/*------------------------------------------------------------------------------------------------*/ - /*------------------------------------------------------------------------------------------------*/ +VescPacketSetCurrent::VescPacketSetCurrent(double current) : VescPacket("SetCurrent", 5, COMM_SET_CURRENT) +{ + int32_t v = static_cast(current * 1000.0); - VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) - { - int32_t v = static_cast(current_brake * 1000.0); - - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - /*------------------------------------------------------------------------------------------------*/ + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - VescPacketSetRPM::VescPacketSetRPM(double rpm) : VescPacket("SetRPM", 5, COMM_SET_RPM) - { - int32_t v = static_cast(rpm); - - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } +/*------------------------------------------------------------------------------------------------*/ - /*------------------------------------------------------------------------------------------------*/ +VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) + : VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) +{ + int32_t v = static_cast(current_brake * 1000.0); - VescPacketSetPos::VescPacketSetPos(double pos) : VescPacket("SetPos", 5, COMM_SET_POS) - { - /** @todo range check pos */ + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - int32_t v = static_cast(pos * 1000000.0); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); - *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); - *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); +/*------------------------------------------------------------------------------------------------*/ - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } +VescPacketSetRPM::VescPacketSetRPM(double rpm) : VescPacket("SetRPM", 5, COMM_SET_RPM) +{ + int32_t v = static_cast(rpm); - /*------------------------------------------------------------------------------------------------*/ + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) - { - /** @todo range check pos */ + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - int16_t v = static_cast(servo_pos * 1000.0); +/*------------------------------------------------------------------------------------------------*/ - *(payload_.first + 1) = static_cast((static_cast(v) >> 8) & 0xFF); - *(payload_.first + 2) = static_cast(static_cast(v) & 0xFF); +VescPacketSetPos::VescPacketSetPos(double pos) : VescPacket("SetPos", 5, COMM_SET_POS) +{ + /** @todo range check pos */ - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - uint16_t crc = crc_calc.checksum(); - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } + int32_t v = static_cast(pos * 1000000.0); - // VescPacketImu::VescPacketImu(boost::shared_ptr raw) - // : VescPacket("ImuData", raw) //check - // { - // uint32_t ind = 1; - // mask_ = static_cast( - // (static_cast(*(payload_.first + ind )) << 8) + - // static_cast(*(payload_.first + ind + 1 )) - // ); - // ind += 2; - - // if (mask_ & ((uint32_t)1 << 0)) {roll_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 1)) {pitch_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 2)) {yaw_ = getFloat32Auto(&ind);} - - // if (mask_ & ((uint32_t)1 << 3)) {acc_x_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 4)) {acc_y_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 5)) {acc_z_ = getFloat32Auto(&ind);} - - // if (mask_ & ((uint32_t)1 << 6)) {gyr_x_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 7)) {gyr_y_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 8)) {gyr_z_ = getFloat32Auto(&ind);} - - // if (mask_ & ((uint32_t)1 << 9)) {mag_x_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 10)) {mag_y_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 11)) {mag_z_ = getFloat32Auto(&ind);} - - // if (mask_ & ((uint32_t)1 << 12)) {q0_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 13)) {q1_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 14)) {q2_ = getFloat32Auto(&ind);} - // if (mask_ & ((uint32_t)1 << 15)) {q3_ = getFloat32Auto(&ind);} - // } - - VescPacketImu::VescPacketImu(boost::shared_ptr raw) - : VescPacket("ImuData", raw) - { - uint32_t ind = 1; - mask_ = static_cast( - (static_cast(*(payload_.first + ind)) << 8) + - static_cast(*(payload_.first + ind + 1))); - ind += 2; - - if (mask_ & ((uint32_t)1 << 0)) - { - roll_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 1)) - { - pitch_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 2)) - { - yaw_ = getFloat32Auto(&ind); - } - - if (mask_ & ((uint32_t)1 << 3)) - { - acc_x_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 4)) - { - acc_y_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 5)) - { - acc_z_ = getFloat32Auto(&ind); - } - - if (mask_ & ((uint32_t)1 << 6)) - { - gyr_x_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 7)) - { - gyr_y_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 8)) - { - gyr_z_ = getFloat32Auto(&ind); - } - - if (mask_ & ((uint32_t)1 << 9)) - { - mag_x_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 10)) - { - mag_y_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 11)) - { - mag_z_ = getFloat32Auto(&ind); - } - - if (mask_ & ((uint32_t)1 << 12)) - { - q0_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 13)) - { - q1_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 14)) - { - q2_ = getFloat32Auto(&ind); - } - if (mask_ & ((uint32_t)1 << 15)) - { - q3_ = getFloat32Auto(&ind); - } - } + *(payload_.first + 1) = static_cast((static_cast(v) >> 24) & 0xFF); + *(payload_.first + 2) = static_cast((static_cast(v) >> 16) & 0xFF); + *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} - int VescPacketImu::mask() const +/*------------------------------------------------------------------------------------------------*/ + +VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) +{ + /** @todo range check pos */ + + int16_t v = static_cast(servo_pos * 1000.0); + + *(payload_.first + 1) = static_cast((static_cast(v) >> 8) & 0xFF); + *(payload_.first + 2) = static_cast(static_cast(v) & 0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} + +// VescPacketImu::VescPacketImu(boost::shared_ptr raw) +// : VescPacket("ImuData", raw) //check +// { +// uint32_t ind = 1; +// mask_ = static_cast( +// (static_cast(*(payload_.first + ind )) << 8) + +// static_cast(*(payload_.first + ind + 1 )) +// ); +// ind += 2; + +// if (mask_ & ((uint32_t)1 << 0)) {roll_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 1)) {pitch_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 2)) {yaw_ = getFloat32Auto(&ind);} + +// if (mask_ & ((uint32_t)1 << 3)) {acc_x_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 4)) {acc_y_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 5)) {acc_z_ = getFloat32Auto(&ind);} + +// if (mask_ & ((uint32_t)1 << 6)) {gyr_x_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 7)) {gyr_y_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 8)) {gyr_z_ = getFloat32Auto(&ind);} + +// if (mask_ & ((uint32_t)1 << 9)) {mag_x_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 10)) {mag_y_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 11)) {mag_z_ = getFloat32Auto(&ind);} + +// if (mask_ & ((uint32_t)1 << 12)) {q0_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 13)) {q1_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 14)) {q2_ = getFloat32Auto(&ind);} +// if (mask_ & ((uint32_t)1 << 15)) {q3_ = getFloat32Auto(&ind);} +// } + +VescPacketImu::VescPacketImu(boost::shared_ptr raw) : VescPacket("ImuData", raw) +{ + uint32_t ind = 1; + mask_ = static_cast((static_cast(*(payload_.first + ind)) << 8) + + static_cast(*(payload_.first + ind + 1))); + ind += 2; + + if (mask_ & ((uint32_t)1 << 0)) { - return mask_; + roll_ = getFloat32Auto(&ind); } - - double VescPacketImu::getFloat32Auto(uint32_t *idx) const + if (mask_ & ((uint32_t)1 << 1)) { - int pos = *idx; - uint32_t res = static_cast( - (static_cast(*(payload_.first + (pos))) << 24) + - (static_cast(*(payload_.first + (pos + 1))) << 16) + - (static_cast(*(payload_.first + (pos + 2))) << 8) + - static_cast(*(payload_.first + (pos + 3)))); - - *idx += 4; - - int e = (res >> 23) & 0xFF; - int fr = res & 0x7FFFFF; - bool negative = res & (1u << 31); - - float f = 0.0; - if (e != 0 || fr != 0) - { - f = static_cast(fr) / (8388608.0 * 2.0) + 0.5; - e -= 126; - } - - if (negative) - { - f = -f; - } - return ldexpf(f, e); + pitch_ = getFloat32Auto(&ind); } - - double VescPacketImu::roll() const + if (mask_ & ((uint32_t)1 << 2)) { - return roll_ * 180 / M_PI; // da rad a gradi per debug deg + yaw_ = getFloat32Auto(&ind); } - double VescPacketImu::pitch() const + if (mask_ & ((uint32_t)1 << 3)) { - return pitch_ * 180 / M_PI; + acc_x_ = getFloat32Auto(&ind); } - - double VescPacketImu::yaw() const + if (mask_ & ((uint32_t)1 << 4)) { - return yaw_ * 180 / M_PI; + acc_y_ = getFloat32Auto(&ind); } - - double VescPacketImu::acc_x() const + if (mask_ & ((uint32_t)1 << 5)) { - return acc_x_; // g/s + acc_z_ = getFloat32Auto(&ind); } - double VescPacketImu::acc_y() const + if (mask_ & ((uint32_t)1 << 6)) { - return acc_y_; + gyr_x_ = getFloat32Auto(&ind); } - - double VescPacketImu::acc_z() const + if (mask_ & ((uint32_t)1 << 7)) { - return acc_z_; + gyr_y_ = getFloat32Auto(&ind); } - - double VescPacketImu::gyr_x() const + if (mask_ & ((uint32_t)1 << 8)) { - return gyr_x_; // deg/s + gyr_z_ = getFloat32Auto(&ind); } - double VescPacketImu::gyr_y() const + if (mask_ & ((uint32_t)1 << 9)) { - return gyr_y_; + mag_x_ = getFloat32Auto(&ind); } - - double VescPacketImu::gyr_z() const + if (mask_ & ((uint32_t)1 << 10)) { - return gyr_z_; + mag_y_ = getFloat32Auto(&ind); } - - double VescPacketImu::mag_x() const + if (mask_ & ((uint32_t)1 << 11)) { - return mag_x_; + mag_z_ = getFloat32Auto(&ind); } - double VescPacketImu::mag_y() const + if (mask_ & ((uint32_t)1 << 12)) { - return mag_y_; + q0_ = getFloat32Auto(&ind); } - - double VescPacketImu::mag_z() const + if (mask_ & ((uint32_t)1 << 13)) { - return mag_z_; + q1_ = getFloat32Auto(&ind); } - - double VescPacketImu::q_w() const + if (mask_ & ((uint32_t)1 << 14)) { - return q0_; + q2_ = getFloat32Auto(&ind); } - - double VescPacketImu::q_x() const + if (mask_ & ((uint32_t)1 << 15)) { - return q1_; + q3_ = getFloat32Auto(&ind); } +} + +int VescPacketImu::mask() const +{ + return mask_; +} + +double VescPacketImu::getFloat32Auto(uint32_t* idx) const +{ + int pos = *idx; + uint32_t res = static_cast((static_cast(*(payload_.first + (pos))) << 24) + + (static_cast(*(payload_.first + (pos + 1))) << 16) + + (static_cast(*(payload_.first + (pos + 2))) << 8) + + static_cast(*(payload_.first + (pos + 3)))); - double VescPacketImu::q_y() const + *idx += 4; + + int e = (res >> 23) & 0xFF; + int fr = res & 0x7FFFFF; + bool negative = res & (1u << 31); + + float f = 0.0; + if (e != 0 || fr != 0) { - return q2_; + f = static_cast(fr) / (8388608.0 * 2.0) + 0.5; + e -= 126; } - double VescPacketImu::q_z() const + if (negative) { - return q3_; + f = -f; } + return ldexpf(f, e); +} - REGISTER_PACKET_TYPE(COMM_GET_IMU_DATA, VescPacketImu) +double VescPacketImu::roll() const +{ + return roll_ * 180 / M_PI; // da rad a gradi per debug deg +} - VescPacketRequestImu::VescPacketRequestImu() - : VescPacket("RequestImuData", 3, COMM_GET_IMU_DATA) - { - *(payload_.first + 1) = static_cast(0xFF); - *(payload_.first + 2) = static_cast(0xFF); +double VescPacketImu::pitch() const +{ + return pitch_ * 180 / M_PI; +} - VescFrame::CRC crc_calc; - crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); - - uint16_t crc = crc_calc.checksum(); +double VescPacketImu::yaw() const +{ + return yaw_ * 180 / M_PI; +} - *(frame_->end() - 3) = static_cast(crc >> 8); - *(frame_->end() - 2) = static_cast(crc & 0xFF); - } - /*------------------------------------------------------------------------------------------------*/ +double VescPacketImu::acc_x() const +{ + return acc_x_; // g/s +} + +double VescPacketImu::acc_y() const +{ + return acc_y_; +} + +double VescPacketImu::acc_z() const +{ + return acc_z_; +} + +double VescPacketImu::gyr_x() const +{ + return gyr_x_; // deg/s +} + +double VescPacketImu::gyr_y() const +{ + return gyr_y_; +} + +double VescPacketImu::gyr_z() const +{ + return gyr_z_; +} + +double VescPacketImu::mag_x() const +{ + return mag_x_; +} + +double VescPacketImu::mag_y() const +{ + return mag_y_; +} + +double VescPacketImu::mag_z() const +{ + return mag_z_; +} + +double VescPacketImu::q_w() const +{ + return q0_; +} + +double VescPacketImu::q_x() const +{ + return q1_; +} + +double VescPacketImu::q_y() const +{ + return q2_; +} + +double VescPacketImu::q_z() const +{ + return q3_; +} + +REGISTER_PACKET_TYPE(COMM_GET_IMU_DATA, VescPacketImu) + +VescPacketRequestImu::VescPacketRequestImu() : VescPacket("RequestImuData", 3, COMM_GET_IMU_DATA) +{ + *(payload_.first + 1) = static_cast(0xFF); + *(payload_.first + 2) = static_cast(0xFF); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + + uint16_t crc = crc_calc.checksum(); + + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} +/*------------------------------------------------------------------------------------------------*/ -} // namespace vesc_driver +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index 470f9c5..ddc3440 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -29,21 +29,24 @@ void VescPacketFactory::registerPacketType(int payload_id, CreateFn fn) } /** Helper function for when createPacket can not create a packet */ -VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, - const std::string& what, int num_bytes_needed = 0) +VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, const std::string& what, + int num_bytes_needed = 0) { - if (p_num_bytes_needed != NULL) *p_num_bytes_needed = num_bytes_needed; - if (p_what != NULL) *p_what = what; + if (p_num_bytes_needed != NULL) + *p_num_bytes_needed = num_bytes_needed; + if (p_what != NULL) + *p_what = what; return VescPacketPtr(); } -VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, +VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, const Buffer::const_iterator& end, int* num_bytes_needed, std::string* what) { // initialize output variables - if (num_bytes_needed != NULL) *num_bytes_needed = 0; - if (what != NULL) what->clear(); + if (num_bytes_needed != NULL) + *num_bytes_needed = 0; + if (what != NULL) + what->clear(); // need at least VESC_MIN_FRAME_SIZE bytes in buffer int buffer_size(std::distance(begin, end)); @@ -52,18 +55,19 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); // buffer must begin with a start-of-frame - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin && - VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin && VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) return createFailed(num_bytes_needed, what, "Buffer must begin with start-of-frame character"); // get a view of the payload BufferRangeConst view_payload; - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) { + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) + { // payload size field is one byte view_payload.first = begin + 2; view_payload.second = view_payload.first + *(begin + 1); } - else { + else + { assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin); // payload size field is two bytes view_payload.first = begin + 3; @@ -82,8 +86,7 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi // do we have enough data in the buffer to complete the frame? int frame_size = boost::distance(view_frame); if (buffer_size < frame_size) - return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", - frame_size - buffer_size); + return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", frame_size - buffer_size); // is the end-of-frame character valid? if (VescFrame::VESC_EOF_VAL != *iter_eof) @@ -100,25 +103,26 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi boost::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); // if the packet has a payload, construct the corresponding subclass - if (boost::distance(view_payload) > 0) { - + if (boost::distance(view_payload) > 0) + { // get constructor function from payload id FactoryMap* p_map(getMap()); FactoryMap::const_iterator search(p_map->find(*view_payload.first)); - if (search != p_map->end()) { + if (search != p_map->end()) + { return search->second(raw_frame); } - else { + else + { // no subclass constructor for this packet return createFailed(num_bytes_needed, what, "Unkown payload type."); } - } - else { + else + { // no payload return createFailed(num_bytes_needed, what, "Frame does not have a payload"); } } - -} // namesapce vesc_driver +} // namespace vesc_driver From bf6e4d098b76ac2b9f4e95a55a1baa50eef64109 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Tue, 11 Apr 2023 17:04:55 -0400 Subject: [PATCH 16/20] restored liscences to all files --- .../vesc_ackermann/ackermann_to_vesc.h | 29 ++++++++ .../include/vesc_ackermann/vesc_to_odom.h | 28 ++++++++ vesc_ackermann/src/ackermann_to_vesc.cpp | 26 +++++++ vesc_ackermann/src/ackermann_to_vesc_node.cpp | 25 +++++++ .../src/ackermann_to_vesc_nodelet.cpp | 25 +++++++ vesc_ackermann/src/vesc_to_odom.cpp | 25 +++++++ vesc_ackermann/src/vesc_to_odom_node.cpp | 25 +++++++ vesc_ackermann/src/vesc_to_odom_nodelet.cpp | 25 +++++++ vesc_driver/CMakeLists.txt | 31 +++++++- vesc_driver/include/vesc_driver/datatypes.h | 71 +++++++------------ vesc_driver/include/vesc_driver/vesc_driver.h | 25 +++++++ .../include/vesc_driver/vesc_interface.h | 25 +++++++ vesc_driver/include/vesc_driver/vesc_packet.h | 25 +++++++ .../include/vesc_driver/vesc_packet_factory.h | 25 +++++++ vesc_driver/src/vesc_driver.cpp | 25 +++++++ vesc_driver/src/vesc_interface.cpp | 25 +++++++ vesc_driver/src/vesc_packet.cpp | 25 +++++++ vesc_driver/src/vesc_packet_factory.cpp | 25 +++++++ 18 files changed, 465 insertions(+), 45 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h index 5aa6e65..2674ab3 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ @@ -17,7 +42,9 @@ class AckermannToVesc private: // ROS parameters // conversion gain and offset + bool previous_mode_speed_ = true; double speed_to_erpm_gain_, speed_to_erpm_offset_; + double accel_to_current_gain_, accel_to_brake_gain_; double steering_to_servo_gain_, steering_to_servo_offset_; /** @todo consider also providing an interpolated look-up table conversion */ @@ -25,6 +52,8 @@ class AckermannToVesc // ROS services ros::Publisher erpm_pub_; ros::Publisher servo_pub_; + ros::Publisher current_pub_; + ros::Publisher brake_pub_; ros::Subscriber ackermann_sub_; // ROS callbacks diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h index fbc610b..7a8b21e 100644 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h @@ -1,8 +1,36 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_ #define VESC_ACKERMANN_VESC_TO_ODOM_H_ +#include +#include + #include #include #include diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 17e846d..01f1c0b 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -1,9 +1,35 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #include "vesc_ackermann/ackermann_to_vesc.h" #include #include +#include #include diff --git a/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/vesc_ackermann/src/ackermann_to_vesc_node.cpp index cfe2268..d7fcec0 100644 --- a/vesc_ackermann/src/ackermann_to_vesc_node.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc_node.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + #include #include "vesc_ackermann/ackermann_to_vesc.h" diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp index 766db51..2900faa 100644 --- a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + #include #include #include diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index cfce965..ad5c63e 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #include "vesc_ackermann/vesc_to_odom.h" diff --git a/vesc_ackermann/src/vesc_to_odom_node.cpp b/vesc_ackermann/src/vesc_to_odom_node.cpp index 9ce992f..caea389 100644 --- a/vesc_ackermann/src/vesc_to_odom_node.cpp +++ b/vesc_ackermann/src/vesc_to_odom_node.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + #include #include "vesc_ackermann/vesc_to_odom.h" diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp index 2a7bd0e..9d0be7c 100644 --- a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp +++ b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + #include #include #include diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index ece8de1..2a8e97d 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,10 +1,23 @@ cmake_minimum_required(VERSION 2.8.3) project(vesc_driver) +# Set minimum C++ standard to C++11 +if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") + message(STATUS "Changing CXX_STANDARD from C++98 to C++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") + message(STATUS "Changing CXX_STANDARD from C++98 to C++11") + set(CMAKE_CXX_STANDARD 11) +endif() + +find_package(Boost REQUIRED) + find_package(catkin REQUIRED COMPONENTS nodelet pluginlib roscpp + roslint + serial std_msgs vesc_msgs serial @@ -12,6 +25,11 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include + CATKIN_DEPENDS + nodelet + pluginlib + std_msgs + vesc_msgs serial CATKIN_DEPENDS nodelet pluginlib roscpp std_msgs vesc_msgs serial ) @@ -47,6 +65,15 @@ target_link_libraries(vesc_driver_nodelet ${catkin_LIBRARIES} ) +file(GLOB_RECURSE ${PROJECT_NAME}_CPP_FILES + RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} + . *.cpp *.h) +list(REMOVE_ITEM ${PROJECT_NAME}_CPP_FILES + include/${PROJECT_NAME}/crc.h) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++11") +roslint_cpp(${${PROJECT_NAME}_CPP_FILES}) + ############# ## Install ## ############# @@ -70,4 +97,6 @@ install(DIRECTORY launch/ ## Testing ## ############# -# TODO \ No newline at end of file +if(CATKIN_ENABLE_TESTING) + roslint_add_test() +endif() \ No newline at end of file diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.h index 616dcb7..5b498b3 100644 --- a/vesc_driver/include/vesc_driver/datatypes.h +++ b/vesc_driver/include/vesc_driver/datatypes.h @@ -1,5 +1,30 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + /* - Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se + Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se This file is part of VESC Tool. @@ -15,7 +40,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - */ +*/ #ifndef DATATYPES_H #define DATATYPES_H @@ -207,48 +232,6 @@ typedef enum COMM_GET_IMU_CALIBRATION } COMM_PACKET_ID; -// typedef enum { -// COMM_FW_VERSION = 0, -// COMM_JUMP_TO_BOOTLOADER, -// COMM_ERASE_NEW_APP, -// COMM_WRITE_NEW_APP_DATA, -// COMM_GET_VALUES, -// COMM_SET_DUTY, -// COMM_SET_CURRENT, -// COMM_SET_CURRENT_BRAKE, -// COMM_SET_RPM, -// COMM_SET_POS, -// COMM_SET_HANDBRAKE, -// COMM_SET_DETECT, -// COMM_SET_SERVO_POS, -// COMM_SET_MCCONF, -// COMM_GET_MCCONF, -// COMM_GET_MCCONF_DEFAULT, -// COMM_SET_APPCONF, -// COMM_GET_APPCONF, -// COMM_GET_APPCONF_DEFAULT, -// COMM_SAMPLE_PRINT, -// COMM_TERMINAL_CMD, -// COMM_PRINT, -// COMM_ROTOR_POSITION, -// COMM_EXPERIMENT_SAMPLE, -// COMM_DETECT_MOTOR_PARAM, -// COMM_DETECT_MOTOR_R_L, -// COMM_DETECT_MOTOR_FLUX_LINKAGE, -// COMM_DETECT_ENCODER, -// COMM_DETECT_HALL_FOC, -// COMM_REBOOT, -// COMM_ALIVE, -// COMM_GET_DECODED_PPM, -// COMM_GET_DECODED_ADC, -// COMM_GET_DECODED_CHUK, -// COMM_FORWARD_CAN, -// COMM_SET_CHUCK_DATA, -// COMM_CUSTOM_APP_DATA, -// COMM_NRF_START_PAIRING, -// COMM_GET_IMU_DATA -// } COMM_PACKET_ID; - typedef struct { int js_x; diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h index b723cbb..9eca3a2 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ b/vesc_driver/include/vesc_driver/vesc_driver.h @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_DRIVER_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.h index 1ea1e91..66be4c0 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.h @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_INTERFACE_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index d4e4ba0..e1b909b 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_PACKET_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/vesc_driver/include/vesc_driver/vesc_packet_factory.h index d05e796..8befc18 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.h +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.h @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_ diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 9695a57..3340bff 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_driver.h" diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 57536ea..cc02dbc 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_interface.h" diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index e42abfc..99a40a6 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_packet.h" diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index ddc3440..02e5a40 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_packet_factory.h" From 92bb2997a948306c75936f68f670690ab2ca3c60 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Wed, 23 Aug 2023 13:06:31 -0400 Subject: [PATCH 17/20] revered exec_depend to depend --- vesc/package.xml | 6 +++--- vesc_ackermann/package.xml | 18 +++++++++--------- vesc_driver/package.xml | 12 ++++++------ vesc_msgs/package.xml | 6 +++--- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/vesc/package.xml b/vesc/package.xml index ad873c9..e961311 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -20,9 +20,9 @@ catkin - vesc_driver - vesc_msgs - vesc_ackermann + vesc_driver + vesc_msgs + vesc_ackermann diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index 8cf64ab..f2ddcf6 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -27,15 +27,15 @@ ackermann_msgs vesc_msgs - nodelet - pluginlib - roscpp - nav_msgs - std_msgs - geometry_msgs - tf - ackermann_msgs - vesc_msgs + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + geometry_msgs + tf + ackermann_msgs + vesc_msgs diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 9109bff..0e5c0b2 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -23,12 +23,12 @@ vesc_msgs serial - nodelet - pluginlib - roscpp - std_msgs - vesc_msgs - serial + nodelet + pluginlib + roscpp + std_msgs + vesc_msgs + serial diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index bc2b8c2..a5128aa 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -20,8 +20,8 @@ message_generation geometry_msgs - std_msgs - message_runtime - geometry_msgs + std_msgs + message_runtime + geometry_msgs From 0d0bf867a135c4afb9d7cb5c9289e508c217b226 Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Wed, 23 Aug 2023 13:10:10 -0400 Subject: [PATCH 18/20] reverted license change --- vesc_driver/src/vesc_driver_node.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/vesc_driver/src/vesc_driver_node.cpp b/vesc_driver/src/vesc_driver_node.cpp index 115191e..2c61547 100644 --- a/vesc_driver/src/vesc_driver_node.cpp +++ b/vesc_driver/src/vesc_driver_node.cpp @@ -1,3 +1,28 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without modification, are permitted +// provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this list of conditions +// and the following disclaimer. +// +// 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. +// +// 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. +// +// 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. + #include #include "vesc_driver/vesc_driver.h" From 424ef1d245beb772113d246d94297ff90532415b Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Wed, 23 Aug 2023 13:10:53 -0400 Subject: [PATCH 19/20] removed commented out / unused code --- vesc_driver/src/vesc_driver.cpp | 18 ------------- vesc_driver/src/vesc_packet.cpp | 46 --------------------------------- 2 files changed, 64 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 3340bff..4d2f466 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -75,7 +75,6 @@ VescDriver::VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) // create vesc state (telemetry) publisher state_pub_ = nh.advertise("sensors/core", 10); imu_pub_ = nh.advertise("sensors/imu", 10); - // imu_std_pub_ = nh.advertise("sensors/imu/raw", 10); // since vesc state does not include the servo position, publish the commanded // servo position as a "sensor" @@ -186,9 +185,6 @@ void VescDriver::vescPacketCallback(const boost::shared_ptr& p { boost::shared_ptr imuData = boost::dynamic_pointer_cast(packet); - // vesc_msgs::VescImuStamped::Ptr imu_msg(new vesc_msgs::VescImuStamped); - // vesc_msgs::VescIMUStamped::Ptr std_imu_msg(new vesc_msgs::VescIMUStamped); - auto imu_msg = new vesc_msgs::VescImuStamped(); // auto std_imu_msg = Imu(); @@ -216,21 +212,7 @@ void VescDriver::vescPacketCallback(const boost::shared_ptr& p imu_msg->imu.orientation.y = imuData->q_y(); imu_msg->imu.orientation.z = imuData->q_z(); - // std_imu_msg.linear_acceleration.x = imuData->acc_x(); - // std_imu_msg.linear_acceleration.y = imuData->acc_y(); - // std_imu_msg.linear_acceleration.z = imuData->acc_z(); - - // std_imu_msg.angular_velocity.x = imuData->gyr_x(); - // std_imu_msg.angular_velocity.y = imuData->gyr_y(); - // std_imu_msg.angular_velocity.z = imuData->gyr_z(); - - // std_imu_msg.orientation.w = imuData->q_w(); - // std_imu_msg.orientation.x = imuData->q_x(); - // std_imu_msg.orientation.y = imuData->q_y(); - // std_imu_msg.orientation.z = imuData->q_z(); - imu_pub_.publish(*imu_msg); - // imu_std_pub_.publish(std_imu_msg); } } diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 99a40a6..bfcb03a 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -126,20 +126,6 @@ VescPacketValues::VescPacketValues(boost::shared_ptr raw) : VescPacke { } -// double VescPacketValues::temp_mos1() const -// { -// int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + -// static_cast(*(payload_.first + 2))); -// return static_cast(v) / 10.0; -// } - -// double VescPacketValues::temp_mos2() const -// { -// int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + -// static_cast(*(payload_.first + 4))); -// return static_cast(v) / 10.0; -// } - double VescPacketValues::temp_mos1() const { int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + @@ -376,38 +362,6 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : VescPacket("Set *(frame_->end() - 2) = static_cast(crc & 0xFF); } -// VescPacketImu::VescPacketImu(boost::shared_ptr raw) -// : VescPacket("ImuData", raw) //check -// { -// uint32_t ind = 1; -// mask_ = static_cast( -// (static_cast(*(payload_.first + ind )) << 8) + -// static_cast(*(payload_.first + ind + 1 )) -// ); -// ind += 2; - -// if (mask_ & ((uint32_t)1 << 0)) {roll_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 1)) {pitch_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 2)) {yaw_ = getFloat32Auto(&ind);} - -// if (mask_ & ((uint32_t)1 << 3)) {acc_x_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 4)) {acc_y_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 5)) {acc_z_ = getFloat32Auto(&ind);} - -// if (mask_ & ((uint32_t)1 << 6)) {gyr_x_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 7)) {gyr_y_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 8)) {gyr_z_ = getFloat32Auto(&ind);} - -// if (mask_ & ((uint32_t)1 << 9)) {mag_x_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 10)) {mag_y_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 11)) {mag_z_ = getFloat32Auto(&ind);} - -// if (mask_ & ((uint32_t)1 << 12)) {q0_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 13)) {q1_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 14)) {q2_ = getFloat32Auto(&ind);} -// if (mask_ & ((uint32_t)1 << 15)) {q3_ = getFloat32Auto(&ind);} -// } - VescPacketImu::VescPacketImu(boost::shared_ptr raw) : VescPacket("ImuData", raw) { uint32_t ind = 1; From 6f22df89718dc0e6324c63e11cba2d8bd5f9864d Mon Sep 17 00:00:00 2001 From: VineetTambe Date: Wed, 23 Aug 2023 13:11:14 -0400 Subject: [PATCH 20/20] reverted madgwick filter for imu --- vesc_driver/launch/vesc_driver_node.launch | 23 +++++----------------- 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/vesc_driver/launch/vesc_driver_node.launch b/vesc_driver/launch/vesc_driver_node.launch index 93f4451..654e348 100644 --- a/vesc_driver/launch/vesc_driver_node.launch +++ b/vesc_driver/launch/vesc_driver_node.launch @@ -4,30 +4,17 @@ - - - + + + - + - - - - - - - - - -