F motor odometry

ros_ethercat_hardware/src/ethercat_hardware.cpp

@@ -555,15 +555,16 @@ void EthercatHardwareDiagnosticsPublisher::publishDiagnostics()
 
   // status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");
   status_.addf("EtherCAT devices (expected)", "%d", num_ethercat_devices_);
-  status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_);
+  status_.addf("EtherCAT devices (current)", "%d", slaves_.size());
   ethernet_interface_info_.publishDiagnostics(status_);
   // status_.addf("Reset state", "%d", reset_state_);
 
   status_.addf("Timeout (us)", "%d", timeout_);
   status_.addf("Max PD Retries", "%d", max_pd_retries_);
 
   // Produce warning if number of devices changed after device initialization
-  if (num_ethercat_devices_ != diagnostics_.device_count_)
+
+  if (num_ethercat_devices_ != slaves_.size())
   {
     status_.mergeSummary(status_.WARN, "Number of EtherCAT devices changed");
   }
@@ -873,7 +874,7 @@ EthercatHardware::configSlave(EtherCAT_SlaveHandler *sh)
 
   if (matching_class_name.size() != 0)
   {
-    ROS_WARN("Using device '%s' with product code %d",
+    ROS_INFO("Using device '%s' with product code %d",
              device_loader_.getClassDescription(matching_class_name).c_str(),
              product_code);
     try
@@ -1030,6 +1031,8 @@ void EthercatHardware::loadNonEthercatDevices()
 
 void EthercatHardware::collectDiagnostics()
 {
+  ROS_WARN_STREAM("collect diagnostics");
+
   if (NULL == oob_com_)
     return;
 
@@ -1050,11 +1053,13 @@ void EthercatHardware::collectDiagnostics()
     oob_com_->txandrx(&frame);
 
     // Worry about locking for single value?
-    diagnostics_.device_count_ = status.get_adp();
+    //diagnostics_.device_count_ = status.get_adp();
+
   }
 
   for (unsigned i = 0; i < slaves_.size(); ++i)
   {
+
     boost::shared_ptr<EthercatDevice> d(slaves_[i]);
     d->collectDiagnostics(oob_com_);
   }

ros_ethercat_loop/src/main.cpp

@@ -207,6 +207,7 @@ void *diagnosticLoop(void *args)
   ptr_vector<EthercatHardware>* ec = (ptr_vector<EthercatHardware>*) args;
   struct timespec tick;
   clock_gettime(CLOCK_MONOTONIC, &tick);
+  ROS_WARN_STREAM("dl");
   while (!g_quit)
   {
     for (ptr_vector<EthercatHardware>::iterator eh = ec->begin(); eh != ec->end(); ++eh)
@@ -672,4 +673,3 @@ int main(int argc, char *argv[])
 
   return rv;
 }
-

ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h

@@ -0,0 +1,95 @@
+/*
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2020, Shadow Robot Company Ltd.
+*  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 the Willow Garage 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.
+*********************************************************************/
+
+#ifndef ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H
+#define ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H
+
+#include <hardware_interface/actuator_command_interface.h>
+#include <ros_ethercat_model/hardware_interface.hpp>
+
+#include <string>
+#include <vector>
+
+
+namespace ros_ethercat_model
+{
+
+/** \brief A handle used to read and command a single actuator. */
+class ActuatorHandle : public hardware_interface::ActuatorHandle
+{
+public:
+  ActuatorHandle() : hardware_interface::ActuatorHandle(), cmd_(0), cmd_type_(0) {}
+  ActuatorHandle(const ActuatorStateHandle& as, double* cmd, ActuatorCommandMode* cmd_type = 0)
+    : hardware_interface::ActuatorHandle(as, cmd), cmd_(cmd), cmd_type_(cmd_type) {}
+
+
+  void setCommand(double command, ActuatorCommandMode command_type)
+  {
+    assert(cmd_); *cmd_ = command;
+    *cmd_type_ = command_type;
+  }
+
+  void setState(ActuatorState* as)
+  {
+    a_ = as;
+  }
+
+  ActuatorState * getState(void)
+  {
+    assert(a_);
+    return a_;
+  }
+
+private:
+  ActuatorState *a_;
+  double* cmd_;
+  ActuatorCommandMode* cmd_type_;
+};
+
+class ActuatorCommandInterface : public hardware_interface::HardwareResourceManager<ActuatorHandle> {};
+
+/// \ref ActuatorCommandInterface for commanding effort-based actuators
+class EffortActuatorInterface : public ActuatorCommandInterface {};
+
+/// \ref ActuatorCommandInterface for commanding velocity-based actuators
+class VelocityActuatorInterface : public ActuatorCommandInterface {};
+
+/// \ref ActuatorCommandInterface for commanding position-based actuators
+class PositionActuatorInterface : public ActuatorCommandInterface {};
+
+
+}  // namespace ros_ethercat_model
+
+#endif  // ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H

ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp

@@ -37,13 +37,40 @@
 
 #include <string>
 #include <vector>
+//#include <ros_ethercat_model/actuator_command_interface.h>
 
 #include <ros/ros.h>
 
 
 namespace ros_ethercat_model
 {
 
+typedef enum ActuatorCommandMode
+{
+  COMMAND_TYPE_PWM = 0,
+  COMMAND_TYPE_EFFORT = 1
+}
+ActuatorCommandMode;
+
+inline std::vector<std::string> command_types_to_string()
+{
+  std::vector<std::string> command_type_strings;
+
+  command_type_strings.push_back("pwm");
+  command_type_strings.push_back("effort");
+
+  return command_type_strings;
+}
+
+typedef struct __attribute__((__packed__)) ActuatorOdometry
+{
+    volatile uint32_t odo_1;
+    volatile uint32_t odo_2;
+    volatile uint32_t odo_3;
+    volatile uint32_t odo_4;
+} ActuatorOdometry;
+
+
 class ActuatorState
 {
 public:
@@ -54,13 +81,19 @@ class ActuatorState
     velocity_(0),
     effort_(0),
     commanded_effort_(0),
+
     last_commanded_current_(0.0),
     last_measured_current_(0.0),
     last_commanded_effort_(0.0),
     last_measured_effort_(0.0),
     max_effort_(0.0),
     motor_voltage_(0.0),
-    flags_(0)
+    flags_(0),
+    pwm_(0),
+    clutch_slip_(0),
+    command_type_(COMMAND_TYPE_PWM),
+    last_command_time_(ros::Time::now())
+
   {
   }
 
@@ -74,11 +107,10 @@ class ActuatorState
   double commanded_effort_;
 
   double temperature_;  //!< Measured motor temperature in degrees C
-  unsigned int flags_;  //!< Motor state
-
+  unsigned int flags_;  //!< Motor info glags
+  signed int pwm_;      //!< PWM currently applied to motor
   double clutch_position_;  //!< Position of output of actuator, distally to the clutch.
 
-
   double last_commanded_current_;  //!< Current computed based on effort specified in ActuatorCommand (in amps)
   double last_measured_current_;  //!< The measured current (in amps)
 
@@ -87,7 +119,15 @@ class ActuatorState
 
   double max_effort_;  //!< Absolute torque limit for actuator (derived from motor current limit). (in Nm)
 
+  int clutch_slip_;
+
+  ros::Time last_command_time_;
+  ros::Duration command_timeout_;
+
   double motor_voltage_;  //!< Motor voltage (in volts)
+  ActuatorCommandMode command_type_;  // switch between pwm and effort
+  ActuatorOdometry odometry_;
+
 };
 
 class ActuatorCommand

ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp

@@ -43,6 +43,8 @@
 #include <string>
 #include <hardware_interface/imu_sensor_interface.h>
 
+#define INIT_ARRAY(array, x) for (size_t n = 0; n < (sizeof(array) / sizeof(array[0])); ++n) { array[n] =  x; }
+
 using std::string;
 
 namespace ros_ethercat_model
@@ -71,6 +73,17 @@ class ImuState
     data_.linear_acceleration_covariance = linear_acceleration_covariance_;
   };
   ImuState(){}
+
+  void initialiseToZero(void)
+  {
+    INIT_ARRAY(orientation_, 0.0);
+    INIT_ARRAY(angular_velocity_, 0.0);
+    INIT_ARRAY(linear_acceleration_, 0.0);
+    INIT_ARRAY(orientation_covariance_, 0.0);
+    INIT_ARRAY(angular_velocity_covariance_, 0.0);
+    INIT_ARRAY(linear_acceleration_covariance_, 0.0);
+  }
+
 };
 };  // namespace ros_ethercat_model
 

