diff --git a/launch/decoder.launch b/launch/decoder.launch index 5d24514..e512f3e 100644 --- a/launch/decoder.launch +++ b/launch/decoder.launch @@ -64,7 +64,7 @@ - + @@ -76,4 +76,4 @@ - \ No newline at end of file + diff --git a/launch/driver.launch b/launch/driver.launch index aa45678..432fc63 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -21,17 +21,4 @@ - - - - - - - - - - - \ No newline at end of file + diff --git a/src/decoder.cpp b/src/decoder.cpp index 0b39c6a..fb23a56 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -4,7 +4,7 @@ #include #include "lidar.h" -#include "ouster_ros/OSConfigSrv.h" +#include "ouster_ros/GetMetadata.h" #include "ouster_ros/PacketMsg.h" namespace ouster_decoder { @@ -138,7 +138,7 @@ void Decoder::InitParams() { void Decoder::InitOuster() { ROS_INFO_STREAM("=== Initializing Ouster Decoder ==="); // wait for service - auto client = pnh_.serviceClient("os_config"); + auto client = pnh_.serviceClient("get_metadata"); // NOTE: it is possible that in replay mode, the service was shutdown and // re-advertised. If the client call is too soon, then we risk getting the old @@ -151,10 +151,10 @@ void Decoder::InitOuster() { client.waitForExistence(); - ouster_ros::OSConfigSrv cfg{}; + ouster_ros::GetMetadata srv{}; // Initialize everything if service call is successful - if (client.call(cfg)) { - InitModel(cfg.response.metadata); + if (client.call(srv)) { + InitModel(srv.response.metadata); InitScan(model_); SendTransform(model_); } else { diff --git a/src/driver.cpp b/src/driver.cpp index 43104b2..a85a168 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -13,7 +13,7 @@ */ // This is a modified version of ouster_ros/os_node.cpp -// It is intended to have the same behavior as os_node. +// It is intended to have the same behavior as os_node. // The only difference is that we also advertise the metadata message. #include @@ -23,14 +23,12 @@ #include #include -// #include "ouster/build.h" -#include "ouster/types.h" -#include "ouster_ros/OSConfigSrv.h" +#include "ouster_ros/GetMetadata.h" #include "ouster_ros/PacketMsg.h" -#include "ouster_ros/ros.h" +#include "ouster_ros/os_ros.h" using PacketMsg = ouster_ros::PacketMsg; -using OSConfigSrv = ouster_ros::OSConfigSrv; +using OsGetMetadata = ouster_ros::GetMetadata; namespace sensor = ouster::sensor; // fill in values that could not be parsed from metadata @@ -130,9 +128,9 @@ void advertise_service(ros::NodeHandle& nh, srv.getService().c_str()); srv.shutdown(); } - srv = nh.advertiseService( - "os_config", - [metadata](OSConfigSrv::Request&, OSConfigSrv::Response& res) { + srv = nh.advertiseService( + "get_metadata", + [metadata](OsGetMetadata::Request&, OsGetMetadata::Response& res) { if (metadata.empty()) return false; res.metadata = metadata; return true; @@ -279,9 +277,9 @@ int main(int argc, char** argv) { pub_meta.publish(meta_msg); ROS_INFO("Publish metadata to %s", pub_meta.getTopic().c_str()); - srv = nh.advertiseService( - "os_config", - [metadata](OSConfigSrv::Request&, OSConfigSrv::Response& res) { + srv = nh.advertiseService( + "get_metadata", + [metadata](OsGetMetadata::Request&, OsGetMetadata::Response& res) { if (metadata.empty()) return false; res.metadata = metadata; return true; diff --git a/src/lidar.h b/src/lidar.h index 0c85ad2..6a161ad 100644 --- a/src/lidar.h +++ b/src/lidar.h @@ -4,7 +4,7 @@ #include #include -#include "ouster_ros/ros.h" +#include "ouster_ros/os_ros.h" namespace ouster_decoder {