ROS-Industrial 之 industrial_robot_client 分析理解(2)

在之前的ROS-Industrial 之 industrial_robot_client 分析理解(1)中分析過client包提供的共用的函數，還有將機器人的狀態信息與關節信息轉換到ROS進行發佈的兩個類，這裏着重理解讀取機器人狀態信息的接口。

ROBOT_STATE_INTERFACE

先來看一下機器人狀態信息接口的整個類結構：

#ifndef ROBOT_STATE_INTERFACE_H
#define ROBOT_STATE_INTERFACE_H

#include <vector>
#include <string>
#include "simple_message/smpl_msg_connection.h"
#include "simple_message/message_manager.h"
#include "simple_message/message_handler.h"
#include "simple_message/socket/tcp_client.h"
#include "industrial_robot_client/joint_relay_handler.h"
#include "industrial_robot_client/robot_status_relay_handler.h"

namespace industrial_robot_client
{
  namespace robot_state_interface
  {
    using industrial::smpl_msg_connection::SmplMsgConnection;
    using industrial::message_manager::MessageManager;
    using industrial::message_handler::MessageHandler;
    using industrial::tcp_client::TcpClient;
    using industrial_robot_client::joint_relay_handler::JointRelayHandler;
    using industrial_robot_client::robot_status_relay_handler::RobotStatusRelayHandler;
    namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts;

    //從機器人控制中讀取機器人狀態數據並將信息發佈到與之匹配的ROS話題中的通用模版
    class RobotStateInterface
    {
    public:
      RobotStateInterface();
      //用缺省的方法初始化機器人的通訊連接
      //default_ip: 用於對機器人進行連接的缺省IP地址[OPTIONAL]
      //default_port：用於機器人連接的缺省端口號[OPTIONAL]
      bool init(std::string default_ip = "", int default_port = StandardSocketPorts::STATE);

      //初始化通訊連接句柄
      bool init(SmplMsgConnection* connection);

      //初始化通訊連接句柄與關節名稱
      bool init(SmplMsgConnection* connection, std::vector<std::string>& joint_names);

      //開始處理信息併發布到指定話題
      void run();

      //獲取當前通訊句柄（通訊實例）
      SmplMsgConnection* get_connection()
      { return this->connection_; }

      //獲取活動消息管理器對象
      MessageManager* get_manager()
      { return &this->manager_;   }

      //獲取關節名稱
      std::vector<std::string> get_joint_names()
      { return this->joint_names_; }

      //增加一個新的處理器
      void add_handler(MessageHandler* handler, bool allow_replace = true)
      { this->manager_.add(handler, allow_replace); }

    protected:
      TcpClient default_tcp_connection_;
      JointRelayHandler default_joint_handler_;
      RobotStatusRelayHandler default_robot_status_handler_;
      SmplMsgConnection* connection_;
      MessageManager manager_;
      std::vector<std::string> joint_names_;
};//class RobotStateInterface
}//robot_state_interface
}//industrial_robot_cliet
#endif /* ROBOT_STATE_INTERFACE_H */

【先放着，要寫一些東西，進行通信調整】

JOINT_TRAJECTORY_INTERFACE

這個類主要實現的功能與之前的relay_handler正好相反，relay_handler是把機器人的信息轉述到ROS中，而JOINT_TRAJECTORY_INTERFACE的功能是把ROS中生成的控制機器人的信息轉換成實際的機器人控制器可以理解的數據格式。直接藉助代碼對其實現的功能進行理解（將整個代碼分段理解，方便解釋）:

#ifndef JOINT_TRAJECTORY_INTERFACE_H
#define JOINT_TRAJECTORY_INTERFACE_H
//包含必要的頭文件
#include <map>
#include <vector>
#include <string>
#include "ros/ros.h"
#include "industrial_msgs/CmdJointTrajectory.h"
#include "industrial_msgs/StopMotion.h"
#include "sensor_msgs/JointState.h"
#include "simple_message/smpl_msg_connection.h"
#include "simple_message/socket/tcp_client.h"
#include "simple_message/messages/joint_traj_pt_message.h"
#include "trajectory_msgs/JointTrajectory.h"

在以上的文件頭部信息中，對關節數據接口所需要的信息格式或變量格式進行了頭文件的包含，同時也包含有類似StopMotion.h這種由ROS的msg直接生成的頭文件。