ros_ethercat_model/include/ros_ethercat_model/joint.hpp

@@ -149,6 +149,9 @@ class JointState
   /// The joint position in radians or meters (read-only variable)
   double position_;
 
+  // The raw value coming from the position sensor
+  int position_raw_;
+
   /// The joint velocity in radians/sec or meters/sec (read-only variable)
   double velocity_;
 
@@ -161,6 +164,52 @@ class JointState
   /// The position the joint should move to in radians or meters (write-to variable)
   double commanded_position_;
 
+  // Duplicates of command fields, specifically for reporting in finger states.
+  // This is due to other command fields being used/modified outside of DEX code.
+  double state_commanded_effort_;
+  double state_commanded_position_;
+  double state_commanded_velocity_;
+  double state_commanded_feedforward_effort_;
+  double state_commanded_position_effort_;
+
+  // Set functions for above commanded state variables.
+  void setCommandedEffort(double effort)
+  {
+    state_commanded_effort_ = effort;
+    state_commanded_position_ = 0;
+    state_commanded_velocity_ = 0;
+    state_commanded_feedforward_effort_ = effort;
+    state_commanded_position_effort_ = 0;
+  };
+
+  void setCommandedPosition(double position, double effort)
+  {
+    state_commanded_effort_ = effort;
+    state_commanded_position_ = position;
+    state_commanded_velocity_ = 0;
+    state_commanded_feedforward_effort_ = 0;
+    state_commanded_position_effort_ = effort;
+  };
+
+  void setCommandedVelocity(double position, double effort, double velocity)
+  {
+    state_commanded_effort_ = effort;
+    state_commanded_position_ = position;
+    state_commanded_velocity_ = velocity;
+    state_commanded_feedforward_effort_ = 0;
+    state_commanded_position_effort_ = effort;
+  };
+
+  void setCommandedForcePosition(double position, double effort, double feed_forward, double pid_output)
+  {
+    state_commanded_effort_ = effort;
+    state_commanded_position_ = position;
+    state_commanded_velocity_ = 0;
+    state_commanded_feedforward_effort_ = feed_forward;
+    state_commanded_position_effort_ = pid_output;
+  };
+
+
   /// The velocity the joint should move with in radians/sec or meters/sec (write-to variable)
   double commanded_velocity_;
 
