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

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="prefixjoint_limits_parameters_filekinematics_parameters_filephysical_parameters_filevisual_parameters_filetransmission_hw_interface:=hardware_interface/EffortJointInterfacesafety_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_robotprefix="${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_controlcontrollers 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看到其发布的话题