namespace industrial_robot_client
{
namespace joint_trajectory_interface
{
  //使用對應的命名空間
  using industrial::smpl_msg_connection::SmplMsgConnection;
  using industrial::tcp_client::TcpClient;
  using industrial::joint_traj_pt_message::JointTrajPtMessage;
  namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts;

這一段聲明瞭兩個嵌套的命名空間，一個是整個package的命名空間，一個是具體接口的命名空間，並且在當前空間中還對要使用的命名空間進行了聲明。

class JointTrajectoryInterface

這一行對類的定義以及下面所有對類功能的聲明，組成了ROS中機器人的軌跡控制接口，其功能是把ROS中生成的控制機器人的信息轉換成實際的機器人控制器可以理解的數據格式。

{
public:
    //構造函數
    JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {};
    //析構函數
    ~virtual ~JointTrajectoryInterface();

類的構造函數，因爲這個類並沒有繼承其它的類，所以構造函數後面對應的代碼對類內的部分成員進行了初始化。

/**
* \簡介   用缺省的方法初始化與機器人的連接
* \參數   default_ip  用於與機器人控制器進行連接的缺省IP地址[OPTIONAL]
*                     - 如果ROS參數"robot_ip_address"無效，則將使用缺省設置
* \參數   default_port 與機器人控制器連接的缺省端口[OPTIONAL]
*                     - 如果ROS參數"~port"無法使用，將使用這裏設置的缺省值
* \返回值 成功返回true, 否則返回false
*/
virtual bool init(std::string default_ip = "", int default_port = StandardSocketPorts::MOTION);

這個函數屬於虛擬函數，就不多講了。

/**
* \簡介 用指定的方法與機器人進行連接
* \參數 connection 新的機器人連接實例(ALREADY INITIALIZED)
* \返回值 成功返回true, 否則返回false
*/
virtual bool init(SmplMsgConnection* connection);

這個函數也是虛擬函數，具體的實現因類而異。

/**
* \簡介 類的初始化函數
*
* \參數 connection 用來向機器人發送命令的簡單消息連接(ALREADY INITIALIZED)
* \參數 joint_names 期望的關節名稱列表
*   - 計數和順序應該匹配向機器人連接發送的數據格式
*   - 用一個空間名稱插入一個關節位置佔位符(typ. 0.0)
* \參數 velocity_limits 對應於每個關節的最大速度限制
*   - 保持爲空，如果要從URDF文件中直接讀取關節速度限制
* \返回值 成功返回true, 否則返回false
*/
virtual bool init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
                    const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());

虛擬函數，具體的實現因類而異，初始化對應於機器人控制器的連接，關節名稱，關節限制。

/**
* \簡介 處理消息發佈話題
*/
virtual void run() { ros::spin(); }

運行函數，在類初始化之後進行功能執行。

protected:
/**
* \簡介 向機器人發送停止指令
*/
virtual void trajectoryStop();

虛擬函數，實現機器人軌跡停止功能。

/**
* \簡介 將ROS trajectory message轉換成JointTrajPtMessages的流數據以便發送給機器人控制器，也可以在之後的繼承類中對函數進行重寫以實現對特定機器人的控制。
* \參數[in] traj ROS的JointTrajectory message類型
* \參數[out] msgs 列舉JointTrajPtMessages信息用於機器人控制
* \返回值 成功返回true, 否則返回false
*/
virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<JointTrajPtMessage>* msgs);

這個虛擬函數實現了將ROS的軌跡數據類型向機器人控制器的軌跡數據類型的轉換。

/**
* \簡介 在發佈信息之前對其進行轉換，對於特定的機器人可以進行重寫
* \參數[in] pt_in 軌跡點
* \參數[out] pt_out 轉換後的軌跡點
* \返回值 成功返回true, 否則返回false
*/
virtual bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out)
{
    *pt_out = pt_in;  // by default, no transform is applied
    return true;
}

默認情況下不進行關節數據的轉換。

/**
* \簡介 選擇指定的關節數據發送給機器人
* \參數[in] ros_joint_names 來自ROS command的關節名稱
* \參數[in] ros_pt 來自ROS command的目標位置或速度
* \參數[in] rbt_joint_names 關節名稱, 匹配機器人連接中的關節次序
* \參數[out] rbt_pt  與rbt_joint_names相匹配的目標點位置或速度
* \返回值 成功返回true, 否則返回false
*/
virtual bool select(const std::vector<std::string>& ros_joint_names, const trajectory_msgs::JointTrajectoryPoint& ros_pt,const std::vector<std::string>& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt);

這個函數用來向機器人發送指定關節的數據，索引便是關節名稱。

