ROS-真机向虚拟机器人映射

问题描述

ROS里的虚拟机械臂可以实现和真实机械臂的位置同步，真实机械臂如何动，ROS里的虚拟机械臂就如何动

效果

步骤

确保库卡机械臂端安装有EthernetKRL辅助软件和KUKAVARPROXY 6.1.101（它是一个 TCP/IP 服务器 ，可通过网络实现对库卡机器人变量的读取和写入，负责 KUKA 控制器与远程 PC 之间全局变量的交换。）

1.利用网线连接库卡机械臂与上位机

机械臂IP在主目录-投入运行-网络配置里（IP：192.168.3.10  端口：7000）将上位机有线连接的IP设置为与机械臂在同一网段下的IP

2. 打开上位机，终端输入验证是否可以实现IP端口连接

telnet 192.168.3.10 7000

3. 编写节点

创建功能包

cd ~/ws_moveit/src
catkin_create_pkg kuka_eki_interface roscpp moveit_ros_planning_interface sensor_msgs geometry_msgs boost_system

在src目录下创建kuka_eki_interface.cpp

touch kuka_eki_interface.cpp

kuka_eki_interface.cpp中写入程序（需考虑真实机械臂和虚拟机械臂的坐标配准问题，我的虚拟机械臂的关节2和关节3与真实机械臂相差了+90度和-90度，需要手动在程序里进行配准）

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_state/robot_state.h>
#include <sensor_msgs/JointState.h>
#include <boost/asio.hpp>
#include <vector>
#include <string>
#include <thread>
#include <mutex>
#include <cmath>

using namespace boost::asio;
using ip::tcp;

std::vector<double> current_joint_positions(6, 0.0);
std::mutex data_mutex;
bool running = true;
bool has_valid_data = false;

class KukaEKIInterface {
private:
    ros::NodeHandle nh_;
    std::string robot_ip_;
    int robot_port_;
    ros::Publisher joint_state_pub_;
    moveit::planning_interface::MoveGroupInterface move_group_;
    ros::Timer timer_;
    moveit::core::RobotStatePtr robot_state_;

    io_service ios;
    tcp::socket socket;
    tcp::endpoint endpoint;

    // 发送请求并接收响应
    bool sendRequestAndReceiveResponse(const std::vector<unsigned char>& request, std::vector<char>& response) {
        try {
            boost::system::error_code ec;
            socket.write_some(buffer(request), ec);
            if (ec) {
                ROS_ERROR("Failed to send request: %s", ec.message().c_str());
                return false;
            }
            size_t len = socket.read_some(buffer(response), ec);
            if (ec) {
                if (ec == boost::asio::error::eof) return false;
                ROS_ERROR("Failed to receive response: %s", ec.message().c_str());
                return false;
            }
            return true;
        } catch (std::exception& e) {
            ROS_ERROR("Exception in sending request and receiving response: %s", e.what());
            return false;
        }
    }

    // 获取当前关节位置
    bool getCurrentJointPositions() {
        std::vector<unsigned char> axis_request = {
            0x00, 0x01, 0x00, 0x0c, 0x00, 0x00, 0x09, 
            0x24, 0x41, 0x58, 0x49, 0x53, 0x5f, 0x41, 0x43, 0x54
        };
        std::vector<char> axis_response(210);
        if (!sendRequestAndReceiveResponse(axis_request, axis_response)) {
            return false;
        }
        std::string axis_str(axis_response.begin() + 8, axis_response.end());
        return parseAxisData(axis_str);
    }

    // 解析轴数据
    bool parseAxisData(const std::string& data) {
        std::vector<double> new_positions(6, 0.0);
        std::vector<std::string> axis_names = {"A1", "A2", "A3", "A4", "A5", "A6"};

        for (size_t i = 0; i < axis_names.size(); ++i) {
            size_t start = data.find(axis_names[i]);
            if (start == std::string::npos) {
                ROS_ERROR("Axis %s not found", axis_names[i].c_str());
                return false;
            }
            start += axis_names[i].length();

            size_t end = data.find_first_of(",\n", start);
            if (end == std::string::npos) end = data.length();

            try {
                double deg = std::stod(data.substr(start, end - start));
                double rad = deg * M_PI / 180.0;
                
                // 调整J2方向并规范化角度，逆时针90度（pi/2弧度）
                if (i == 1) {
                    //rad *= -1;  
                    rad += M_PI / 2 ;  // 再加上逆时针90度的偏移
                }
                if (i == 2) {
                    rad += -M_PI / 2 ;  // 再加上顺时针90度的偏移
                }
                rad = fmod(rad + M_PI, 2*M_PI) - M_PI;

                new_positions[i] = rad;
            } catch (const std::exception& e) {
                ROS_ERROR("Parse error: %s", e.what());
                return false;
            }
        }

        std::lock_guard<std::mutex> lock(data_mutex);
        current_joint_positions = new_positions;
        return true;
    }

    // 更新MoveIt状态
    void updateMoveItState() {
        auto new_state = std::make_shared<moveit::core::RobotState>(move_group_.getRobotModel());
        {
            std::lock_guard<std::mutex> lock(data_mutex);
            new_state->setJointGroupPositions("kuka_arm", current_joint_positions);
        }
        robot_state_.swap(new_state);
    }

