Geomaic Touch手柄在 ROS Noetic环境下的基本使用

庐州学子

已于 2025-04-10 10:40:29 修改

阅读量1.1k

点赞数 32

分类专栏：科研所得文章标签： ubuntu 机器人 linux

于 2025-01-13 11:01:44 首次发布

本文链接：https://blog.youkuaiyun.com/qq_38236680/article/details/145094374

版权

科研所得专栏收录该内容

5 篇文章

订阅专栏

前言

Touch手柄在ROS环境下的配置，笔者在另一篇文章中有叙述：

Geomagic Touch 力反馈手柄在Ubuntu 20.04 ROS1 noetic 环境下的配置_touch手柄 ubuntu-优快云博客

这篇文章中已有RVIZ模型显示、同步运动的内容，可以供参考。

本文继续给出力反馈方面的基础实现教程，包括：位姿获取、按键状态获取、三轴力反馈实现、手柄位姿锁死，以飨后学。

20250410更新：

优化了功能包，解决了手柄静态下姿态不更新的问题，其余不变。

https://download.youkuaiyun.com/download/qq_38236680/90590826

功能包创建

这里依旧是依赖前述功能包，可以在这里下载：

https://download.youkuaiyun.com/download/qq_38236680/89757312?spm=1001.2014.3001.5503

在同一工作空间下，创建力控程序包，注意这里的依赖项：

catkin_create_pkg geomagic_myhaptics geomagic_control roscpp rospy std_msgs geometry_msgs

创建成功。

位姿与按键信息获取

查看话题消息类型

运行launch文件，具体步骤可见前一篇博客：

roslaunch geomagic_control geomagic.launch

这里可以将launch文件修改下，注释掉RVIZ那一行，暂且不必显示模型。

查看位姿与按键话题的信息：

位姿使用 geometry_msgs 中的类型，以一定频率不断向该 /Geomagic/pose 话题发送当前位姿信息，手柄末端扭转信息同理。

按键信息属于自定义类型，可见于：

简单的 int 类型，两个按键，一灰一白，数值0 表示未按下，1 表示按下。在按键状态改变时会向 /Geomagic/button 话题发送信息。

可以看到，在双键按下时会有先后，需要注意。

编写节点程序

创建源文件与头文件。

编写头文件：

#ifndef PUBFORCE_H
#define PUBFORCE_H

#include "ros/ros.h"
#include "geomagic_control/DeviceButtonEvent.h"
#include "geometry_msgs/PoseStamped.h"

void cb_pose(const geometry_msgs::PoseStampedConstPtr& msg);
void cb_button(const geomagic_control::DeviceButtonEvent::ConstPtr &bt);

#endif // !PUBFORCE_H

这里只是对位姿与按键的回调函数进行了定义，注意传入的变量类型。

转至源文件，先编写主函数，创建订阅者来订阅话题消息，并绑定回调函数。

#include "geomagic_myhaptics/pubforce.h"

//存储按键信息
bool flag_whitebutton=false;
bool flag_greybutton=false;
geometry_msgs::PoseStamped current_pose;

