write a C++ example shows how to do force or torque control in offboard mode from a ROS 2 node with XRCE-DDS

Here's an example of how to do force control in offboard mode using C++ and XRCE-DDS:

#include <iostream>
#include <chrono>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "rosidl_runtime_cpp/message_type_support_decl.hpp"
#include "rosidl_typesupport_cpp/message_type_support.hpp"
#include "rmw_fastrtps_cpp/get_participant.hpp"
#include "rmw_fastrtps_cpp/get_publisher.hpp"
#include "rmw_fastrtps_cpp/get_subscriber.hpp"
#include "rmw_uros/options.h"
#include "uxr/client/client.h"
#include "sensor_msgs/msg/joint_state.hpp"

using namespace std::chrono_literals;

#define DEFAULT_TIMEOUT 1000

void set_wrench(float fx, float fy, float fz, float tx, float ty, float tz, geometry_msgs::msg::WrenchStamped& wrench)
{
    wrench.wrench.force.x = fx;
    wrench.wrench.force.y = fy;
    wrench.wrench.force.z = fz;
    wrench.wrench.torque.x = tx;
    wrench.wrench.torque.y = ty;
    wrench.wrench.torque.z = tz;
}

class ForceControlNode : public rclcpp::Node
{
public:
    ForceControlNode() : Node("force_control_node")
    {
        joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>("joint_states", 10,
        std::bind(&ForceControlNode::joint_state_callback, this, std::placeholders::_1));

        wrench_pub_ = this->create_publisher<geometry_msgs::msg::WrenchStamped>("wrench", 10);

        setup_dds();
    }

private:
    void setup_dds()
    {
        auto domain_id = 0;
        uxr_set_custom_transport(uxr::Transport::CUSTOM);
        rmw_uros_options_t custom_transport_options = rmw_get_zero_initialized_uros_options();
        uxr_init_custom_transport(&custom_transport_options, uxr_common_tcp_platform);

        const char* participant_name = "force_control_participant";
        const char* topic_name = "force_control_topic";

        // Create participant
        uxr_init_options_t init_options = uxr_init_options_create();
        uxr_set_custom_transport_callbacks(
            &init_options,
            uxr_common_tcp_platform,
            custom_transport_open_cb,
            custom_transport_close_cb,
            custom_transport_write_cb,
            custom_transport_read_cb,
            reinterpret_cast<void*>(this));

        participant_ = rmw_fastrtps_cpp::get_participant(
            this->get_node_base_interface(),
            this->get_node_topics_interface(),
            domain_id,
            participant_name,
            "", // empty node namespace
            std::vector<std::string>());

        // Register ROS message type with XRCE-DDS
        const rosidl_message_type_support_t* type_support =
            rosidl_typesupport_cpp::get_message_type_support_handle<geometry_msgs::msg::WrenchStamped>();
        uxr_register_topic_xml(
            &session_,
            participant_->impl_->participant,
            topic_name,
            type_support->typesupport_identifier,
            "<dds>"
            "<topic>"
            "<name>force_control_topic</name>"
            "<dataType>geometry_msgs::msg::WrenchStamped_</dataType>"
            "</topic>"
            "</dds>");

        // Create publisher
        publisher_ = rmw_fastrtps_cpp::get_publisher(
            this->get_node_base_interface(),
            this->get_node_topics_interface(),
            participant_,
            type_support,
            topic_name,
            "",
            &rmw_qos_profile_default);

        // Create subscriber
        subscriber_ = rmw_fastrtps_cpp::get_subscriber(
            this->get_node_base_interface(),
            this->get_node_topics_interface(),
            participant_,
            type_support,
            topic_name,
            "",
            &rmw_qos_profile_default,
            false);

        // Create session
        uxr_init_session_xml(&init_options, &session_, participant_->impl_->participant, domain_id);
        UXR_AGENT_LOG_INFO(
            &session_.info,
            UXR_CREATE_ENTITIES_FROM_REF_RESOURCE,
            reinterpret_cast<uint64_t>(participant_->impl_->participant),
            session_.last_requested_resource);

        // Get publisher and subscriber handles
        publisher_handle_ = static_cast<uxrObjectId>(publisher_->publisher_->id_);
        subscriber_handle_ = static_cast<uxrObjectId>(subscriber_->subscriber_->id_);
    }

    void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        // Compute force and torque based on joint positions and publish to topic
        float fx = 0.0;
        float fy = 0.0;
        float fz = 0.0;
        float tx = 0.0;
        float ty = 0.0;
        float tz = 0.0;

        // Compute desired force and torque values here, based on joint positions and other sensor data

        geometry_msgs::msg::WrenchStamped wrench;
        set_wrench(fx, fy, fz, tx, ty, tz, wrench);
        wrench_pub_->publish(wrench);

