这篇博客通过ros与gazebo的默认方式控制了joint的转动 ， 本篇博客从源码出发，探究joint是如何通过ros在gazebo里被控制的。
大体分为两部分：1、gazebo里的硬件抽象构建 2、ros里的控制器
相关源码git地址：
gazebo里的硬件抽象构建：[gazebo_ros_pkgs里面的gazebo_ros_control](https://github.com/ros-simulation/gazebo_ros_pkgs)
gazebo_ros_control 插件功能包
首先来看 gazebo_ros_control 插件功能包。
功能包结构如下：
为什么说是插件功能包呢，此功能包的主要作用并不是生成可执行的节点，而是生成可动态调用的插件。
什么时候调用的。
如何调用的。
调用的什么库。 在后面进行解答
package.xml
如何证明是插件功能包呢，看package.xml即可
ROS插件相关可参考：[ROS：pluginlib](https://blog.csdn.net/qq_32761549/article/details/103178683)robot_hw_sim_plugins.xml
接下来看plugin导入的文件
robot_hw_sim_plugins.xml
源码解释 ：其中
<library path="lib/libdefault_robot_hw_sim">说明了生成的插件或者叫库的地址 及 名称 。 库的生成在下面CMakeLists.txt解释中，
- [ ] 注意与此处对应
因为是默认的，也就是说ros里面自带的插件，所以在安装ros时，该库就存在在ros的系统包下：
自己写的插件会在自己的工作空间下的：
源码解释 : 其中
name="gazebo_ros_control/DefaultRobotHWSim"
type="gazebo_ros_control::DefaultRobotHWSim"
base_class_type="gazebo_ros_control::RobotHWSim">分别是 class的名称 、类型 、基类 会体现在该插件的源码中
- [ ] 注意与此处对应
CMakeLists.txt
里面生成插件的代码
生成的插件的name是default_robot_hw_sim ，所以对应robot_hw_sim_plugins.xml文件里的
< library path="lib/libdefault_robot_hw_sim">
总结：package.xml、CMakeLists.txt、robot_hw_sim_plugins.xml、default_robot_hw_sim.cpp 对应关系
画了下面的图，里面相同的颜色部分 表示其需要相互对应的地方
总结的目的是：在构建自己的插件的时候也需要根据默认方式对应
default_robot_hw_sim.cpp &&default_robot_hw_sim.h
最后从CMakeLists.txt文件中对应到了插件的源码文件
default_robot_hw_sim.cpp
下面对该源码文件进行探究。
- [ ] 然后再指明该库是如何调用的
default_robot_hw_sim.cpp和正常的C++ 文件没什么区别
要和default_robot_hw_sim.h文件联合起来看
主要是声明定义了类 gazebo_ros_control ::DefaultRobotHWSim
首先看 default_robot_hw_sim.h 文件
整体为 gazebo_ros_control 命名空间下
继承与父类： gazebo_ros_control::RobotHWSim
该类定义于robot_hw_sim.h，与default_robot_hw_sim.h在同一文件夹下
下面看看其父类gazebo_ros_control::RobotHWSim
父类 gazebo_ros_control::RobotHWSim
该类定义了gazebo plugin版本的RobotHW，RobotHW定义于ros control包中，定义了与实际机器人通讯的基本接口。
在编写实际机器人的ros control时， 该类是用户必须实现的类。 主要重载read(), write(), preparewitch() 以及doSwitch()四个接口。 在RobotHW中， 这四个函数都有默认实现（默认实现是空， 什么都不做）， 都不是纯虚函数。 一般而言， 重载读写函数即可正常工作了。
RobotHWSim继承于RobotHW， 将接口换了个名字， 几个纯虚函数都必须在子类中进行实现。
也就是
namespace gazebo_ros_control {
// Struct for passing loaded joint data
struct JointData
{
std::string name_;
std::string hardware_interface_;
JointData(const std::string& name, const std::string& hardware_interface) :
name_(name),
hardware_interface_(hardware_interface)
{}
};
/// \brief RobotHW 的gazebo插件版本
/// RobotHWSim 类对象代表 一个机器人的仿真硬件
class RobotHWSim : public hardware_interface::RobotHW
{
public:
virtual ~RobotHWSim() { }
//纯虚函数 完成 初始化 仿真机器人硬件 里面的参数是子类必须一致的
virtual bool initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions) = 0;
/// 从仿真机器人硬件读取状态数据 例如joint position velocities
virtual void readSim(ros::Time time, ros::Duration period) = 0;
///向仿真机器人硬件写 指令 例如 joint position and velocity commands
virtual void writeSim(ros::Time time, ros::Duration period) = 0;
/// 设置仿真机器人的紧急停止状态 The default implementation of this function does nothing.
virtual void eStopActive(const bool active) {}
};
}接下来继续看 gazebo_ros_control ::DefaultRobotHWSim 类
gazebo_ros_control ::DefaultRobotHWSim
该类的定义和声明就在上面的
default_robot_hw_sim.cpp &&default_robot_hw_sim.h 文件中
继承于 gazebo_ros_control::RobotHWSim
类的定义部分
首先看 default_robot_hw_sim.h 文件 类的定义部分
namespace gazebo_ros_control
{
class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
{
public:
//继承的四个虚函数 必须在子类中进行实现。 函数的实现在default_robot_hw_sim.cpp中
//功能与父类中注释的一样
virtual bool initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions);
virtual void readSim(ros::Time time, ros::Duration period);
virtual void writeSim(ros::Time time, ros::Duration period);
virtual void eStopActive(const bool active);
protected:
// Methods used to control a joint.
enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
// joint 限制得部分
void registerJointLimits(const std::string& joint_name,
const hardware_interface::JointHandle& joint_handle,
const ControlMethod ctrl_method,
const ros::NodeHandle& joint_limit_nh,
const urdf::Model *const urdf_model,
int *const joint_type, double *const lower_limit,
double *const upper_limit, double *const effort_limit);
unsigned int n_dof_;//关节个数
//关节接口
hardware_interface::JointStateInterface js_interface_;
hardware_interface::EffortJointInterface ej_interface_;
hardware_interface::PositionJointInterface pj_interface_;
hardware_interface::VelocityJointInterface vj_interface_;
//关节限制
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
std::vector<std::string> joint_names_;//关节名称
std::vector<int> joint_types_;//关节类型
std::vector<double> joint_lower_limits_;
std::vector<double> joint_upper_limits_;
std::vector<double> joint_effort_limits_;
std::vector<ControlMethod> joint_control_methods_;
std::vector<control_toolbox::Pid> pid_controllers_;//关节控制器
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> joint_effort_command_;
std::vector<double> joint_position_command_;
std::vector<double> last_joint_position_command_;
std::vector<double> joint_velocity_command_;
std::vector<gazebo::physics::JointPtr> sim_joints_;
std::string physics_type_;
// e_stop_active_ is true if the emergency stop is active.
bool e_stop_active_, last_e_stop_active_;
};
typedef boost::shared_ptr<DefaultRobotHWSim> DefaultRobotHWSimPtr;
}类函数定义部分
类函数定义部分在 default_robot_hw_sim.cpp 文件中
一共5个函数
- initSim()
- readSim()
- writeSim()
- eStopActive()
- registerJointLimits()
DefaultRobotHWSim::initSim()
太长了，放新博客里
DefaultRobotHWSim::initSim()详解
DefaultRobotHWSim::readSim()
源码部分（已加注释）
较短不进行分块解释
void DefaultRobotHWSim::readSim(ros::Time time, ros::Duration period)
{
//遍历每个 joint
for(unsigned int j=0; j < n_dof_; j++)
{
// Gazebo has an interesting API...
#if GAZEBO_MAJOR_VERSION >= 8
double position = sim_joints_[j]->Position(0);
#else
//得到joint得角度（弧度）
// 具体怎么得到的 要看 gazebo::physics::JointPtr GetAngle(0) 函数
double position = sim_joints_[j]->GetAngle(0).Radian();
#endif
if (joint_types_[j] == urdf::Joint::PRISMATIC) //如果 joint_types_ 是 PRISMATIC（原始的）
{
joint_position_[j] = position;
}
else // 其它 得 实际上是 以哪个为0点 PRISMATIC以默认0点为0点 其它以初始位置为0点
{
joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
position);
}
joint_velocity_[j] = sim_joints_[j]->GetVelocity(0);//得到joint得角速
joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));//得到joint得力
}
}DefaultRobotHWSim::writeSim()
太长了，放新博客里
DefaultRobotHWSim::writeSim()详解
DefaultRobotHWSim::eStopActive()
void DefaultRobotHWSim::eStopActive(const bool active)
{
e_stop_active_ = active;
}这部分简单 就是进行 急停状态设置
DefaultRobotHWSim::registerJointLimits()
joint 关节限制的相关内容
- [ ] 先不看了
gazebo_ros_control_plugin.cpp &&gazebo_ros_control_plugin.h
在功能包下还可以看到这两个源码文件，在CMakelists.txt文件中，指明了该源码对应生成
gazebo_ros_control.so文件
CMakelists.txt关键部分如下
而我们在xacro中直接调用的也是gazebo_ros_control.so，然后在该标签下才用了DefaultRobotHWSim.so的插件
所以gazebo_ros_control_plugin.cpp对应生成的库的一个很大的作用就是加载调用DefaultRobotHWSim或者用户自己定义的robotSimType也就是自己写的类似DefaultRobotHWSim的插件的功能。
具体如何实现的要看其中的源码
从.h文件中可以看到 GazeboRosControlPlugin 继承于 gazebo::ModelPlugin 类
是由Gazebo加载的Plugin，其中gazebo是如何加载该类的就不向下探究gazebo的内容了。
下面 弄清楚该类是如何加载调用 robotSimType 标签下的插件功能同时弄清楚了上面DefaultRobotHWSim插件的原理就可以仿照自己写插件了。
其中最主要的就是
void GazeboRosControlPlugin::Load（）
void GazeboRosControlPlugin::Update()
两个函数
void GazeboRosControlPlugin::Load（）
GazeboRosControlPlugin::Load（） 函数详解
void GazeboRosControlPlugin::Update()
比较简单
调用 默认或者自己插件写的那个readSim(sim_time_ros, sim_period) 函数
调用controller_manager 的 更新函数 
调用 默认或者自己插件写的那个readSim(sim_time_ros, sim_period) 函数