int main(int argc, char **argv)
{
    // 避免中文乱码
    setlocale(LC_ALL,"");

    // 初始化ROS节点
    ros::init(argc, argv, "pubforce");

    // 创建节点句柄
    ros::NodeHandle nh;

    ros::Subscriber sub_pose=nh.subscribe<geometry_msgs::PoseStamped>("/Geomagic/pose",3,cb_pose);
    ros::Subscriber sub_button = nh.subscribe<geomagic_control::DeviceButtonEvent>("/Geomagic/button", 3, cb_button);

    ros::Rate r(30);
    //在while循环中执行任务
    while(ros::ok())
    {
        //接收最新的状态信息
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

这里定义了两个按键标志量，以及位姿变量，后续力反馈会用到。

编写回调函数：

// 接收到订阅的消息后，会进入消息回调函数
// 注意，这里需要在前面加const，因后为const ptr
void cb_button(const geomagic_control::DeviceButtonEvent::ConstPtr &bt)
{
    flag_whitebutton=true?bt->white_button==1:false;
    flag_greybutton=true?bt->grey_button==1:false;
    ROS_INFO("按键状态改变：    灰色：%d，白色：%d",bt->grey_button,bt->white_button);
    ROS_INFO("这时的位姿：      x: %.2f, y: %.2f, z: %.2f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    ROS_INFO("这时的朝向：      w: %.2f, x: %.2f, y: %.2f, z: %.2f",current_pose.pose.orientation.w,current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z);
}


void cb_pose(const geometry_msgs::PoseStampedConstPtr& msg)
{
    //存储当前位置
    current_pose=*msg;
}

这里只是简单输出按键变化，以及按下时的手柄末端坐标，在后续的力反馈实现中会有更复杂的实现。

修改CMakeList：

cmake_minimum_required(VERSION 3.0.2)
project(geomagic_myhaptics)

find_package(catkin REQUIRED COMPONENTS
  geomagic_control
  geometry_msgs
  roscpp
  rospy
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES geomagic_myhaptics
#  CATKIN_DEPENDS geomagic_control geometry_msgs roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
  # 这里取消注释
include
  ${catkin_INCLUDE_DIRS}
)

add_executable(pubforce src/pubforce.cpp)
add_dependencies(pubforce ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(pubforce
  ${catkin_LIBRARIES}
)

编译，成功通过。

source一下，运行前述launch文件。

source ./devel/setup.bash
roslaunch geomagic_control geomagic.launch

另开终端，source一下，运行生成的文件。

source ./devel/setup.bash
rosrun geomagic_myhaptics pubforce

成功实现位姿与按键信息的获取。

力反馈实现

Touch手柄力反馈规格

查询官网，可见Touch对外输出的力最大为3.3N，也即XYZ三轴均能输出最大为3.3N的力。

Geomagic Touch触觉设备及软件工具组_上海形宙数字技术有限公司

查看话题消息类型

连接上Touch手柄，运行笔者上传的launch文件，步骤详见前述博客。

roslaunch geomagic_control geomagic.launch

另开终端，查看当前消息：

这里的，/Geomagic/force_feedback，就是我们需要用到的话题。

查看话题类型：

它使用 geomagic_control/DeviceFeedback 类型的消息，由控制节点接收，并下发给Touch设备，以实现力反馈。

消息定义文件在 geomagic_control 功能包中，如下：

可见，包含三维力向量，三维位姿向量，以及三轴锁死向量。

力反馈快速尝试

这里，可以利用终端补全，来快速尝试力反馈的功能。

先source下，否则无法获取消息文件，不能补全。

source ./devel/setup.bash

而后，输入

rostopic pub /Geomagic/force_feedback

不断使用Tab补全，可得消息模板：

rostopic pub /Geomagic/force_feedback geomagic_control/DeviceFeedback "force:
  x: 0.0
  y: 0.0
  z: 0.0
position:
  x: 0.0
  y: 0.0
  z: 0.0
lock:
- false"

这里大家可以自由编辑force下的各轴力大小以测试，position和lock部分暂且不要改动。

如：

rostopic pub /Geomagic/force_feedback geomagic_control/DeviceFeedback "force:
  x: 1.0
  y: 0.0
  z: 0.0
position:
  x: 0.0
  y: 0.0
  z: 0.0
lock:
- false"

即，对x轴的正方向施加1N的力。回车，发布后，可以感觉到手柄有向右的力反馈。

体验完后记得发布归零消息，释放手柄。

编写节点程序

创建头文件与源文件。

编写头文件，在前述位姿与按键信息获取的程序基础上修改即可。

#ifndef PUBFORCE_H
#define PUBFORCE_H

#include "ros/ros.h"
#include "geomagic_control/DeviceButtonEvent.h"
#include "geometry_msgs/PoseStamped.h"
#include "geomagic_control/DeviceFeedback.h"
#include  <vector>

// 单轴输出的最大力 官网显示最大3.3N 测试可用1N
#define MAX_FORCE   1
// 表征Touch手柄所处的各种状态
#define RELEASE 0
#define FORCE_FEEDBACK  1
#define LOCK    2

//位姿与按键订阅者的回调函数
void cb_pose(const geometry_msgs::PoseStampedConstPtr& msg);
void cb_button(const geomagic_control::DeviceButtonEvent::ConstPtr &bt);
//释放手柄
void touch_release(void);
//在指定位置锁住touch手柄
void touch_lock(geometry_msgs::PoseStamped* pose);

#endif // !PUBFORCE_H

这里，定义了单轴输出的最大力，最大为3.3N。测试建议使用1N，避免力量过大，损坏设备。后续为回调函数，以及释放与锁住手柄的函数，注意传入的参数类型。

编写源文件，首先定义一些全局变量。

#include "geomagic_myhaptics/pubforce.h"

using namespace std;

//存储按键信息
bool flag_whitebutton=false;
bool flag_greybutton=false;
//Touch手柄状态：0  释放（自由状态） 1  力反馈状态  2   锁死状态
int touch_state=0;
ros::Publisher pub_force;
geometry_msgs::PoseStamped current_pose;

底部两行分别定义力反馈信息的发布者，以及存储当前位姿的变量。

而后，初步编写主函数，编写两回调函数：

int main(int argc, char **argv)
{
    // 避免中文乱码
    setlocale(LC_ALL,"");

    // 初始化ROS节点
    ros::init(argc, argv, "pubforce");

    // 创建节点句柄
    ros::NodeHandle nh;

    ros::Subscriber sub_pose=nh.subscribe<geometry_msgs::PoseStamped>("/Geomagic/pose",3,cb_pose);
    ros::Subscriber sub_button = nh.subscribe<geomagic_control::DeviceButtonEvent>("/Geomagic/button", 3, cb_button);
    pub_force=nh.advertise<geomagic_control::DeviceFeedback>("/Geomagic/force_feedback",10);
    touch_state=RELEASE;

    ros::Rate r(30);

    //在while循环中执行任务
    while(ros::ok())
    {
        vector<uint8_t> lockv;
        geomagic_control::DeviceFeedback fb;
        //接收最新的状态信息
        ros::spinOnce();

        r.sleep();
    }

    return 0;
}


// 接收到订阅的消息后，会进入消息回调函数
// 注意，这里需要在前面加const，因后为const ptr
void cb_button(const geomagic_control::DeviceButtonEvent::ConstPtr &bt)
{
    flag_whitebutton=true?bt->white_button==1:false;
    flag_greybutton=true?bt->grey_button==1:false;
    // ROS_INFO("按键状态改变：    灰色：%d，白色：%d",bt->grey_button,bt->white_button);
    // ROS_INFO("这时的位姿：      x: %.2f, y: %.2f, z: %.2f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    // ROS_INFO("这时的朝向：      w: %.2f, x: %.2f, y: %.2f, z: %.2f",current_pose.pose.orientation.w,current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z);
}

void cb_pose(const geometry_msgs::PoseStampedConstPtr& msg)
{
    //存储当前位置
    current_pose=*msg;
}

主函数中，初始化了位姿与按键信息的订阅者，以及力反馈信息的发布者。并在主循环中定义了用于发布力反馈信息的向量与消息。回调函数仅进行简单的信息记录，更新当前最新的位姿与按键信息。

编写释放Touch手柄与锁住手柄的函数：

//在指定位置锁住touch手柄
void touch_lock(geometry_msgs::PoseStamped* pose)
{
    vector<uint8_t> lockv;
    geomagic_control::DeviceFeedback fb;

    //三方向均锁死
    lockv.push_back(1);
    lockv.push_back(1);
    lockv.push_back(1);
    fb.position.x=pose->pose.position.x;
    fb.position.y=pose->pose.position.y;
    fb.position.z=pose->pose.position.z;
    fb.lock=lockv;

    pub_force.publish(fb);

    touch_state=LOCK;

    ROS_INFO("手柄已锁住");
}


//释放Touch手柄 解锁、不输出力
void touch_release(void)
{
    vector<uint8_t> lockv;
    geomagic_control::DeviceFeedback fb;
    lockv.push_back(0);
    lockv.push_back(0);
    lockv.push_back(0);
    fb.lock=lockv;
    pub_force.publish(fb);

    touch_state=RELEASE;

    ROS_INFO("手柄已释放");
}

这里也仅仅是对力反馈消息的填充，发布。而后，更新Touch手柄当前的状态，并在终端进行提示。

继续编写主循环函数，实现稍复杂的力反馈功能。

//在while循环中执行任务
    while(ros::ok())
    {
        vector<uint8_t> lockv;
        geomagic_control::DeviceFeedback fb;
        //接收最新的状态信息
        ros::spinOnce();

        // 双键按下 双按则锁死当前位置 再次双按则解锁
        if((flag_greybutton==true)&&(flag_whitebutton==true))
        {
            if(touch_state!=LOCK)
            {
                touch_lock(&current_pose);
                //双键按下后，全部置否，等待下一次按下,避免因长时间按住，重复操作
                flag_greybutton=false;
                flag_whitebutton=false;
            }
            else
            {
                touch_release();
                //双键按下后，全部置否，等待下一次按下,避免因长时间按住，重复操作
                flag_greybutton=false;
                flag_whitebutton=false;
            }
            continue;
        }
        //锁死状态下不进行力反馈操作
        if(touch_state==LOCK)
        {
            continue;
        }
        //按下灰色按键，启闭力反馈功能
        if(flag_greybutton==true)
        {
            if(touch_state!=FORCE_FEEDBACK)
            {
                touch_state=FORCE_FEEDBACK;
                ROS_INFO("手柄力反馈开启");
            }
            else
            {
                ROS_INFO("手柄力反馈关闭");
                touch_release();
            }
            //按下后，标志置否，等待下一次按下,避免因长时间按住，重复操作
            flag_greybutton=false;
        }
        //根据手柄位置施加相应力，使其归于三轴0点
        if(touch_state==FORCE_FEEDBACK)
        {
            // 超出运动范围则退出力反馈
            if(abs(current_pose.pose.position.x)>100||abs(current_pose.pose.position.y)>100||abs(current_pose.pose.position.z)>50)
            {
                ROS_INFO("超出运动范围，手柄力反馈关闭");
                touch_release();
                continue;
            }
            // 运动范围：x、y大概是正负两百，z正负50，具体可见echo /Geomagic/pose的信息
            // 这里取xy正负一百，z正负50
            // 根据位置 施加反方向的力即可
            fb.force.x=-((int)current_pose.pose.position.x/100.0)*MAX_FORCE;
            fb.force.y=-((int)current_pose.pose.position.y/100.0)*MAX_FORCE;
            fb.force.z=-((int)current_pose.pose.position.z/50.0)*MAX_FORCE;
            //三方向均解锁  需要解锁，否则不可实现力反馈
            lockv.push_back(0);
            lockv.push_back(0);
            lockv.push_back(0);
            fb.lock=lockv;
            pub_force.publish(fb);
        }
        r.sleep();
    }

这里的控制逻辑为：

Touch手柄初始为自由状态（释放状态）。

按下灰色按键进入力反馈状态（需在三轴0点附近按下，勿太靠近底座，避免刚进入即退出），会根据手柄位置施加相应的力，使其保持在0点附近，超出一定空间范围自动退出力反馈。再次按下灰色按键退出力反馈状态，返回自由状态。

两个按键同时按下则进入位姿锁死状态，手柄保持在当前位置，不可进行其它操作。再次双按则退出锁死状态，返回自由状态。

整体的源文件为：

/*
发送力控指令给Touch手柄

根据手柄按键状态以及位姿进行力反馈操作

初始为自由状态（释放状态）
按下灰色按键进入力反馈状态（需在三轴0点附近按下，勿太靠近底座，避免刚进入即退出）
会根据手柄位置施加相应的力 使其保持在0点附近 超出一定空间范围自动退出力反馈
再次按下灰色按键退出力反馈状态
两个按键同时按下则进入锁死状态  手柄保持在当前位置  不可进行其它操作
再次双按则退出锁死状态  返回自由状态

庐州学子
2025.1.12
*/

#include "geomagic_myhaptics/pubforce.h"

using namespace std;

//存储按键信息
bool flag_whitebutton=false;
bool flag_greybutton=false;
//Touch手柄状态：0  释放（自由状态） 1  力反馈状态  2   锁死状态
int touch_state=0;
ros::Publisher pub_force;
geometry_msgs::PoseStamped current_pose;

int main(int argc, char **argv)
{
    // 避免中文乱码
    setlocale(LC_ALL,"");

    // 初始化ROS节点
    ros::init(argc, argv, "pubforce");

    // 创建节点句柄
    ros::NodeHandle nh;

    ros::Subscriber sub_pose=nh.subscribe<geometry_msgs::PoseStamped>("/Geomagic/pose",3,cb_pose);
    ros::Subscriber sub_button = nh.subscribe<geomagic_control::DeviceButtonEvent>("/Geomagic/button", 3, cb_button);
    pub_force=nh.advertise<geomagic_control::DeviceFeedback>("/Geomagic/force_feedback",10);
    touch_state=RELEASE;

    ros::Rate r(30);

    //在while循环中执行任务
    while(ros::ok())
    {
        vector<uint8_t> lockv;
        geomagic_control::DeviceFeedback fb;
        //接收最新的状态信息
        ros::spinOnce();

        // 双键按下 双按则锁死当前位置 再次双按则解锁
        if((flag_greybutton==true)&&(flag_whitebutton==true))
        {
            if(touch_state!=LOCK)
            {
                touch_lock(&current_pose);
                //双键按下后，全部置否，等待下一次按下,避免因长时间按住，重复操作
                flag_greybutton=false;
                flag_whitebutton=false;
            }
            else
            {
                touch_release();
                //双键按下后，全部置否，等待下一次按下,避免因长时间按住，重复操作
                flag_greybutton=false;
                flag_whitebutton=false;
            }
            continue;
        }
        //锁死状态下不进行力反馈操作
        if(touch_state==LOCK)
        {
            continue;
        }
        //按下灰色按键，启闭力反馈功能
        if(flag_greybutton==true)
        {
            if(touch_state!=FORCE_FEEDBACK)
            {
                touch_state=FORCE_FEEDBACK;
                ROS_INFO("手柄力反馈开启");
            }
            else
            {
                ROS_INFO("手柄力反馈关闭");
                touch_release();
            }
            //按下后，标志置否，等待下一次按下,避免因长时间按住，重复操作
            flag_greybutton=false;
        }
        //根据手柄位置施加相应力，使其归于三轴0点
        if(touch_state==FORCE_FEEDBACK)
        {
            // 超出运动范围则退出力反馈
            if(abs(current_pose.pose.position.x)>100||abs(current_pose.pose.position.y)>100||abs(current_pose.pose.position.z)>50)
            {
                ROS_INFO("超出运动范围，手柄力反馈关闭");
                touch_release();
                continue;
            }
            // 运动范围：x、y大概是正负两百，z正负50，具体可见echo /Geomagic/pose的信息
            // 这里取xy正负一百，z正负50
            // 根据位置 施加反方向的力即可
            fb.force.x=-((int)current_pose.pose.position.x/100.0)*MAX_FORCE;
            fb.force.y=-((int)current_pose.pose.position.y/100.0)*MAX_FORCE;
            fb.force.z=-((int)current_pose.pose.position.z/50.0)*MAX_FORCE;
            //三方向均解锁  需要解锁，否则不可实现力反馈
            lockv.push_back(0);
            lockv.push_back(0);
            lockv.push_back(0);
            fb.lock=lockv;
            pub_force.publish(fb);
        }
        r.sleep();
    }

    return 0;
}


// 接收到订阅的消息后，会进入消息回调函数
// 注意，这里需要在前面加const，因后为const ptr
void cb_button(const geomagic_control::DeviceButtonEvent::ConstPtr &bt)
{
    flag_whitebutton=true?bt->white_button==1:false;
    flag_greybutton=true?bt->grey_button==1:false;
    // ROS_INFO("按键状态改变：    灰色：%d，白色：%d",bt->grey_button,bt->white_button);
    // ROS_INFO("这时的位姿：      x: %.2f, y: %.2f, z: %.2f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    // ROS_INFO("这时的朝向：      w: %.2f, x: %.2f, y: %.2f, z: %.2f",current_pose.pose.orientation.w,current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z);
}

void cb_pose(const geometry_msgs::PoseStampedConstPtr& msg)
{
    //存储当前位置
    current_pose=*msg;
}

//在指定位置锁住touch手柄
void touch_lock(geometry_msgs::PoseStamped* pose)
{
    vector<uint8_t> lockv;
    geomagic_control::DeviceFeedback fb;

    //三方向均锁死
    lockv.push_back(1);
    lockv.push_back(1);
    lockv.push_back(1);
    fb.position.x=pose->pose.position.x;
    fb.position.y=pose->pose.position.y;
    fb.position.z=pose->pose.position.z;
    fb.lock=lockv;

    pub_force.publish(fb);

    touch_state=LOCK;

    ROS_INFO("手柄已锁住");
}


//释放Touch手柄 解锁、不输出力
void touch_release(void)
{
    vector<uint8_t> lockv;
    geomagic_control::DeviceFeedback fb;
    lockv.push_back(0);
    lockv.push_back(0);
    lockv.push_back(0);
    fb.lock=lockv;
    pub_force.publish(fb);

    touch_state=RELEASE;

    ROS_INFO("手柄已释放");
}

至此，头文件与源文件编写完成。

编写CMakeList文件，与前述位姿与按键信息获取时的相同，已修改的直接编译即可。

cmake_minimum_required(VERSION 3.0.2)
project(geomagic_myhaptics)

find_package(catkin REQUIRED COMPONENTS
  geomagic_control
  geometry_msgs
  roscpp
  rospy
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES geomagic_myhaptics
#  CATKIN_DEPENDS geomagic_control geometry_msgs roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
  # 这里取消注释
include
  ${catkin_INCLUDE_DIRS}
)

add_executable(pubforce src/pubforce.cpp)
add_dependencies(pubforce ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(pubforce
  ${catkin_LIBRARIES}
)

编译，成功通过。

编写launch文件

在功能包下新建launch文件夹，以及launch文件。

填入以下内容：

<launch>

    <include file="$(find geomagic_control)/launch/geomagic_headless.launch" />
    <node pkg="geomagic_myhaptics" type="pubforce" name="pubforce" output="screen" />

</launch>

再次编译。

连接上Touch手柄，运行launch文件。

 roslaunch geomagic_myhaptics pubforce.launch

至此，成功实现基础的力反馈功能。

Touch手柄力反馈

本文用到的三个功能包已上传，可自行下载：

https://download.youkuaiyun.com/download/qq_38236680/90261813

祝科研有进，前程似锦。