ODrive (#46)

* odrive_node but in efficient cpp

* odrive_node yaml parameters

* Launching odrive_node if not simulation

* Updating gitmodules

* Removing odrive_node

* Adding custom odrive lib directly into drive node

* revert to old launch strategy

* fix float velocities values

* Removing old motor control way

* Adding USE_I2C compilation option for testing purpose

* Hello odrive parameters

* odrive yamled parameters

* compute_velocity_cmd updated + get_motors_turns_from_odrive function

* computing wheels_turns + motors initialization

* computing tf from turns

* Adapting new computing strategy to simulation

* overall drive node cleaning

* update black to 21.4b0

* checksum sometimes isn't sent by odrive but data still here

* asterix and obelix yaml update + removing drive debug logs

* Fixing localisation zero division

* Removal of USE_I2C + robot launcher interface full revert

* 0 division fix

* naming convention

.gitmodules

@@ -1,6 +1,3 @@
 [submodule "src/modules/lcd-driver"]
 	path = src/modules/lcd-driver
 	url = https://github.com/funtech-makers/ros2-i2c-lcd-driver.git
-[submodule "src/modules/odrive_ros"]
-	path = src/modules/odrive_ros
-	url = git@github.com:robotique-ecam/odrive_ros.git

src/asterix/param/asterix.in.yml

@@ -1,10 +1,6 @@
 !Defaults
 footprint: "[[-0.095, -0.128], [-0.095, 0.128], [0.095, 0.128], [0.095, -0.128]]"
 use_sim_time: False
-max_steps_frequency: 10000
-speedramp_resolution: 128
-microsteps: 16
-steps_per_turn: 200
 wheels_separation: 0.244
 wheels_radius: 0.0405
 min_turning_radius: 0.0
@@ -21,17 +17,14 @@ asterix:
   drive_node:
     ros__parameters:
       use_sim_time: !Var use_sim_time
+      odrive_usb_port: "/dev/ttyS1"
+      invert_wheel: false
 
       wheels:
         separation: !Var wheels_separation
         radius: !Var wheels_radius
 
-      microsteps: !Var microsteps
-      steps_per_turn: !Var steps_per_turn
-
-      microcontroler:
-        max_steps_frequency: !Var max_steps_frequency
-        speedramp_resolution: !Var speedramp_resolution
+      gearbox_ratio: 1.0
 
       i2c_bus: 1
 
@@ -70,7 +63,7 @@ asterix:
   tree:
     ros__parameters:
       use_sim_time: !Var use_sim_time
-      
+
 
 controller_server:
   ros__parameters:

src/modules/drive/CMakeLists.txt

@@ -48,8 +48,17 @@ set(dependencies
   "actuators_srvs"
 )
 
+set(odrive_dependencies
+  "rclcpp"
+  "std_msgs"
+)
+
 set(EXEC_NAME "drive")
+set(ODRIVE_LIB "odrive")
 
+add_library(${ODRIVE_LIB} SHARED
+  src/ODrive.cpp
+)
 
 add_executable(${EXEC_NAME}
   src/drive_node.cpp
@@ -82,12 +91,23 @@ if(TIMER)
   add_definitions(-DUSE_TIMER)
 endif()
 
+ament_target_dependencies(${ODRIVE_LIB} ${odrive_dependencies})
+
+target_link_libraries(${EXEC_NAME} ${ODRIVE_LIB})
+
 ament_target_dependencies(${EXEC_NAME} ${dependencies})
 
 
 ################################################################################
 # Install
 ################################################################################
+install(TARGETS
+  ${ODRIVE_LIB}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 install(TARGETS ${EXEC_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )

src/modules/drive/include/ODrive.hpp

@@ -0,0 +1,72 @@
+/* BSD 3-Clause License
+
+Copyright (c) 2021, Framework Robotics GmbH
+All rights reserved.
+
+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 <stdio.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+#include <unistd.h>
+#include <iostream>
+#include <sys/file.h>
+#include <stdexcept>
+#include <regex>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include "rclcpp/rclcpp.hpp"
+
+class ODrive
+{
+public:
+    ODrive(const std::string port, const rclcpp::Node *node);
+    ~ODrive();
+
+    // Commands
+    int set_velocity(const int motor, const float velocity);
+    int stop();
+
+    // Getters
+    std::pair<float, float> get_position_velocity(const int motor_number);
+
+    // General helper functions
+    int checksum(const std::string cmd);
+
+private:
+    int open_port(const std::string port);
+    int send(const std::string cmd, const std::string *funct_name);
+    std::string receive(const std::string *funct_name);
+    bool check_double_output(const std::string s, const std::string *funct_name);
+
+    const rclcpp::Node *node;
+
+    int fd;
+};

src/modules/drive/include/drive.hpp

@@ -2,13 +2,19 @@
 #define DRIVE_NODE_HPP
 
 #ifndef SIMULATION
+
+#include <ODrive.hpp>
 #include "i2c.hpp"
+
 #else
+
 #include <limits>
 #include <webots/Motor.hpp>
 #include <webots/Robot.hpp>
 #include <webots/PositionSensor.hpp>
-#endif
+
+#endif //SIMULATION
+
 #include "std_srvs/srv/set_bool.hpp"
 #include "std_srvs/srv/trigger.hpp"
 #include "actuators_srvs/srv/slider.hpp"
@@ -31,10 +37,6 @@
 #define LEFT 0
 #define RIGHT 1
 
-#define STEPPER_LEFT 1
-#define STEPPER_RIGHT 2
-#define STEPPER_CMD -128
-
 using namespace rclcpp;
 using namespace std::chrono;
 
@@ -44,18 +46,6 @@ class Drive : public rclcpp::Node {
   ~Drive();
 
 private:
-  struct TinyCMD {
-    /* ATTiny speed command to I2C */
-    int8_t left = 0;
-    int8_t right = 0;
-  };
-
-  struct TinyData {
-    /* ATTiny steps from I2C */
-    int32_t left = 0;
-    int32_t right = 0;
-  };
-
   struct Differential {
     /* Computed values of d and theta from steps */
     double left = 0;
@@ -75,24 +65,34 @@ class Drive : public rclcpp::Node {
   };
 
 #ifndef SIMULATION
-  // For communicating with ATTiny85 over I2C
+  std::string _odrive_usb_port;
+  bool _invert_wheel;
+  int odrive_motor_order[2] = {0, 1};
+  double _gearbox_ratio;
+  Differential old_motor_turns_returned;
+  ODrive *odrive;
+  void get_motors_turns_from_odrive(double &left, double &right);
+  float compute_velocity_cmd(double velocity);
+  // For communicating with the slider over I2C
   int i2c_bus;
   std::shared_ptr<I2C> i2c;
   std::mutex i2c_mutex;
 #else
   std::shared_ptr<webots::Robot> wb_robot;
-  webots::Motor *wb_left_motor;
-  webots::Motor *wb_right_motor;
-  webots::PositionSensor *wp_left_encoder;
-  webots::PositionSensor *wp_right_encoder;
-  Differential old_steps_returned;
+  webots::Motor
+    *wb_left_motor, *wb_right_motor;
+  webots::PositionSensor
+    *wp_left_encoder, *wp_right_encoder;
+  Differential old_turns_returned;
   double timestep;
   rclcpp::TimerBase::SharedPtr time_stepper_;
-#endif
+  void sim_step();
+  rclcpp::Time get_sim_time();
+#endif //SIMULATION
 
   // ROS time
-  rclcpp::Time time_since_last_sync_;
-  rclcpp::Time previous_time_since_last_sync_;
+  rclcpp::Time
+    time_since_last_sync_, previous_time_since_last_sync_;
 
   // ROS topic publishers
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
@@ -125,41 +125,18 @@ class Drive : public rclcpp::Node {
   std::vector<geometry_msgs::msg::TransformStamped> _previous_tf;
   rclcpp::Subscription<geometry_msgs::msg::TransformStamped>::SharedPtr _adjust_odometry_sub;
 
-  double _accel;
-  double _wheel_separation;
-  double _wheel_radius;
-  int _max_freq;
-  int _speed_resolution;
-  int _steps_per_turn;
-  int _microsteps;
+  double _wheel_separation, _wheel_radius;
 
   /* Computed values */
-  uint16_t _total_steps_per_turn;
-  double _rads_per_step;
-  double _meters_per_turn;
-  double _meters_per_step;
-  double _speed_multiplier;
-  double _max_speed;
-  double _min_speed;
-
-  /* Temporary values */
-  double dt = 1; /* us */
-  double trajectory_radius = 0;
-  double trajectory_radius_left = 0;
-
-  bool sign_steps_left = false;
-  bool sign_steps_right = false;
-
-  TinyData attiny_speed_cmd_;
-  TinyData attiny_steps_returned_;
-  TinyCMD differential_speed_cmd_;
+  double _wheel_circumference, dt; /* us */
+
+  Differential
+    wheels_turns_returned_, cmd_vel_, differential_speed_, differential_move_;
+
+  Instantaneous
+    instantaneous_speed_, instantaneous_move_;
+
   OdometricPose odom_pose_;
-  Differential cmd_vel_;
-  Differential differential_speed_;
-  Differential differential_move_;
-  Instantaneous instantaneous_speed_;
-  Instantaneous instantaneous_move_;
-  rclcpp::TimerBase::SharedPtr serial_read_timer_;
 
   void init_parameters();
   void init_variables();
@@ -168,11 +145,8 @@ class Drive : public rclcpp::Node {
   void update_odometry();
   void update_velocity();
   void update_diagnostic();
-  void read_from_serial();
-  void compute_pose_velocity(TinyData steps_returned);
-  void steps_received_callback(int32_t steps, uint8_t id);
+  void compute_pose_velocity(Differential turns_returned);
   void command_velocity_callback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel_msg);
-  int8_t compute_velocity_cmd(double velocity);
   void handle_drivers_enable(const std::shared_ptr<rmw_request_id_t> request_header, const std_srvs::srv::SetBool::Request::SharedPtr request,
                              const std_srvs::srv::SetBool::Response::SharedPtr response);
   void handle_set_slider_position(const std::shared_ptr<rmw_request_id_t> request_header, const actuators_srvs::srv::Slider::Request::SharedPtr request,
@@ -186,11 +160,6 @@ class Drive : public rclcpp::Node {
   tf2::Vector3 get_tf2_vector(geometry_msgs::msg::Vector3 &p);
   void set_pose_from_tf_t_q(tf2::Vector3 &t, tf2::Quaternion &q, geometry_msgs::msg::PoseStamped &pose_out);
   double dummy_tree_digits_precision(double a);
-
-#ifdef SIMULATION
-  void sim_step();
-  rclcpp::Time get_sim_time();
-#endif /* SIMULATION */
 };
 
 #endif /* DRIVE_NODE_HPP */