    // 发布关节状态
    void publishJointStates() {
        sensor_msgs::JointState joint_state;
        joint_state.header.stamp = ros::Time::now();
        joint_state.name = {"j1", "j2", "j3", "j4", "j5", "j6"};
        
        {
            std::lock_guard<std::mutex> lock(data_mutex);
            joint_state.position = current_joint_positions;
        }

        joint_state.velocity.assign(6, 0.0);
        joint_state.effort.assign(6, 0.0);
        joint_state_pub_.publish(joint_state);
    }

    // 定时器回调函数
    void timerCallback(const ros::TimerEvent& event) {
        if (!running || !ros::ok()) return;

        static ros::Time last_valid_time = ros::Time::now();
        bool success = getCurrentJointPositions();
        if (success) {
            // 更新MoveIt内部状态
            updateMoveItState();
            
            // 发布关节状态
            publishJointStates();

            last_valid_time = ros::Time::now();
            has_valid_data = true;
        } else {
            ROS_WARN_THROTTLE(1, "Failed to get joint positions");
            // 使用最后有效数据更新
            if ((ros::Time::now() - last_valid_time).toSec() < 1.0) {
                updateMoveItState();
                publishJointStates();
            }
        }
    }

public:
    KukaEKIInterface() : 
        nh_("~"),
        move_group_("kuka_arm"),
        socket(ios)
    {
        nh_.param<std::string>("robot_ip", robot_ip_, "192.168.3.10");
        nh_.param<int>("robot_port", robot_port_, 7000);

        // 初始化机器人状态
        robot_state_ = std::make_shared<moveit::core::RobotState>(move_group_.getRobotModel());
        
        try {
            // 连接机器人
            socket.connect(tcp::endpoint(ip::address::from_string(robot_ip_), robot_port_));
            ROS_INFO("Connected to KUKA at %s:%d", robot_ip_.c_str(), robot_port_);

            // 获取初始状态
            if (!getCurrentJointPositions()) {
                throw std::runtime_error("Initial position fetch failed");
            }
            updateMoveItState();

            // 初始化发布者
            joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
            
            // 配置MoveIt
            move_group_.setPlanningTime(0.1);
            move_group_.allowReplanning(true);
            move_group_.stop();  // 清除残留运动

            // 设置定时器（50Hz）
            timer_ = nh_.createTimer(ros::Duration(0.02), &KukaEKIInterface::timerCallback, this);
            
            ROS_INFO("Interface initialized successfully");

        } catch (std::exception& e) {
            ROS_ERROR("Initialization failed: %s", e.what());
            throw;
        }
    }

    ~KukaEKIInterface() {
        running = false;
        if (socket.is_open()) {
            socket.close();
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "kuka_eki_interface");
    ros::AsyncSpinner spinner(2);
    spinner.start();
    
    try {
        KukaEKIInterface interface;
        ros::waitForShutdown();
    } catch (const std::exception& e) {
        ROS_ERROR("Fatal error: %s", e.what());
        return 1;
    }
    return 0;
}    

修改CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(kuka_eki_interface)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  moveit_ros_planning_interface
  roscpp
  sensor_msgs
)

## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   geometry_msgs#   sensor_msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES kuka_eki_interface
#  CATKIN_DEPENDS boost_system geometry_msgs moveit_ros_planning_interface roscpp sensor_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/kuka_eki_interface.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/kuka_eki_interface_node.cpp)
add_executable(kuka_eki_interface src/kuka_eki_interface.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )
target_link_libraries(kuka_eki_interface
  ${catkin_LIBRARIES}
  ${Boost_LIBRARIES}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
install(TARGETS kuka_eki_interface
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_kuka_eki_interface.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

4.修改一下启动文件demo.launch的配置

（我的节点程序里发布了一个关节状态（fake控制器），与demo.launch里发布的虚拟关节状态发生线性冲突，需要注释掉虚拟关节的发布，否则MoveIt 会交替接收两种不同的关节状态，造成显示跳跃。）

<launch>

  <!-- specify the planning pipeline -->
  <arg name="pipeline" default="ompl" />

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find kuka4_moveit_config)/default_warehouse_mongo_db" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

  <!-- By default, we will load or override the robot_description -->
  <arg name="load_robot_description" default="true"/>

  <!-- Choose controller manager: fake, simple, or ros_control -->
  <arg name="moveit_controller_manager" default="fake" />
  <!-- Set execution mode for fake execution controllers -->
  <arg name="fake_execution_type" default="interpolate" />

  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
  <arg name="use_gui" default="false" />
  <arg name="use_rviz" default="true" />

  <!-- If needed, broadcast static tf for robot root -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_link" />

  <!-- 完全禁用 fake 控制器//确保ros机械臂不会出现向原始关节状态跳跃闪动 -->
<!-- 
<group if="$(eval arg('moveit_controller_manager') == 'fake')">
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
    </node>
    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
    </node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
</group>
-->


  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(dirname)/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="pipeline" value="$(arg pipeline)"/>
    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
  </include>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

</launch>

5.编译运行

catkin build

确保所有功能包正确被编译

启动机械臂

roslaunch kuka4_moveit_config demo.launch

运行编译的节点

rosrun kuka_eki_interface kuka_eki_interface

实现真机向虚拟机械臂映射