【Touch主手在Ubuntu20.04 Noetic下的主从控制实现】

提示：下载内容免费

---

前言

本文描述的是通过六自由度的3D System Touch控制虚拟手术器械上下移动，即仅一自由度。

---

一、实验基础

1. Ubuntu20.04
2. ros版本：Noetic
3. 主手设备：3D System Touch

二、话题通信准备工作

1.查看Touch发布内容

1. 电脑连上Touch后（前端放入孔洞内）打开TouchDriver文件，打开bin文件夹，在bin目录下打开终端，运行Touch_Diagnostic：

./Touch_Diagnostic

2. 打开文件touch_ws，进入终端，运行rviz：

source devel/setup.bash
roslaunch omni_common omni.launch

3. 另起终端中运行：

rostopic list

找到要订阅的Touch话题**/phantom/pose**，运行：

rostopic echo /phantom/pose

/*
header: 
  seq: 131793
  stamp: 
    secs: 1702559617
    nsecs: 736536918
  frame_id: "/map"
pose: 
  position: 
    x: 0.0
    y: 0.08811419677734375
    z: -0.06551071166992188
  orientation: 
    x: -0.10388978672494428
    y: 0.18293045140756542
    z: 0.8478933427010706
    w: 0.4866418000596281
*/

因为只控制执行器上下运动，所以只需要订阅**.pose.position.z**的信息

找到要发布信息的话题**/move_group/fake_controller_joint_states**，运行：

//输入：
rostopic pub /phantom/pose
//Tab补齐：
rostopic pub /phantom/pose geometry_msgs/PoseStamped
//geometry_msgs/PoseStamped是消息类型。代码实现时需要使用geometry_msgs/PoseStamped消息类型来订阅。

2.查看手术执行器订阅内容

注：把surgical_new中的devel和build文件夹删掉并执行catkin_make后再进行如下操作：

1. 在vscode中打开surgical_new文件夹，执行：

source devel/setup.bash
roslaunch vision_config demo.launch

可以看到手术执行器有两个关节trans1和joint2，把消息发布到哪个关节才能让其实现上下运动呢？运行：

rostopic list

找到要发布信息的话题**/move_group/fake_controller_joint_states**，运行：

//输入：
rostopic pub /move_group/fake_controller_joint_states
//Tab补齐：
rostopic pub /move_group/fake_controller_joint_states sensor_msgs/JointState 
//sensor_msgs/JointState是消息类型。代码实现时需要使用sensor_msgs/JointState消息类型来发布。
//Tab再补齐：
rostopic pub /move_group/fake_controller_joint_states sensor_msgs/JointState "header: 
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
name: ['']
position: [0]
velocity: [0]
effort: [0]"

在name中输入上边的两个关节名并设置position参数其他不动，观察rviz中执行器的运动，发现trans1为上下移动关节。

订阅方有name[2]、position[2]

三、实验环境配置

1. 在surgical_new文件夹下（手术执行器）创建工作空间这里命名为touch_sur，添加依赖：

geometry_msgs message_generation roscpp rospy sensor_msgs std_msgs

2. 在src中创建cpp文件，命名为touch_sur_sub，创建msg文件夹，将Touch的.msg文件放入其中。下载链接
3. 配置package.xml文件

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

4. 配置CMakeLists

add_message_files(
   FILES
   OmniButtonEvent.msg
   OmniFeedback.msg
   OmniState.msg
 )
generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs)
catkin_package(
  CATKIN_DEPENDS geometry_msgs message_generation roscpp rospy sensor_msgs std_msgs
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
add_executable(touch_sur_sub src/touch_sur_sub.cpp)
target_link_libraries(touch_sur_sub
  ${catkin_LIBRARIES}
)

5. 话题重复问题
   Touch omni.launch中修改：

</node> -->
	<node name="robot_state_publisher_omni" pkg="robot_state_publisher" type="robot_state_publisher">
	<remap from="/joint_states" to="/joint_states_omni" />
	</node>

执行器 demo.launch中修改：

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="$(arg use_gui)"/>
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
    <remap from="joint_states" to="joint_states_dis"/>  
  </node>

四、发布订阅方实现

代码实现：

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/Float64.h"
#include <Eigen/Core> 

class touch_sur_sub
{
public:
    Eigen::Matrix<double,1,2> pose_follow;//用来接收Touch消息    
    void doPose(const geometry_msgs::PoseStamped::ConstPtr &msg);
};

//回调函数
void touch_sur_sub::doPose(const geometry_msgs::PoseStamped::ConstPtr &msg){
    pose_follow[0] = 0.1*(msg->pose.position.z);//接收要发布到trans1的Touch.pose.position.z信息，double类型 
    //设置了一个系数，保证执行器运动幅度稳定  
    pose_follow[1] = 0.0;//旋转关节设置为0发布到joint1
}

int main(int argc, char *argv[])
{
    touch_sur_sub processor;//实例化一个对象

    ros::init(argc,argv,"touch_updown");
    ros::NodeHandle node; 
    
    ros::Subscriber sub = node.subscribe<geometry_msgs::PoseStamped>("/phantom/pose",1,&touch_sur_sub::doPose,&processor);//订阅来自Touch的信息
   
    ros::Publisher pub = node.advertise<sensor_msgs::JointState>("/joint_states", 100);//将订阅到的Touch消息发布到执行器
    
    sensor_msgs::JointState joint_states;//消息声明
    joint_states.position.resize(2);
    joint_states.name.resize(2); 
    joint_states.name[0] = "trans1";
    joint_states.name[1] = "joint2";

    ros::Rate loop_rate(10);
    while(ros::ok())
    {
        //发布消息类型必须是sensor_msgs::JointState，但接收的消息为double类型，处理：
        joint_states.position[0] = processor.pose_follow[0];
        joint_states.position[1] = processor.pose_follow[1];
        
        pub.publish(joint_states);

        ROS_INFO("%f",joint_states.position[0]);

        ros::spinOnce();
        loop_rate.sleep();

    }
    return 0;
}

五、实验验证

1. 运行Touch

roslaunch omni_common omni.launch

2. 运行cpp文件

source devel/setup.bash
rosrun touch_sur touch_sur_sub

3. 运行执行器

source devel/setup.bash
roslaunch vision_config demo.launch

六、可能出现的错误

fatal error: ros/ros.h: 没有那个文件或目录

在package.xml文件中加入：

  <build_export_depend>message_generation</build_export_depend>

---