        // Send desired force and torque values to robot using XRCE-DDS
        uint8_t buffer[1024];
        uint32_t length = 0;
        const uint16_t writer_id = 0x01;
        const uint16_t reader_id = 0x02;

        // Serialize message
        ucdrBuffer ub;
        ucdr_init_buffer(&ub, buffer, sizeof(buffer));
        length = serialize_wrench(msg, &ub);

        // Write message to DDS network
        uxrStreamId output_stream = uxr_create_output_stream(&session_, UXR_RELIABLE_STREAM);
        uxr_prepare_output_stream(&session_, output_stream, publisher_handle_, writer_id, buffer, length);
        uxr_run_session_until_timeout(&session_, DEFAULT_TIMEOUT);
        uxr_delete_output_stream(&session_, output_stream);
    }

    uint32_t serialize_wrench(const geometry_msgs::msg::WrenchStamped::SharedPtr msg, ucdrBuffer* ub)
    {
        uint32_t length = 0;
        length += ucdr_serialize_uint32_t(ub, msg->header.stamp.sec);
        length += ucdr_serialize_uint32_t(ub, msg->header.stamp.nanosec);
        length += ucdr_serialize_float(ub, msg->wrench.force.x);
        length += ucdr_serialize_float(ub, msg->wrench.force.y);
        length += ucdr_serialize_float(ub, msg->wrench.force.z);
        length += ucdr_serialize_float(ub, msg->wrench.torque.x);
        length += ucdr_serialize_float(ub, msg->wrench.torque.y);
        length += ucdr_serialize_float(ub, msg->wrench.torque.z);
        return length;
    }

    static bool custom_transport_open_cb(void* args, const char* ip, uint16_t port)
    {
        return true;
    }

    static bool custom_transport_close_cb(void* args)
    {
        return true;
    }

    static size_t custom_transport_write_cb(
        void* args,
        const uint8_t* buf,
        size_t len,
        uint8_t* errcode)
    {
        auto* node = reinterpret_cast<ForceControlNode*>(args);
        return node->write_cb(buf, len, errcode);
    }

    static size_t custom_transport_read_cb(void* args, uint8_t* buf, size_t len, int timeout, uint8_t* errcode)
    {
        auto* node = reinterpret_cast<ForceControlNode*>(args);
        return node->read_cb(buf, len, timeout, errcode);
    }

    size_t write_cb(const uint8_t* buf, size_t len, uint8_t* errcode)
    {
        // Write data to ROS topic
        geometry_msgs::msg::WrenchStamped wrench;
        ucdrBuffer ub;
        ucdr_init_buffer(&ub, const_cast<uint8_t*>(buf), len);
        deserialize_wrench(&ub, wrench);
        wrench_pub_->publish(wrench);
        return len;
    }

    size_t read_cb(uint8_t* buf, size_t len, int timeout, uint8_t* errcode)
    {
        // Read data from ROS topic
        geometry_msgs::msg::WrenchStamped wrench;
        if (subscriber_->take(&wrench, nullptr, nullptr) == RMW_RET_OK) {
            ucdrBuffer ub;
            ucdr_init_buffer(&ub, buf, len);
            serialize_wrench(wrench, &ub);
            return ub.iterator - ub.init;
        }
        return 0;
    }

    void deserialize_wrench(ucdrBuffer* ub, geometry_msgs::msg::WrenchStamped& wrench)
    {
        wrench.header.stamp.sec = ucdr_deserialize_uint32_t(ub);
        wrench.header.stamp.nanosec = ucdr_deserialize_uint32_t(ub);
        wrench.wrench.force.x = ucdr_deserialize_float(ub);
        wrench.wrench.force.y = ucdr_deserialize_float(ub);
        wrench.wrench.force.z = ucdr_deserialize_float(ub);
        wrench.wrench.torque.x = ucdr_deserialize_float(ub);
        wrench.wrench.torque.y = ucdr_deserialize_float(ub);
        wrench.wrench.torque.z = ucdr_deserialize_float(ub);
    }

    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
    rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_pub_;

    rmw_fastrtps_cpp::Participant* participant_;
    rmw_fastrtps_cpp::Publisher* publisher_;
    rmw_fastrtps_cpp::Subscriber* subscriber_;
    uxrSession session_;
    uxrObjectId publisher_handle_;
    uxrObjectId subscriber_handle_;
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ForceControlNode>());
    rclcpp::shutdown();
    return 0;
}

This example subscribes to joint state data and computes the desired force and torque values based on the current joint positions. It then publishes these values to a ROS topic and sends them to the robot using XRCE-DDS. The robot can then use these values to apply the desired force and torque using a force/torque controller.