@@ -176,11 +225,17 @@ class JointState
   /// Constructor
   JointState() :
     position_(0.0),
+    position_raw_(0),
     velocity_(0.0),
     effort_(0.0),
     commanded_position_(0.0),
     commanded_velocity_(0.0),
     commanded_effort_(0.0),
+    state_commanded_effort_(0.0),
+    state_commanded_position_(0.0),
+    state_commanded_velocity_(0.0),
+    state_commanded_feedforward_effort_(0.0),
+    state_commanded_position_effort_(0.0),
     calibrated_(false),
     reference_position_(0.0)
   {

ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp

@@ -140,7 +140,7 @@ class RobotState : public hardware_interface::HardwareInterface
     }
     catch (const std::runtime_error &ex)
     {
-      ROS_FATAL_STREAM("ros_ethercat_model failed to parse the URDF xml into a robot model\n" << ex.what());
+      ROS_WARN_STREAM("ros_ethercat_model failed to parse the URDF xml into a robot model\n" << ex.what());
     }
   }
 

ros_ethercat_model/launch/joint_state_publisher.launch

@@ -1,8 +1,11 @@
 <launch>
   <!-- load configuration -->
   <arg name="publish_rate" default='100'/>
+  <arg name="wait_for_calibrated" default="false" />
+
   <rosparam command="load" file="$(find ros_ethercat_model)/launch/joint_state_controller.yaml" />
   <param name="joint_state_controller/publish_rate" value="$(arg publish_rate)"/>
   <!-- spawn controller -->
-  <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="--wait-for=calibrated joint_state_controller" />
+  <node if="$(arg wait_for_calibrated)" name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="--wait-for=calibrated joint_state_controller" />
+  <node unless="$(arg wait_for_calibrated)" name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="joint_state_controller" />
 </launch>