ur3+robotiq ft sensor+robotiq 2f 140+realsense d435i配置rviz，gazebo仿真环境

搭建环境：

ubuntu: 20.04
ros: Nonetic
sensor: robotiq_ft300
gripper: robotiq_2f_140_gripper
UR: UR3
reasense： D435i

通过下面几篇博客配置好了ur3、力传感器和robotiq夹爪的rviz和gazebo仿真环境

ubuntu20.04配置UR机械臂的仿真环境
ur3+robotiq ft sensor+robotiq 2f 140配置rviz仿真环境
ur3+robotiq ft sensor+robotiq 2f 140配置gazebo仿真环境

下面我们给仿真环境加上Realsense D435i

1. 安装realsense-ros

克隆仓库链接

因为我使用的是ros1故采用ros1-legacy分支

将仓库克隆到之前的工作路径下

cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros.git

然后按照仓库中的readme教程安装,这里我采用的是方法二，因为之前我已经安装过RealSense SDK了，之前这一篇博客有安装RealSense SDK的教程，所以我下面直接跳到step2，仓库按照上面克隆到相应路径后，执行下面命令，跟着readme做就行了

cd cd ~/catkin_ws/src/realsense-ros/
git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1`
cd ..

完了之后，需要安装一个ros工具包ddynamic_reconfigure，不然后面编译会出问题，做法是直接将仓库克隆到工作路径src文件夹下

cd ~/catkin_ws/src
git clone https://github.com/pal-robotics/ddynamic_reconfigure.git

后面直接按照readme命令编译，如果编译不过的话将工作目录下的build和devel删除重新编译试试

cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
source devel/setup.bash

至此，realsense-ros就安装好了

2. 在rviz仿真环境中加上realsense D435i

根据之前ur3+robotiq ft sensor+robotiq 2f 140配置rviz仿真环境的博客，我们复制universal_robot/ur_description/urdf文件夹下的ur3_ft_gripper.xacro文件，并命名为ur3_ft_gripper_realsense.xacro，在该文件后面加上调用realsense的代码，加入代码如下：

<!-- Add realsense d435i -->
  <xacro:arg name="use_nominal_extrinsics" default="false" />
  <xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>

  <xacro:sensor_d435i parent="base" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
    <origin xyz="0.4 0 1" rpy="0 1.57 0"/>
  </xacro:sensor_d435i>

parent指定为base是因为在机械臂描述文件(urdf)中，base_link是连接在base上的，不是之前的world，该文件完整代码如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="ur3_robot_ft_gripper">
  <xacro:include filename="$(find ur_description)/urdf/inc/ur3_macro.xacro"/>
  <xacro:ur3_robot prefix="" />
  
  <xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro"/> 
  <xacro:include filename="$(find robotiq_2f_140_gripper_visualization)/urdf/robotiq_arg2f_140.xacro" />

  <!--robotiq_ft_sensor-->
  <xacro:robotiq_ft300 prefix="" parent="tool0">
      <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_ft300>

  <!--robotiq_arg2f_140.xacro已经调用了robotiq_arg2f_140宏定义，这里只需要把gripper和sensor用joint连接起来即可-->

  <joint name="ft_gripper_joint" type="fixed">
    <parent link="robotiq_ft_frame_id"/>
    <child link="robotiq_arg2f_base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- Add realsense d435i -->
  <xacro:arg name="use_nominal_extrinsics" default="false" />
  <xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>

  <xacro:sensor_d435i parent="base" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
    <origin xyz="0.4 0 1" rpy="0 1.57 0"/>
  </xacro:sensor_d435i>
  
</robot>

复制universal_robot/ur_description/launch文件夹下中的view_ur3_ft_gripper.launch，load_ur3_ft_gripper.launch，load_ur_ft_gripper.launch文件，分别重命名为view_ur3_ft_gripper_realsense.launch，load_ur3_ft_gripper_realsense.launch，load_ur_ft_gripper_realsense.launch，分别改一下调用的文件名称

view_ur3_ft_gripper_realsense.launch修改如下图

load_ur3_ft_gripper_realsense.launch修改如下图

load_ur_ft_gripper_realsense.launch修改如下图

查看效果

运行命令

cd ~/catkin_ws
source devel/setup.bash
roslaunch ur_description view_ur3_ft_gripper_realsense.launch

可以看到realsense已经加上去了

3. 在gazebo仿真环境中加上realsense D435i

在这里需要下载一个realsense模型和插件包，仓库地址

cd ~/catkin_ws/src
git clone https://github.com/nilseuropa/realsense_ros_gazebo.git
cd ..
# 编译一下，如果编译不过的话将工作目录下的`build`和`devel`删除重新编译试试
catkin_make

然后在universal_robot/ur_gazebo/urdf目录下ur_macro.xacro的文件中加上以下调用代码

<!-- Add realsense d435 -->
<xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/>

<xacro:realsense_d435 sensor_name="d435" parent_link="base" rate="10">
<origin xyz="0.4 0 1" rpy="0 1.57 0"/>
</xacro:realsense_d435>

这里使用的是D435,而不是D435i，因为这个包里面没有D435i，先D435用着，后面参考这个博客手动添加IMU，达到D435i的效果，ur_macro.xacro文件完整代码如下：

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- Definition of the main macro -->
  <xacro:macro name="ur_robot_gazebo" params="
   prefix
   joint_limits_parameters_file
   kinematics_parameters_file
   physical_parameters_file
   visual_parameters_file
   transmission_hw_interface:=hardware_interface/EffortJointInterface
   safety_limits:=false safety_pos_margin:=0.15 safety_k_position:=20"
  >
  <xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro"/>


    <!-- Instantiate model for the REAL robot. -->
    <xacro:ur_robot
      prefix="${prefix}"
      joint_limits_parameters_file="${joint_limits_parameters_file}"
      kinematics_parameters_file="${kinematics_parameters_file}"
      physical_parameters_file="${physical_parameters_file}"
      visual_parameters_file="${visual_parameters_file}"
      transmission_hw_interface="${transmission_hw_interface}"
      safety_limits="${safety_limits}"
      safety_pos_margin="${safety_pos_margin}"
      safety_k_position="${safety_k_position}"
    />

    <!-- Configure self collision properties per link -->
    <gazebo reference="${prefix}shoulder_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}upper_arm_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}forearm_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}wrist_1_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}wrist_3_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}wrist_2_link">
      <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="${prefix}ee_link">
      <selfCollide>true</selfCollide>
    </gazebo>

    <!--
      Inject Gazebo ROS Control plugin, which allows us to use ros_control
      controllers to control the virtual robot hw.
    -->
    <gazebo>
      <plugin name="ros_control" filename="libgazebo_ros_control.so">
        <!--robotNamespace>/</robotNamespace-->
        <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
      </plugin>
    </gazebo>
  </xacro:macro>
  <xacro:include filename="$(find robotiq_ft_sensor)/urdf/robotiq_ft300.urdf.xacro"/> 
  <xacro:include filename="$(find robotiq_2f_140_gripper_gazebo)/urdf/robotiq_arg2f_140.xacro" />

  <!--robotiq_ft_sensor-->
  <xacro:robotiq_ft300 prefix="" parent="tool0">
      <origin xyz="0 0 0" rpy="0 0 0"/>
  </xacro:robotiq_ft300>

  <!--robotiq_arg2f_140.xacro已经调用了robotiq_arg2f_140宏定义，这里只需要把gripper和sensor用joint连接起来即可-->

  <joint name="ft_gripper_joint" type="fixed">
    <parent link="robotiq_ft_frame_id"/>
    <child link="robotiq_arg2f_base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

    <!-- Add realsense d435 -->
  <xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/>

  <xacro:realsense_d435 sensor_name="d435" parent_link="base" rate="10">
    <origin xyz="0.4 0 1" rpy="0 1.57 0"/>
  </xacro:realsense_d435>
</robot>

查看效果

运行命令

cd ~/catkin_ws
source devel/setup.bash
roslaunch ur_gazebo ur3_bringup.launch

可以看到realsens已经加入到gazebo中，并且通过rostopic list看到其发布的话题