/**
* \簡介 將ros速度命令(每關節速度)降爲單個標量，以便與機器人通信
* \參數[in] pt 軌跡點數據, 匹配機器人連接中的關節次序
* \參數[out] rbt_velocity 爲機器人信息計算速度標量
* \參數[out] rbt_duration 計算機器人信息的運動間隔
* \返回值 成功返回true, 否則返回false
*/
virtual bool calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration);

爲了靈活起見，機器人命令信息包含“速度”和“持續時間”兩個字段。專用機器人的實現可以酌情利用這兩個字段中的任何一個或兩個字段。

/**
* \簡介 將ros速度命令(每關節速度)降爲單個標量，以便與機器人通信
* 如果服務沒有使用，則設置爲0或其它任何值。
* \參數[in] pt 軌跡點數據, 匹配機器人連接中的關節次序
* \參數[out] rbt_velocity 爲機器人信息計算速度標量
* \返回值 成功返回true, 否則返回false
*/
virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity);

將ros速度命令(每關節速度)降爲單個標量，以便與機器人通信。

/**
* \簡介 計算與機器人通信的預期移動持續時間。如果服務器不需要則設置爲0（或任意值）。
* \參數[in] pt 軌跡點數據, 匹配機器人連接中的關節次序
* \參數[out] rbt_duration計算機器人信息的運動間隔
* \返回值 成功返回true, 否則返回false
*/
virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration);

計算與機器人通信的預期移動持續時間。如果服務器不需要則設置爲0（或任意值）。

  /**
   * \簡介 向機器人發送軌跡數據
   * \參數 messages List of SimpleMessage JointTrajPtMessages to send to robot.
   * \返回值 成功返回true, 否則返回false
   */
  virtual bool send_to_robot(const std::vector<JointTrajPtMessage>& messages)=0;

向機器人控制器發送軌跡數據的函數。對於特定的機器人要重寫此函數。

/**
* \簡介 ROS topic-subscribe調用函數，將消息轉換成simple message對象並傳送給機器人
* \參數 msg 來自ROS的軌跡規劃器的JointTrajectory message 
*/
virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);

軌跡規劃器的消息向控制器信息的轉換與發送。

/**
* \簡介 ROS stopMotion service的調用函數，向機器人發送停止命令
* \參數 req StopMotion request from service call
* \參數 res StopMotion response to service call
* \返回值 總是返回true. Look at res.code.val to see if call actually succeeded.
*/
virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req,
                                    industrial_msgs::StopMotion::Response &res);

向控制器發送停止命令。

/**
* \簡介 檢查軌跡命令是否滿足最低要求
* \參數 traj 傳遞來的軌跡參數
* \返回值 如果軌跡有效則返回true, 否則返回false
*/
virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj);

這個函數用來測試軌跡的有效性。

/*
* \簡介 JointState話題的調用
* \參數 msg JointState message
*/
virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg);

關節狀態的調用函數。

TcpClient default_tcp_connection_;
ros::NodeHandle node_;
SmplMsgConnection* connection_;
ros::Subscriber sub_cur_pos_;  // 收聽關節狀態的話題
ros::Subscriber sub_joint_trajectory_; // 收聽關節軌跡話題
ros::ServiceServer srv_joint_trajectory_;  // 處理關節軌跡服務
ros::ServiceServer srv_stop_motion_;   // 處理停止狀態服務
std::vector<std::string> all_joint_names_;
double default_joint_pos_;  // 缺省關節位置
double default_vel_ratio_;  // 缺省速度比率
double default_duration_;   // 關節命令的缺省週期設置
std::map<std::string, double> joint_vel_limits_;  // 從URDF中讀取關節最大速度
sensor_msgs::JointState cur_joint_pos_;  // 獲取最後接收到的關節位置

類內的成員變量。

private:
  static JointTrajPtMessage create_message(int seq, std::vector<double> joint_pos, double velocity, double duration);

構建關節運動指令消息。

/**
* \簡介 ROS服務CmdJointTrajectory中的調用函數，複製消息-主題功能，但以服務形式出現。
* \參數 req CmdJointTrajectory 服務調用的 request
* \參數 res CmdJointTrajectory 服務調用的 response
* \返回值 總是返回true。通過res.code.val來分辨調用是否成功
*/
bool jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,industrial_msgs::CmdJointTrajectory::Response &res);
};
} //joint_trajectory_interface
} //industrial_robot_client
#endif /* JOINT_TRAJECTORY_INTERFACE_H */