diff --git a/ros_ethercat_hardware/src/ethercat_hardware.cpp b/ros_ethercat_hardware/src/ethercat_hardware.cpp index 646a223..1e7f17c 100644 --- a/ros_ethercat_hardware/src/ethercat_hardware.cpp +++ b/ros_ethercat_hardware/src/ethercat_hardware.cpp @@ -555,7 +555,7 @@ 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_); @@ -563,7 +563,8 @@ void EthercatHardwareDiagnosticsPublisher::publishDiagnostics() 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 d(slaves_[i]); d->collectDiagnostics(oob_com_); } diff --git a/ros_ethercat_loop/src/main.cpp b/ros_ethercat_loop/src/main.cpp index 61832fb..d7428aa 100644 --- a/ros_ethercat_loop/src/main.cpp +++ b/ros_ethercat_loop/src/main.cpp @@ -207,6 +207,7 @@ void *diagnosticLoop(void *args) ptr_vector* ec = (ptr_vector*) args; struct timespec tick; clock_gettime(CLOCK_MONOTONIC, &tick); + ROS_WARN_STREAM("dl"); while (!g_quit) { for (ptr_vector::iterator eh = ec->begin(); eh != ec->end(); ++eh) @@ -672,4 +673,3 @@ int main(int argc, char *argv[]) return rv; } - diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h new file mode 100644 index 0000000..7638fe9 --- /dev/null +++ b/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 +#include + +#include +#include + + +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 {}; + +/// \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 diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index ce28294..32f50bd 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -37,6 +37,7 @@ #include #include +//#include #include @@ -44,6 +45,32 @@ namespace ros_ethercat_model { +typedef enum ActuatorCommandMode +{ + COMMAND_TYPE_PWM = 0, + COMMAND_TYPE_EFFORT = 1 +} +ActuatorCommandMode; + +inline std::vector command_types_to_string() +{ + std::vector 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 diff --git a/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp b/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp index 7872d57..73ea956 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp @@ -43,6 +43,8 @@ #include #include +#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 diff --git a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp index 8d54e4b..1e3b65a 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp +++ b/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) { diff --git a/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp b/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp index c47cfbb..a0e0211 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp +++ b/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()); } } diff --git a/ros_ethercat_model/launch/joint_state_publisher.launch b/ros_ethercat_model/launch/joint_state_publisher.launch index 1d93e74..ad181ad 100644 --- a/ros_ethercat_model/launch/joint_state_publisher.launch +++ b/ros_ethercat_model/launch/joint_state_publisher.launch @@ -1,8 +1,11 @@ + + - + +