ROS-3-Gazebo

1.为了更好的显示机器人的三维模型，我们调用Gazebo软件。先前我们通过joint_state_publisher来计算出机器人每个关节之间的坐标系关系，但是在这里，机器人应该自己提供这些信息，否则Ga’ze’bo不会知道机器人关节之间的坐标系关系。这就需要以下两个条件：Plugins和Transmission.

 roslaunch urdf_sim_tutorial gazebo.launch 

2. Plugin
   Plugin主要功能是将Gazebo与ROS相连接，使用方法是在urdf文件中添加说明，如下所示：

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
    </plugin>
  </gazebo>

可以在以下的代码中查看结果，虽然没什么变化：

roslaunch urdf_sim_tutorial gazebo.launch model:=urdf/09-publishjoints.urdf.xacro

虽然在URDF文件中建立了两者的关系，但是我们仍然需要代码来说明我们想要将机器人模型在Gazebo中运行，也就是要建立Controller.控制器最初加载到ROS参数空间中。 我们有一个yaml文件joints.yaml，它指定了我们的第一个控制器。该控制器在joint_state_controller package中，可直接从Gazebo将机器人关节的状态发布到ROS中。在09-joints.launch中，可以看到我们应该如何将这个yaml文件加载到r2d2_joint_state_controller命名空间中。 然后我们使用该命名空间调用controller_manager / spawner`脚本，并将其加载到Gazebo中。通过运行以下代码会运行控制器，并将消息发布在/joint_states 话题上。

roslaunch urdf_sim_tutorial 09-joints.launch 

3.Transmissions
对于每个非固定的关节，我们需要指定Transmission来告诉Gazebo这个关节可以做什么，因此在urdf文件中我们需要添加如下的代码，需要注意的是，joint name要和urdf文件中的joint名称相同。

  <transmission name="head_swivel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="$head_swivel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_swivel">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

我们可以通过以下代码试运行：

roslaunch urdf_sim_tutorial 09-joints.launch model:=urdf/10-firsttransmission.urdf.xacro 

5. Joint Control
   我们想要具体的来控制Transmission，因此我们在上述的controller中添加如下代码，注意type要和hardwareInterface相匹配：

type: "position_controllers/JointPositionController"
joint: head_swivel

之后可以使用如下代码来试运行：

roslaunch urdf_sim_tutorial 10-head.launch 

之后Gazebo会订阅新的话题，我们可以通过在ROS中发布新的数值来任意改变joint的位置，例如：

rostopic pub /r2d2_head_controller/command std_msgs/Float64 "data: -0.707"

6. 实现Joint Controller的协同配合
   为了避免对单个joint进行操作而达到组群协同的目的，我们只需要在ROS parameters 中指定一个新的controller即可，该控制器如下：

type: "position_controllers/JointGroupPositionController"
joints:
  - gripper_extension
  - left_gripper_joint
  - right_gripper_joint

我们可以通过以下代码来试运行：

roslaunch urdf_sim_tutorial 12-gripper.launch 

之后我们可以通过发布数组，来指定每个joint的位置：

rostopic pub  /r2d2_gripper_controller/command std_msgs/Float64MultiArray "layout:
  dim:
  - label: ''
    size: 3
    stride: 1
  data_offset: 0
data: [0, 0.5, 0.5]"

7.实现轮子类型的joint的连续转动
为了实现这一目标，首先我们需要一个控制轮子速度的controller,其次我们需要一个控制joint转动位置的controller。
7.1速度controller

    <transmission name="${prefix}_${suffix}_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${prefix}_${suffix}_wheel_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${prefix}_${suffix}_wheel_joint">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>

7.2转动位置controller
为了避免单独给每个轮子指定controller,我们可以将所有轮子一起控制，采用DiffDriverController，代码如下：

type: "diff_drive_controller/DiffDriveController"
  publish_rate: 50

  left_wheel: ['left_front_wheel_joint', 'left_back_wheel_joint']
  right_wheel: ['right_front_wheel_joint', 'right_back_wheel_joint']

  wheel_separation: 0.44

  # Odometry covariances for the encoder output of the robot. These values should
  # be tuned to your robot's sample odometry data, but these values are a good place
  # to start
  pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

  # Top level frame (link) of the robot description
  base_frame_id: base_link

  # Velocity and acceleration limits for the robot
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 0.2   # m/s
      has_acceleration_limits: true
      max_acceleration       : 0.6   # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 2.0   # rad/s
      has_acceleration_limits: true
      max_acceleration       : 6.0   # rad/s^2

DiffDriverController会订阅Twist cmd_vel类型的消息，然后移动机器人，如下面的代码所示：

 roslaunch urdf_sim_tutorial 13-diffdrive.launch

8.创建自己的urdf文件

8.1我们想要建立一个自己的机器人，在文件my_robot.urdf粘贴如下代码：

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
  </joint>
</robot>

之后运行以下命令来检查urdf文件语法是否正确来通过编译，首先安装独立于ROS的检查报urdfdom

  $ sudo apt-get install liburdfdom-tools

之后开始检查：

$ rosmake urdfdom_model              # only needed if installed from source
$ check_urdf my_robot.urdf                    # hydro and later

8.2 用标签来指定link坐标系之间的偏移量，如下面代码所示：

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />


  <joint name="joint1" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 3 0" rpy="0 0 0" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="-2 5 0" rpy="0 0 1.57" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 -1.57" />
  </joint>
</robot>

之后运行命令检查有无错误：

  $ check_urdf my_robot.urdf

8.3 使用标签为Joint添加旋转轴

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 3 0" rpy="0 0 0" />
    <axis xyz="-0.9 0.15 0" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="-2 5 0" rpy="0 0 1.57" />
    <axis xyz="-0.707 0.707 0" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 -1.57" />
    <axis xyz="0.707 -0.707 0" />
  </joint>
</robot>

之后运行检查，并通过在rviz和pdf文件中查看坐标系和Joint的状态

  $ check_urdf my_robot.urdf
  $ urdf_to_graphiz my_robot.urdf
  $ evince test_robot.pdf

9. 编译urdf文件
   首先创立工作空间

  $ cd ~/catkin_ws/src
  $ catkin_create_pkg testbot_description urdf
  $ cd testbot_description

之后建立所需要的urdf文件夹以及src文件夹，如果有需要，还应该建立mesh,materials,cad文件夹

mkdir urdf
mkdir src

之后将8中创建的.urdf文件拷贝到urdf文件夹下面，并将如下代码粘贴到新建的src/parser.cpp文件中：

#include <urdf/model.h>
#include "ros/ros.h"

int main(int argc, char** argv){
  ros::init(argc, argv, "my_parser");
  if (argc != 2){
    ROS_ERROR("Need a urdf file as argument");
    return -1;
  }
  std::string urdf_file = argv[1];

  urdf::Model model;
  if (!model.initFile(urdf_file)){
    ROS_ERROR("Failed to parse urdf file");
    return -1;
  }
  ROS_INFO("Successfully parsed urdf file");
  return 0;
}

之后再CMakeLists.txt后面粘贴如下代码：

 add_executable(parser src/parser.cpp)
 target_link_libraries(parser ${catkin_LIBRARIES})

将package进行编译

$ cd ~/catkin_ws   
$ catkin_make
$ .<path>/parser <path>my_robot.urdf
# Example: ./devel/lib/testbot_description/parser ./src/testbot_description/urdf/my_robot.urdf

10. 从urdf文件创建KDL Tree
    首先安装KDL parser

 $ rosdep install kdl_parser

之后build这个package:

 $ rosmake kdl_parser

之后将KDL设置为package.xml的依赖项：

  <package>
    ...
    <build_depend package="kdl_parser" />
    ...
    <run_depend package="kdl_parser" />
    ...
  </package>

在文件中添加include从而能够调用KDL：

  #include <kdl_parser/kdl_parser.hpp>

10.1 从file中创建KDL

   KDL::Tree my_tree;
   if (!kdl_parser::treeFromFile("filename", my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

之后完成pr2.urdf文件的创立：

$ rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > pr2.urdf

10.2 从parameter server创建

   KDL::Tree my_tree;
   ros::NodeHandle node;
   std::string robot_desc_string;
   node.param("robot_description", robot_desc_string, std::string());
   if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

10.3从.xml文件中创立

   KDL::Tree my_tree;
   TiXmlDocument xml_doc;
   xml_doc.Parse(xml_string.c_str());
   xml_root = xml_doc.FirstChildElement("robot");
   if (!xml_root){
      ROS_ERROR("Failed to get robot from xml document");
      return false;
   }
   if (!kdl_parser::treeFromXml(xml_root, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

10.4 从urdf model创立

  KDL::Tree my_tree;
   urdf::Model my_model;
   if (!my_model.initXml(....)){
      ROS_ERROR("Failed to parse urdf robot model");
      return false;
   }
   if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

11.使用robot_state_publisher来向/tf广播所使用的urdf模型的状态
首先有一个机器人模型model.xml
然后创建如下的安装包，并把代码拷贝到src/state_publisher.cpp文件中：

cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    const double degree = M_PI/180;

    // robot state
    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "axis";

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(3);
        joint_state.position.resize(3);
        joint_state.name[0] ="swivel";
        joint_state.position[0] = swivel;
        joint_state.name[1] ="tilt";
        joint_state.position[1] = tilt;
        joint_state.name[2] ="periscope";
        joint_state.position[2] = height;


        // update transform
        // (moving in a circle with radius=2)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;
        odom_trans.transform.translation.y = sin(angle)*2;
        odom_trans.transform.translation.z = .7;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);

        // Create new robot state
        tilt += tinc;
        if (tilt<-.5 || tilt>0) tinc *= -1;
        height += hinc;
        if (height>.2 || height<0) hinc *= -1;
        swivel += degree;
        angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }


    return 0;
}

之后建立display.launch文件准备广播

<launch>
        <param name="robot_description" command="cat $(find r2d2)/model.xml" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
        <node name="state_publisher" pkg="r2d2" type="state_publisher" />
</launch>

之后再CMakeLists.txt文件中粘贴以下代码：

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})

$ catkin_make
$ roslaunch r2d2 display.launch
$ rosrun rviz rviz

Choose odom as your fixed frame (under Global Options). Then choose “Add Display” and add a Robot Model Display and a TF Display