websocket_client.cpp

/*
 * This is an example file to get a feeling for the usage of this library.
 * You may also check our unit tests in tests/tests.cpp to see more functions in action.
 *
 * This example file uses the socket_websocket_connection class to talk
 * to the rosbridge server via WebSocket protocol, which is compatible with ROS2.
 */
#include <iostream>
#include <thread>

#include "types.h"
#include "ros_bridge.h"
#include "ros_topic.h"
#include "client/socket_websocket_connection.h"

using namespace rosbridge2cpp;

void connection_error_handler(TransportError err) {
  if(err == TransportError ::R2C_CONNECTION_CLOSED)
    std::cout << "Connection closed - You should reinit ROSBridge" << std::endl;
  if(err == TransportError ::R2C_SOCKET_ERROR)
    std::cout << "Error on ROSBridge Socket - You should reinit ROSBridge" << std::endl;
}


int main()
{
  SocketWebSocketConnection t;
  t.RegisterErrorCallback(connection_error_handler);


  ROSBridge ros(t);
  // ros.enable_bson_mode();
  
  std::cout << "Attempting to connect to ROSBridge server at ws://127.0.0.1:9090..." << std::endl;
  if(!ros.Init("127.0.0.1", 9090))
  {
    std::cerr << "Failed to connect to rosbridge websocket server!" << std::endl;
    std::cerr << "Please ensure that:" << std::endl;
    std::cerr << "1. ROS2 is running (ros2 run rosbridge_server rosbridge_websocket)" << std::endl;
    std::cerr << "2. ROSBridge WebSocket server is running:" << std::endl;
    std::cerr << "   ros2 run rosbridge_server rosbridge_websocket" << std::endl;
    std::cerr << "3. The server is listening on port 9090" << std::endl;
    return 1;
  }
  
  std::cout << "Successfully connected to ROSBridge server!" << std::endl;

  ROSTopic test_topic(ros, "/test_rosbridge2cpp", "std_msgs/String");
  std::cout << "Created topic: " << test_topic.TopicName() << std::endl;

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // std::cout << "Publishing BSON message..." << std::endl;
  // bson_t *message = BCON_NEW(
  //     "data", "Publish from Test Client"
  //     );
  // test_topic.Publish(message);



  std::cout << "Starting continuous publishing loop..." << std::endl;
  while(true){
    std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    
    // Create a JSON message and publish it to the test_topic
    rapidjson::Document json_msg;
    json_msg.SetObject();
    json_msg.AddMember("data", "Publish from Test Client (JSON)", json_msg.GetAllocator());
    test_topic.Publish(json_msg);
    
    std::cout << "Published JSON message to " << test_topic.TopicName() << std::endl;
  }
  
}