dynamixel_interface_controller.h

Go to the documentation of this file.

/* CSIRO Open Source Software License Agreement (variation of the BSD / MIT License)
 * Copyright (c) 2020, Commonwealth Scientific and Industrial Research Organisation (CSIRO) ABN 41 687 119 230.
 * All rights reserved. CSIRO is willing to grant you a license to the dynamixel_actuator ROS packages on the following
 * terms, except where otherwise indicated for third party material. Redistribution and use of this software 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 CSIRO nor the names of its contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission of CSIRO.
 *
 * EXCEPT AS EXPRESSLY STATED IN THIS AGREEMENT AND TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, THE SOFTWARE IS
 * PROVIDED "AS-IS". CSIRO MAKES NO REPRESENTATIONS, WARRANTIES OR CONDITIONS OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
 * BUT NOT LIMITED TO ANY REPRESENTATIONS, WARRANTIES OR CONDITIONS REGARDING THE CONTENTS OR ACCURACY OF THE SOFTWARE,
 * OR OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT, THE ABSENCE OF LATENT OR OTHER
 * DEFECTS, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT DISCOVERABLE.
 * TO THE FULL EXTENT PERMITTED BY APPLICABLE LAW, IN NO EVENT SHALL CSIRO BE LIABLE ON ANY LEGAL THEORY (INCLUDING,
 * WITHOUT LIMITATION, IN AN ACTION FOR BREACH OF CONTRACT, NEGLIGENCE OR OTHERWISE) FOR ANY CLAIM, LOSS, DAMAGES OR
 * OTHER LIABILITY HOWSOEVER INCURRED.  WITHOUT LIMITING THE SCOPE OF THE PREVIOUS SENTENCE THE EXCLUSION OF LIABILITY
 * SHALL INCLUDE: LOSS OF PRODUCTION OR OPERATION TIME, LOSS, DAMAGE OR CORRUPTION OF DATA OR RECORDS; OR LOSS OF
 * ANTICIPATED SAVINGS, OPPORTUNITY, REVENUE, PROFIT OR GOODWILL, OR OTHER ECONOMIC LOSS; OR ANY SPECIAL, INCIDENTAL,
 * INDIRECT, CONSEQUENTIAL, PUNITIVE OR EXEMPLARY DAMAGES, ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, ACCESS
 * OF THE SOFTWARE OR ANY OTHER DEALINGS WITH THE SOFTWARE, EVEN IF CSIRO HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
 * CLAIM, LOSS, DAMAGES OR OTHER LIABILITY. APPLICABLE LEGISLATION SUCH AS THE AUSTRALIAN CONSUMER LAW MAY APPLY
 * REPRESENTATIONS, WARRANTIES, OR CONDITIONS, OR IMPOSES OBLIGATIONS OR LIABILITY ON CSIRO THAT CANNOT BE EXCLUDED,
 * RESTRICTED OR MODIFIED TO THE FULL EXTENT SET OUT IN THE EXPRESS TERMS OF THIS CLAUSE ABOVE "CONSUMER GUARANTEES".
 * TO THE EXTENT THAT SUCH CONSUMER GUARANTEES CONTINUE TO APPLY, THEN TO THE FULL EXTENT PERMITTED BY THE APPLICABLE
 * LEGISLATION, THE LIABILITY OF CSIRO UNDER THE RELEVANT CONSUMER GUARANTEE IS LIMITED (WHERE PERMITTED AT CSIRO'S
 * OPTION) TO ONE OF FOLLOWING REMEDIES OR SUBSTANTIALLY EQUIVALENT REMEDIES:
 * (a)  THE REPLACEMENT OF THE SOFTWARE, THE SUPPLY OF EQUIVALENT SOFTWARE, OR SUPPLYING RELEVANT SERVICES AGAIN;
 * (b)  THE REPAIR OF THE SOFTWARE;
 * (c)  THE PAYMENT OF THE COST OF REPLACING THE SOFTWARE, OF ACQUIRING EQUIVALENT SOFTWARE, HAVING THE RELEVANT
 *      SERVICES SUPPLIED AGAIN, OR HAVING THE SOFTWARE REPAIRED.
 * IN THIS CLAUSE, CSIRO INCLUDES ANY THIRD PARTY AUTHOR OR OWNER OF ANY PART OF THE SOFTWARE OR MATERIAL DISTRIBUTED
 * WITH IT.  CSIRO MAY ENFORCE ANY RIGHTS ON BEHALF OF THE RELEVANT THIRD PARTY.
 *
 * Third Party Components:
 *
 * The following third party components are distributed with the Software.  You agree to comply with the license terms
 * for these components as part of accessing the Software.  Other third party software may also be identified in
 * separate files distributed with the Software.
 * ___________________________________________________________________
 *
 * dynamixel_interface is forked from projects authored by Brian
 * Axelrod (on behalf of Willow Garage):
 *
 * https://github.com/baxelrod/dynamixel_pro_controller
 * https://github.com/baxelrod/dynamixel_pro_driver
 *
 * Thus they retain the following notice:
 *
 * Software License Agreement (BSD License)
 * Copyright (c) 2013, Willow Garage
 * 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 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 DYNAMIXEL_INTERFACE_CONTROLLER_H_
#define DYNAMIXEL_INTERFACE_CONTROLLER_H_

#include <algorithm>
#include <map>
#include <memory>
#include <mutex>
#include <string>

#include <XmlRpcValue.h>

#include <ros/callback_queue.h>
#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/JointState.h>

#include <dynamixel_interface/dynamixel_interface_driver.h>

#include <dynamixel_interface/DataPort.h>
#include <dynamixel_interface/DataPorts.h>
#include <dynamixel_interface/ServoDiag.h>
#include <dynamixel_interface/ServoDiags.h>

namespace dynamixel_interface
{
struct DynamixelInfo
{
  int id;                  
  std::string joint_name;  

  double max_vel;       
  double torque_limit;  

  int zero_pos;  
  int min_pos;   
  int max_pos;   

  const DynamixelSpec *model_spec;  

  bool torque_enabled;                
  DynamixelControlMode current_mode;  
  uint8_t hardware_status;            
};

struct PortInfo
{
  std::string port_name;  

  std::string device;  
  int baudrate;        

  bool use_legacy_protocol;  

  std::unique_ptr<DynamixelInterfaceDriver> driver;  

  std::unordered_map<std::string, DynamixelInfo> joints;  
};

class DynamixelInterfaceController
{
public:
  DynamixelInterfaceController();

  ~DynamixelInterfaceController();

  bool parseParameters(void);

  //      - check response on bus
  bool initialise();

  inline double getLoopRate(void) { return loop_rate_; };

  void loop(void);

private:
  bool parsePortInformation(XmlRpc::XmlRpcValue ports);

  bool parseServoInformation(PortInfo &port, XmlRpc::XmlRpcValue servos);

  bool initialisePort(PortInfo &port);

  bool initialiseDynamixel(PortInfo &port, DynamixelInfo &dynamixel);

  void jointStateCallback(const sensor_msgs::JointState::ConstPtr &joint_commands);

  void multiThreadedIO(PortInfo &port, sensor_msgs::JointState &read_msg, dynamixel_interface::DataPorts &dataport_msg,
                       dynamixel_interface::ServoDiags &status_msg, bool perform_write) const;

  void multiThreadedWrite(PortInfo &port, sensor_msgs::JointState joint_commands) const;

  void multiThreadedRead(PortInfo &port, sensor_msgs::JointState &read_msg,
                         dynamixel_interface::DataPorts &dataports_msg,
                         dynamixel_interface::ServoDiags &diags_msg) const;

  std::unique_ptr<ros::NodeHandle> nh_;  

  std::vector<PortInfo> dynamixel_ports_;  

  sensor_msgs::JointState write_msg_;  

  std::mutex write_mutex_;    
  bool write_ready_ = false;  

  ros::Subscriber joint_state_subscriber_;  

  ros::Publisher joint_state_publisher_;  
  ros::Publisher diagnostics_publisher_;  
  ros::Publisher dataport_publisher_;     
  ros::Publisher debug_publisher_;        

  ros::Timer broadcast_timer_;  

  // variables for tracking diagnostics reading rate
  bool read_diagnostics_;         
  uint diagnostics_counter_ = 0;  
  uint diagnostics_iters_ = 0;    

  // variables for tracking dataport reading rate
  bool read_dataport_;         
  uint dataport_counter_ = 0;  
  uint dataport_iters_ = 0;    

  DynamixelControlMode control_type_;  

  bool parameters_parsed_ = false;  
  bool initialised_ = false;        

  bool stop_motors_on_shutdown_;  
  bool ignore_input_velocity_;    

  double global_max_vel_;       
  double global_torque_limit_;  

  double loop_rate_;         
  double diagnostics_rate_;  
  double dataport_rate_;     
  int recv_queue_size_ = 1;  
};

}  // namespace dynamixel_interface

#endif  // DYNAMIXEL_INTERFACE_CONTROLLER_H_