diff --git a/src/ros_bridge/src/webots_controller.cpp b/src/ros_bridge/src/webots_controller.cpp index 8b75351..ce4e3fd 100644 --- a/src/ros_bridge/src/webots_controller.cpp +++ b/src/ros_bridge/src/webots_controller.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include "ros_bridge/robot_client/robot_client.hpp" @@ -28,6 +28,7 @@ class WebotsController : public rclcpp::Node { // Parameters this->declare_parameter("host", "127.0.0.1"); this->declare_parameter("port", 10001); + this->declare_parameter("devices", "src/ros_bridge/resources/devices.json"); // Publishers clock_publisher_ = this->create_publisher("clock", 10); @@ -52,8 +53,14 @@ class WebotsController : public rclcpp::Node { // Enable devices ActuatorRequests request; Json::Value devices; - std::ifstream json_file("src/ros_bridge/resources/devices.json"); - json_file >> devices; + std::ifstream json_file(this->get_parameter("devices").as_string()); + + // Check if we can open the file + if (json_file.is_open()) { + json_file >> devices; + } else { + throw std::runtime_error("Could not open devices.json, check 'devices' parameter!"); + } // Cameras for (unsigned int i = 0; i < devices["cameras"].size(); i++) { @@ -95,7 +102,10 @@ class WebotsController : public rclcpp::Node { client->sendRequest(request); SensorMeasurements sensors = client->receive(); auto clk = rosgraph_msgs::msg::Clock(); - clk.clock = rclcpp::Time(sensors.time()); + int time = sensors.time(); // Time in ms + clk.clock.sec = time / 1000; + clk.clock.nanosec = (time % 1000) * 1000000; + clock_publisher_->publish(clk); publishImage(sensors); publishSensors(sensors);