Add motor test sequence as default autorun sequence

Author: ValentinPitre

niryo_one_driver/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
   niryo_one_msgs
   mcp_can_rpi
   dynamixel_sdk
+  urdf
 )
 
 find_package(Boost REQUIRED COMPONENTS system)
@@ -37,7 +38,7 @@ catkin_package(
 
 include_directories(include ${catkin_INCLUDE_DIRS})
 
-add_executable(niryo_one_driver
+add_executable(${PROJECT_NAME}
     src/utils/change_hardware_version.cpp
     src/utils/motor_offset_file_handler.cpp
     src/hw_driver/niryo_one_can_driver.cpp
@@ -48,6 +49,7 @@ add_executable(niryo_one_driver
     src/hw_comm/can_communication.cpp
     src/hw_comm/niryo_one_communication.cpp
     src/hw_comm/fake_communication.cpp
+    src/test_motors.cpp
     src/ros_interface.cpp
     src/rpi_diagnostics.cpp
     src/niryo_one_hardware_interface.cpp
@@ -74,5 +76,6 @@ else()
     )
 endif()
 
-add_dependencies(niryo_one_driver niryo_one_msgs_gencpp)
-
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+)

niryo_one_driver/include/niryo_one_driver/ros_interface.h

@@ -29,6 +29,7 @@
 #include "niryo_one_driver/communication_base.h"
 #include "niryo_one_driver/rpi_diagnostics.h"
 #include "niryo_one_driver/change_hardware_version.h"
+#include "niryo_one_driver/test_motors.h"
 
 #include "niryo_one_msgs/SetInt.h"
 #include "niryo_one_msgs/SetLeds.h"
@@ -68,11 +69,15 @@ class RosInterface {
         RpiDiagnostics* rpi_diagnostics;
         ros::NodeHandle nh_;
 
+        NiryoOneTestMotor test_motor; 
+
         bool* flag_reset_controllers;
         int hardware_version;
         bool learning_mode_on;
         int calibration_needed;
+        bool calibration_in_progress;
         bool last_connection_up_flag;
+        int motor_test_status;
 
         std::string rpi_image_version;
         std::string ros_niryo_one_version;
@@ -111,6 +116,8 @@ class RosInterface {
         ros::ServiceServer calibrate_motors_server;
         ros::ServiceServer request_new_calibration_server;
 
+        ros::ServiceServer test_motors_server;
+
         ros::ServiceServer activate_learning_mode_server;
         ros::ServiceServer activate_leds_server;
 
@@ -131,6 +138,7 @@ class RosInterface {
         ros::ServiceServer update_conveyor_id_server;
 
         // callbacks
+        bool callbackTestMotors(niryo_one_msgs::SetInt::Request &req, niryo_one_msgs::SetInt::Response &res);
 
         bool callbackCalibrateMotors(niryo_one_msgs::SetInt::Request &req, niryo_one_msgs::SetInt::Response &res);
         bool callbackRequestNewCalibration(niryo_one_msgs::SetInt::Request &req, niryo_one_msgs::SetInt::Response &res);

niryo_one_driver/include/niryo_one_driver/test_motors.h

@@ -0,0 +1,70 @@
+#ifndef NIRYO_TEST_MOTORS_H
+#define NIRYO_TEST_MOTORS_H
+
+
+#include <boost/shared_ptr.hpp>
+#include <ros/ros.h>
+#include <string>
+#include <thread>
+#include <queue>
+#include <functional>
+#include <vector>
+
+#include <urdf/model.h>
+
+#include <std_msgs/Float64MultiArray.h>
+#include <std_msgs/Int64MultiArray.h>
+// #include <moveit/move_group_interface/move_group_interface.h>
+
+#include <actionlib/client/simple_action_client.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+
+#include "niryo_one_msgs/SetBool.h"
+#include "niryo_one_msgs/SetInt.h"
+
+#include <std_msgs/Empty.h>
+#include <sensor_msgs/JointState.h>
+
+
+typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
+
+class NiryoOneTestMotor {
+
+    private:
+        ros::NodeHandle nh;
+        ros::ServiceClient calibrate_motor_client;
+        ros::Subscriber joint_state_subscriber;
+
+        ros::Publisher reset_stepper_publisher;
+
+        TrajClient* traj_client_;
+
+        std::vector<double> pose_start{0.0, 0.0, 0.3, 0.0, 0.0, 0.0};
+
+        bool enable_test;
+        int _n_joints = 6;
+        std::vector<std::string>  _joint_names;
+        std::vector<double>  _joint_upper_limits;
+        std::vector<double>  _joint_lower_limits;
+        std::vector<double>  _joint_has_position_limits;
+
+        std::vector<double> _current_joint_pose;
+
+
+    public:
+        NiryoOneTestMotor();
+
+        void callbackJointSate(const sensor_msgs::JointState& msg);
+        
+        bool getJointsLimits();
+
+        bool runTest(int nb_loops);
+        void stopTest();
+        void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal);
+        bool playTrajectory(control_msgs::FollowJointTrajectoryGoal goal);
+
+        control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory(std::vector<double> joint_positions);
+        actionlib::SimpleClientGoalState getState();
+
+};
+#endif

niryo_one_driver/package.xml

@@ -25,6 +25,7 @@
     <build_depend>trajectory_msgs</build_depend>
     <build_depend>dynamixel_sdk</build_depend>
     <build_depend>mcp_can_rpi</build_depend>
+    <build_depend>urdf</build_depend>
 
     <run_depend>roscpp</run_depend>
     <run_depend>rospy</run_depend>
@@ -42,6 +43,7 @@
     <run_depend>geometry_msgs</run_depend>
     <run_depend>sensor_msgs</run_depend>
     <run_depend>trajectory_msgs</run_depend>
+    <run_depend>urdf</run_depend>
 
     <export>
     </export>

niryo_one_driver/src/ros_interface.cpp

@@ -45,6 +45,49 @@ RosInterface::RosInterface(CommunicationBase* niryo_one_comm, RpiDiagnostics* rp
     calibration_needed = 0;
 }
 
+bool RosInterface::callbackTestMotors(niryo_one_msgs::SetInt::Request &req, niryo_one_msgs::SetInt::Response &res) 
+{    
+    if (motor_test_status==1)
+    {
+        test_motor.stopTest();
+        learning_mode_on = true;
+        comm->activateLearningMode(learning_mode_on);
+        return true;
+    }
+    
+    motor_test_status = 1;
+    if (calibration_needed)
+    {
+        learning_mode_on = false;
+        comm->activateLearningMode(learning_mode_on);
+        
+        int calibration_mode = 1; 
+        std::string result_message = "";
+        int result = comm->allowMotorsCalibrationToStart(calibration_mode, result_message);
+
+        ros::Duration(1).sleep();
+        while (calibration_in_progress) { ros::Duration(0.05).sleep();}
+
+        learning_mode_on = true;
+        ros::Duration(1).sleep();
+    }
+    
+    learning_mode_on = false;
+    comm->activateLearningMode(learning_mode_on);
+
+    bool status = test_motor.runTest(req.value);
+
+    learning_mode_on = true;
+    comm->activateLearningMode(learning_mode_on);
+
+    motor_test_status = status ? 0 : -1; 
+    res.status = status ? 200 : 400; 
+    res.message = status ? "Success" : "Fail";
+
+    return true;
+}
+
+
 bool RosInterface::callbackCalibrateMotors(niryo_one_msgs::SetInt::Request &req, niryo_one_msgs::SetInt::Response &res) 
 {
     int calibration_mode = req.value; 
@@ -230,6 +273,8 @@ void RosInterface::startServiceServers()
     calibrate_motors_server = nh_.advertiseService("niryo_one/calibrate_motors", &RosInterface::callbackCalibrateMotors, this);
     request_new_calibration_server = nh_.advertiseService("niryo_one/request_new_calibration", &RosInterface::callbackRequestNewCalibration, this);
 
+    test_motors_server = nh_.advertiseService("niryo_one/test_motors", &RosInterface::callbackTestMotors, this);
+
     activate_learning_mode_server = nh_.advertiseService("niryo_one/activate_learning_mode", &RosInterface::callbackActivateLearningMode, this);
     activate_leds_server = nh_.advertiseService("niryo_one/set_dxl_leds", &RosInterface::callbackActivateLeds, this);
 
@@ -260,7 +305,7 @@ void RosInterface::publishHardwareStatus()
         ros::Time time_now = ros::Time::now();
 
         bool connection_up = false;
-        bool calibration_in_progress = false;
+        
         std::string error_message;
         std::vector<std::string> motor_names;
         std::vector<std::string> motor_types;
@@ -287,6 +332,10 @@ void RosInterface::publishHardwareStatus()
         msg.rpi_temperature = rpi_diagnostics->getRpiCpuTemperature();
         msg.hardware_version = hardware_version;
         msg.connection_up = connection_up;
+        if (motor_test_status<0)
+        {
+            error_message += " motor test error";
+        }
         msg.error_message = error_message;
         msg.calibration_needed = calibration_needed;
         msg.calibration_in_progress = calibration_in_progress;