Merge pull request '机器狗与雷达代码' (#4) from huaijin into main

.vscode/settings.json

@@ -0,0 +1,15 @@
+{
+    "files.associations": {
+        "complex": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "algorithm": "cpp",
+        "variant": "cpp",
+        "string": "cpp",
+        "system_error": "cpp",
+        "fstream": "cpp",
+        "thread": "cpp",
+        "typeinfo": "cpp",
+        "qwidget": "cpp"
+    }
+}

src/.vscode/settings.json

@@ -0,0 +1,31 @@
+{
+    "files.associations": {
+        "fstream": "cpp",
+        "thread": "cpp",
+        "complex": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "algorithm": "cpp",
+        "typeinfo": "cpp",
+        "array": "cpp",
+        "functional": "cpp",
+        "istream": "cpp",
+        "ostream": "cpp",
+        "ratio": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "variant": "cpp",
+        "condition_variable": "cpp",
+        "string_view": "cpp",
+        "initializer_list": "cpp",
+        "list": "cpp",
+        "optional": "cpp",
+        "system_error": "cpp",
+        "string": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "deque": "cpp",
+        "vector": "cpp"
+    }
+}

src/Client/BattlefieldExplorationSystem.pro

@@ -24,6 +24,8 @@ SOURCES += \
     src/main.cpp \
     src/core/database/UAVDatabase.cpp \
     src/core/database/DogDatabase.cpp \
+    src/core/database/DatabaseConfig.cpp \
+    src/core/database/DatabaseHelper.cpp \
     src/ui/main/MainWindow.cpp \
     src/ui/dialogs/DeviceDialog.cpp \
     src/ui/components/DeviceCard.cpp \
@@ -39,6 +41,8 @@ SOURCES += \
 HEADERS += \
     include/core/database/UAVDatabase.h \
     include/core/database/DogDatabase.h \
+    include/core/database/DatabaseConfig.h \
+    include/core/database/DatabaseHelper.h \
     include/ui/main/MainWindow.h \
     include/ui/dialogs/DeviceDialog.h \
     include/ui/components/DeviceCard.h \

src/Client/config/database.ini

@@ -0,0 +1,39 @@
+# 战场探索系统数据库配置文件
+# Database Configuration for BattlefieldExplorationSystem
+# 
+# 修改此文件中的配置后，重启应用程序生效
+# Restart the application after modifying this configuration file
+
+[Database]
+# 数据库服务器地址 (Database server host)
+host=localhost
+
+# 数据库端口 (Database port)
+port=3306
+
+# 数据库名称 (Database name)
+databaseName=Client
+
+# 数据库用户名 (Database username)
+username=root
+
+# 数据库密码 (Database password)
+password=root
+
+# 连接超时时间，单位毫秒 (Connection timeout in milliseconds)
+connectionTimeout=30000
+
+# 数据库驱动名称 (Database driver name)
+driverName=QMYSQL
+
+# 配置说明:
+# 1. 如果你的MySQL用户名不是root，请修改username字段
+# 2. 如果你的MySQL密码不是root，请修改password字段
+# 3. 如果你的MySQL运行在不同的主机或端口，请修改host和port字段
+# 4. 如果你使用不同的数据库名称，请修改databaseName字段
+#
+# Configuration Notes:
+# 1. If your MySQL username is not 'root', modify the username field
+# 2. If your MySQL password is not 'root', modify the password field  
+# 3. If your MySQL runs on different host or port, modify host and port fields
+# 4. If you use different database name, modify databaseName field

src/Client/src/core/database/DogDatabase.cpp

@@ -1,4 +1,5 @@
 #include "core/database/DogDatabase.h"
+#include "core/database/DatabaseConfig.h"
 
 // 单例实例指针定义
 DogDatabase *DogDatabase::m_instance = nullptr;
@@ -27,7 +28,8 @@ DogDatabase::DogDatabase()
 //添加记录
 bool DogDatabase::addDevice(const Dog &data)
 {
-    if(openDatabase("fly_land_database","root","hzk200407140238"))
+    DatabaseConnectionInfo dbInfo = DatabaseConfig::getInstance()->getConnectionInfo();
+    if(openDatabase(dbInfo.databaseName, dbInfo.username, dbInfo.password))
     {
         beginAddField("dogdatabase");
         addField("id");
@@ -63,7 +65,8 @@ Point DogDatabase::getDevicePosition(const QString &id)
     position.lon = 0;
     position.lat = 0;
 
-    if(openDatabase("fly_land_database","root","hzk200407140238"))
+    DatabaseConnectionInfo dbInfo = DatabaseConfig::getInstance()->getConnectionInfo();
+    if(openDatabase(dbInfo.databaseName, dbInfo.username, dbInfo.password))
     {
         QSqlQuery query(m_sqlDatabase);
 
@@ -99,7 +102,8 @@ Point DogDatabase::getDevicePosition(const QString &id)
 int DogDatabase::getDeviceState(const QString &id)
 {
     int state = -1;
-    if(openDatabase("fly_land_database","root","hzk200407140238"))
+    DatabaseConnectionInfo dbInfo = DatabaseConfig::getInstance()->getConnectionInfo();
+    if(openDatabase(dbInfo.databaseName, dbInfo.username, dbInfo.password))
     {
         QSqlQuery query(m_sqlDatabase);
 

src/Client/src/core/database/UAVDatabase.cpp

@@ -1,4 +1,5 @@
 #include "core/database/UAVDatabase.h"
+#include "core/database/DatabaseConfig.h"
 
 UAVDatabase *UAVDatabase::getInstance()
 {
@@ -21,7 +22,8 @@ UAVDatabase::UAVDatabase()
 //添加记录
 bool UAVDatabase::add(const UAV &data)
 {
-    if(open("fly_land_database","root","hzk200407140238"))
+    DatabaseConnectionInfo dbInfo = DatabaseConfig::getInstance()->getConnectionInfo();
+    if(open(dbInfo.databaseName, dbInfo.username, dbInfo.password))
     {
     beginAddFiled("uavdatabase");
     addFiled("id");
@@ -57,7 +59,8 @@ Point UAVDatabase::ReturnUAVPosition(QString id)
     position.lon = 0;
     position.lat = 0;
 
-    if(open("fly_land_database","root","hzk200407140238"))
+    DatabaseConnectionInfo dbInfo = DatabaseConfig::getInstance()->getConnectionInfo();
+    if(open(dbInfo.databaseName, dbInfo.username, dbInfo.password))
     {
     QSqlQuery query(m_sqlDb);
 
@@ -93,7 +96,8 @@ Point UAVDatabase::ReturnUAVPosition(QString id)
 int UAVDatabase::giveInfo(QString id)
 {
     int state = -1;
-    if(open("fly_land_database","root","hzk200407140238"))
+    DatabaseConnectionInfo dbInfo = DatabaseConfig::getInstance()->getConnectionInfo();
+    if(open(dbInfo.databaseName, dbInfo.username, dbInfo.password))
     {
         QSqlQuery query(m_sqlDb);
 

src/Client/src/ui/components/DeviceCard.cpp

@@ -8,7 +8,11 @@
 
 #include "ui/components/DeviceCard.h"
 #include "utils/SystemLogger.h"
+<<<<<<< HEAD
+#include "core/database/DatabaseHelper.h"
+=======
 #include "styles/LeftPanelStyleManager.h"
+>>>>>>> cfdfc879c736be6524c03273d48cc75ec7c1d523
 
 // Qt GUI头文件
 #include <QMouseEvent>
@@ -732,12 +736,7 @@ bool DeviceCard::updateDeviceStatusInDatabase(DeviceStatus status)
 {
     // 创建唯一的数据库连接名称（包含时间戳避免重复）
     QString connectionName = QString("DeviceCard_%1_%2").arg(m_deviceInfo.id).arg(QDateTime::currentMSecsSinceEpoch());
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", connectionName);
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createConnection(connectionName);
     
     if (!db.open()) {
         qWarning() << "Failed to connect to database for device status update:" << db.lastError().text();

src/Client/src/ui/components/DeviceListPanel.cpp

@@ -8,7 +8,11 @@
 
 #include "ui/components/DeviceListPanel.h"
 #include "utils/SystemLogger.h"
+<<<<<<< HEAD
+#include "core/database/DatabaseHelper.h"
+=======
 #include "styles/LeftPanelStyleManager.h"
+>>>>>>> cfdfc879c736be6524c03273d48cc75ec7c1d523
 
 // Qt GUI头文件
 #include <QDebug>
@@ -488,12 +492,7 @@ QList<DeviceInfo> DeviceListPanel::loadDevicesFromDatabase()
         qDebug() << "Attempting to connect to unified devices database...";
         
         // 创建数据库连接
-        QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "DeviceListPanel_Connection");
-        db.setHostName("localhost");
-        db.setPort(3306);
-        db.setDatabaseName("Client");
-        db.setUserName("root");
-        db.setPassword("hzk200407140238");
+        QSqlDatabase db = DatabaseHelper::createTempConnection("DeviceListPanel_Connection");
         
         if (db.open()) {
             qDebug() << "Successfully connected to Client database";
@@ -568,12 +567,7 @@ bool DeviceListPanel::deleteDeviceFromDatabase(const QString &deviceId)
     qDebug() << "Attempting to delete device from database:" << deviceId;
     
     // 创建数据库连接
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "DeviceListPanel_Delete_Connection");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("DeviceListPanel_Delete_Connection");
     
     bool success = false;
     

src/Client/src/ui/dialogs/DeviceDialog.cpp

@@ -9,6 +9,7 @@
 #include "ui/dialogs/DeviceDialog.h"
 #include "build/ui_DeviceDialog.h"
 #include "utils/SystemLogger.h"
+#include "core/database/DatabaseHelper.h"
 
 // Qt headers
 #include <QMessageBox>
@@ -156,12 +157,7 @@ void DeviceDialog::setDeviceInfo(const QString &deviceId, const QString &name, c
 void DeviceDialog::loadOperationLogs(const QString &deviceId)
 {
     // 连接数据库
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "DeviceDialog_LogQuery");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("DeviceDialog_LogQuery");
     
     if (!db.open()) {
         qWarning() << "Failed to connect to database for operation logs:" << db.lastError().text();
@@ -228,12 +224,7 @@ void DeviceDialog::refreshDeviceInfo()
     }
     
     // 连接数据库
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "DeviceDialog_Refresh");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("DeviceDialog_Refresh");
     
     if (!db.open()) {
         qWarning() << "Failed to connect to database for refresh:" << db.lastError().text();
@@ -573,12 +564,7 @@ void DeviceDialog::initializeOperationLogTable()
 void DeviceDialog::logOperation(const QString &operation, const QString &operatorName)
 {
     // 记录操作到数据库
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "DeviceDialog_LogOperation");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("DeviceDialog_LogOperation");
     
     if (!db.open()) {
         qWarning() << "Failed to connect to database for logging operation:" << db.lastError().text();

src/Client/src/ui/main/MainWindow.cpp

@@ -10,7 +10,11 @@
 #include "build/ui_MainWindow.h"
 #include "ui/dialogs/DeviceDialog.h"
 #include "utils/SystemLogger.h"
+<<<<<<< HEAD
+#include "core/database/DatabaseHelper.h"
+=======
 #include "styles/ModernStyleManager.h"
+>>>>>>> cfdfc879c736be6524c03273d48cc75ec7c1d523
 
 // Qt GUI头文件
 #include <QWebEngineView>
@@ -519,12 +523,7 @@ void MainWindow::onAddRobotClicked()
     double defaultLatitude = 39.9;   // 默认纬度
     
     // 尝试从数据库获取配置的地图中心位置
-    QSqlDatabase configDb = QSqlDatabase::addDatabase("QMYSQL", "TempConnection_Config_Robot");
-    configDb.setHostName("localhost");
-    configDb.setPort(3306);
-    configDb.setDatabaseName("Client"); 
-    configDb.setUserName("root");
-    configDb.setPassword("hzk200407140238");
+    QSqlDatabase configDb = DatabaseHelper::createTempConnection("TempConnection_Config_Robot");
     
     if (configDb.open()) {
         QSqlQuery configQuery(configDb);
@@ -556,12 +555,7 @@ void MainWindow::onAddRobotClicked()
     latitudeEdit->setToolTip(QString("默认地图中心纬度: %1°N").arg(defaultLatitude));
     
     // 自动生成设备ID
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "TempConnection_DOG_ID_Add");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client"); 
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("TempConnection_DOG_ID_Add");
     
     QString suggestedId = "DOG001";
     if (db.open()) {
@@ -710,12 +704,7 @@ void MainWindow::onAddUAVClicked()
     double defaultLatitude = 39.9;   // 默认纬度
     
     // 尝试从数据库获取配置的地图中心位置
-    QSqlDatabase configDb = QSqlDatabase::addDatabase("QMYSQL", "TempConnection_Config");
-    configDb.setHostName("localhost");
-    configDb.setPort(3306);
-    configDb.setDatabaseName("Client"); 
-    configDb.setUserName("root");
-    configDb.setPassword("hzk200407140238");
+    QSqlDatabase configDb = DatabaseHelper::createTempConnection("TempConnection_Config");
     
     if (configDb.open()) {
         QSqlQuery configQuery(configDb);
@@ -747,12 +736,7 @@ void MainWindow::onAddUAVClicked()
     latitudeEdit->setToolTip(QString("默认地图中心纬度: %1°N").arg(defaultLatitude));
     
     // 自动生成设备ID
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "TempConnection_UAV_ID");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client"); 
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("TempConnection_UAV_ID");
     
     QString suggestedId = "UAV001";
     if (db.open()) {
@@ -999,12 +983,7 @@ void MainWindow::onDeviceDetailsRequested(const QString &deviceId)
     qDebug() << "Device details requested for:" << deviceId;
     
     // 连接数据库获取设备详细信息
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "MainWindow_DeviceDetails");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("MainWindow_DeviceDetails");
     
     if (!db.open()) {
         qWarning() << "Failed to connect to database for device details:" << db.lastError().text();
@@ -1173,12 +1152,7 @@ void MainWindow::initializeDeviceMarkersOnMap()
 
 bool MainWindow::addDeviceToDatabase(const QString &deviceId, const QString &name, const QString &type, const QString &ip, int port, int state, double longitude, double latitude)
 {
-    QSqlDatabase db = QSqlDatabase::addDatabase("QMYSQL", "MainWindow_AddDevice_Connection");
-    db.setHostName("localhost");
-    db.setPort(3306);
-    db.setDatabaseName("Client");
-    db.setUserName("root");
-    db.setPassword("hzk200407140238");
+    QSqlDatabase db = DatabaseHelper::createTempConnection("MainWindow_AddDevice_Connection");
     
     if (!db.open()) {
         qWarning() << "Failed to connect to database for adding device:" << db.lastError().text();

src/catkin_dog/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/CMakeLists.txt

@@ -0,0 +1,204 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(slamware_ros_sdk)
+
+find_path(slamware_sdk_INCLUDE_DIR rpos/rpos.h ${PROJECT_SOURCE_DIR}/../slamware_sdk/include)
+find_path(slamware_sdk_LIBRARY librpos_framework.a ${PROJECT_SOURCE_DIR}/../slamware_sdk/lib)
+if(slamware_sdk_INCLUDE_DIR AND slamware_sdk_LIBRARY)
+  set(SLTC_SDK_INC_DIR "${slamware_sdk_INCLUDE_DIR}")
+  set(SLTC_SDK_LIB_DIR "${slamware_sdk_LIBRARY}")
+else(slamware_sdk_INCLUDE_DIR AND slamware_sdk_LIBRARY)
+  if(NOT DEFINED SLTC_SDK_ROOT OR SLTC_SDK_ROOT STREQUAL "")
+    set(SLTC_SDK_ROOT "/home/slamtec/slamware_sdk_linux-x86_64")
+  endif()
+  set(SLTC_SDK_DIR "${SLTC_SDK_ROOT}/linux-x86_64-release")
+  set(SLTC_SDK_INC_DIR "${SLTC_SDK_DIR}/include")
+  set(SLTC_SDK_LIB_DIR "${SLTC_SDK_DIR}/lib")
+endif(slamware_sdk_INCLUDE_DIR AND slamware_sdk_LIBRARY)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -std=gnu++11")
+
+find_package(catkin REQUIRED COMPONENTS
+  nav_msgs
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+  tf
+  message_generation
+)
+
+find_package(Threads)
+
+add_message_files(
+  FILES
+  OptionalBool.msg
+  OptionalInt8.msg
+  OptionalInt16.msg
+  OptionalInt32.msg
+  OptionalInt64.msg
+  OptionalUInt8.msg
+  OptionalUInt16.msg
+  OptionalUInt32.msg
+  OptionalUInt64.msg
+  OptionalFlt32.msg
+  OptionalFlt64.msg
+  Vec2DInt32.msg
+  Vec2DFlt32.msg
+  Line2DFlt32.msg
+  Line2DFlt32Array.msg
+  RectInt32.msg
+  RectFlt32.msg
+  RobotDeviceInfo.msg
+  MapKind.msg
+  ArtifactUsage.msg
+  SensorType.msg
+  ImpactType.msg
+  BasicSensorInfo.msg
+  BasicSensorInfoArray.msg
+  BasicSensorValue.msg
+  BasicSensorValueData.msg
+  BasicSensorValueDataArray.msg
+  ActionDirection.msg
+  RobotBasicState.msg
+  SyncMapRequest.msg
+  MoveOptionFlag.msg
+  MoveOptions.msg
+  MoveByDirectionRequest.msg
+  MoveByThetaRequest.msg
+  MoveToRequest.msg
+  MoveToLocationsRequest.msg
+  RotateToRequest.msg
+  RotateRequest.msg
+  LocalizationMovement.msg
+  OptionalLocalizationMovement.msg
+  LocalizationOptions.msg
+  RecoverLocalizationRequest.msg
+  ClearMapRequest.msg
+  SetMapUpdateRequest.msg
+  SetMapLocalizationRequest.msg
+  GoHomeRequest.msg
+  CancelActionRequest.msg
+  AddLineRequest.msg
+  AddLinesRequest.msg
+  RemoveLineRequest.msg
+  ClearLinesRequest.msg
+  MoveLineRequest.msg
+  MoveLinesRequest.msg
+)
+
+add_service_files(
+  FILES
+  SyncGetStcm.srv
+  SyncSetStcm.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  nav_msgs
+  sensor_msgs
+  std_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES slamware_ros_sdk
+  CATKIN_DEPENDS nav_msgs roscpp rospy sensor_msgs std_msgs tf message_runtime
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+message("${INCLUDE_DIRECTORIES}")
+
+add_library(${PROJECT_NAME} STATIC
+  src/client/slamware_ros_sdk_client.cpp
+)
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
+
+add_executable(slamware_ros_sdk_server_node
+  src/server/msg_convert.cpp
+  src/server/server_params.cpp
+  src/server/server_map_holder.cpp
+  src/server/server_work_data.cpp
+  src/server/server_worker_base.cpp
+  src/server/server_workers.cpp
+  src/server/slamware_ros_sdk_server_node.cpp
+  src/server/slamware_ros_sdk_server.cpp
+)
+
+add_executable(go1_slamware_bridge_node
+  src/server/go1_slamware_bridge_node.cpp
+)
+
+add_dependencies(slamware_ros_sdk_server_node ${PROJECT_NAME}_generate_messages_cpp)
+add_dependencies(go1_slamware_bridge_node ${PROJECT_NAME}_generate_messages_cpp)
+
+target_include_directories(slamware_ros_sdk_server_node
+  PRIVATE ${SLTC_SDK_INC_DIR}
+)
+
+target_include_directories(go1_slamware_bridge_node
+  PRIVATE ${SLTC_SDK_INC_DIR}
+)
+
+target_compile_options(slamware_ros_sdk_server_node
+  PRIVATE -Wno-deprecated-declarations
+)
+
+target_compile_options(go1_slamware_bridge_node
+  PRIVATE -Wno-deprecated-declarations
+)
+
+target_link_libraries(slamware_ros_sdk_server_node
+  ${SLTC_SDK_LIB_DIR}/librpos_robotplatforms_rpslamware.a
+  ${SLTC_SDK_LIB_DIR}/librpos_framework.a
+  ${SLTC_SDK_LIB_DIR}/libbase64.a
+  ${SLTC_SDK_LIB_DIR}/librlelib.a
+  ${SLTC_SDK_LIB_DIR}/libjsoncpp.a
+  ${SLTC_SDK_LIB_DIR}/libcurl.a
+  ${SLTC_SDK_LIB_DIR}/libcares.a
+  ${SLTC_SDK_LIB_DIR}/libssl.a
+  ${SLTC_SDK_LIB_DIR}/libcrypto.a
+  ${SLTC_SDK_LIB_DIR}/libboost_atomic.a
+  ${SLTC_SDK_LIB_DIR}/libboost_chrono.a
+  ${SLTC_SDK_LIB_DIR}/libboost_date_time.a
+  ${SLTC_SDK_LIB_DIR}/libboost_regex.a 
+  ${SLTC_SDK_LIB_DIR}/libboost_filesystem.a
+  ${SLTC_SDK_LIB_DIR}/libboost_system.a
+  ${SLTC_SDK_LIB_DIR}/libboost_thread.a
+  ${SLTC_SDK_LIB_DIR}/libboost_random.a
+  ${SLTC_SDK_LIB_DIR}/libz.a
+  ${catkin_LIBRARIES}
+  pthread
+  dl
+  rt
+)
+
+target_link_libraries(go1_slamware_bridge_node
+  ${SLTC_SDK_LIB_DIR}/libjsoncpp.a
+  ${catkin_LIBRARIES}
+  pthread
+)
+
+install(TARGETS ${PROJECT_NAME} slamware_ros_sdk_server_node go1_slamware_bridge_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+install(FILES
+  launch/slamware_ros_sdk_server_node.launch
+  launch/view_slamware_ros_sdk_server_node.launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+install(DIRECTORY rviz
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/LICENSE

@@ -0,0 +1,24 @@
+Copyright (c) 2009 - 2014 RoboPeak Team
+Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/README.md

@@ -0,0 +1 @@
+# slamware_ros_sdk

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/cfg/diff_drive/costmap_common_params.yaml

@@ -0,0 +1,30 @@
+#---(in meters)---
+robot_radius: 0.0
+footprint_padding: 0.0
+footprint: [[-0.4,0.25],[0.4,0.25],[0.4,-0.25],[-0.4,-0.25]]  
+transform_tolerance: 0.2
+map_type: costmap
+
+always_send_full_costmap: true
+
+obstacle_layer:
+ enabled: true
+ obstacle_range: 3.0
+ raytrace_range: 4.0
+ inflation_radius: 0.2
+ track_unknown_space: true 
+ combination_method: 1  
+
+ observation_sources: laser_scan_sensor
+ laser_scan_sensor: {data_type: LaserScan, topic: /slamware_ros_sdk_server_node/scan, marking: true, clearing: true}
+
+
+inflation_layer:
+  enabled:              true
+  cost_scaling_factor:  0.55 
+  inflation_radius:     0.5 
+
+static_layer:
+  enabled:              true
+  map_topic:            "/slamware_ros_sdk_server_node/map"
+

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/cfg/diff_drive/global_costmap_params.yaml

@@ -0,0 +1,12 @@
+global_costmap:
+  global_frame: slamware_map
+  robot_base_frame: base_link
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+ 
+  transform_tolerance: 1.0
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/cfg/diff_drive/local_costmap_params.yaml

@@ -0,0 +1,15 @@
+local_costmap:
+  global_frame: slamware_map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 4
+  height: 4
+  resolution: 0.1
+  transform_tolerance: 0.5
+  robot_radius: 0.0
+  plugins:
+   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/cfg/diff_drive/teb_local_planner_params.yaml

@@ -0,0 +1,86 @@
+TebLocalPlannerROS:
+
+ odom_topic: slamware_ros_sdk_server_node/odom
+    
+#Trajectory
+teb_autosize: True
+dt_ref: 0.3
+dt_hysteresis: 0.1
+global_plan_overwrite_orientation: True
+allow_init_with_backwards_motion: True
+max_global_plan_lookahead_dist: 3.0
+feasibility_check_no_poses: 5
+    
+# Robot
+max_vel_x: 0.7
+max_vel_x_backwards: 0.5
+max_vel_y: 0.0
+max_vel_theta: 0.2
+acc_lim_x: 0.2
+acc_lim_theta: 0.2
+min_turning_radius: 0.0
+
+footprint_model:
+    type:  "polygon" 
+    vertices: [[-0.4,0.25],[0.4,0.25],[0.4,-0.25],[-0.4,-0.25]]  
+
+# GoalTolerance
+xy_goal_tolerance: 0.2
+yaw_goal_tolerance: 0.1
+free_goal_vel: False
+    
+# Obstacles
+min_obstacle_dist: 0.1 
+include_costmap_obstacles: True
+costmap_obstacles_behind_robot_dist: 0.5
+obstacle_poses_affected: 20
+costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
+costmap_converter_spin_thread: True
+costmap_converter_rate: 5
+costmap_converter/CostmapToLinesDBSRANSAC:
+    cluster_max_distance: 0.4
+    cluster_min_pts: 2
+    ransac_inlier_distance: 0.15
+    ransac_min_inliers: 10
+    ransac_no_iterations: 1500
+    ransac_remainig_outliers: 3
+    ransac_convert_outlier_pts: True
+    ransac_filter_remaining_outlier_pts: False
+    convex_hull_min_pt_separation: 0.1
+
+# Optimization
+no_inner_iterations: 4
+no_outer_iterations: 3
+optimization_activate: True
+optimization_verbose: False
+penalty_epsilon: 0.05
+weight_max_vel_x: 5
+weight_max_vel_theta: 1
+weight_acc_lim_x: 1
+weight_acc_lim_theta: 1
+weight_kinematics_nh: 1000
+weight_kinematics_forward_drive: 1
+weight_kinematics_turning_radius: 1
+weight_optimaltime: 1
+weight_obstacle: 50
+weight_dynamic_obstacle: 10 
+weight_adapt_factor: 2
+
+# Homotopy Class Planner
+enable_homotopy_class_planning: True
+enable_multithreading: True
+simple_exploration: False
+max_number_classes: 3
+selection_cost_hysteresis: 1.0
+selection_obst_cost_scale: 1.0
+selection_alternative_time_cost: False
+ 
+roadmap_graph_no_samples: 15
+roadmap_graph_area_width: 5
+h_signature_prescaler: 0.5
+h_signature_threshold: 0.1
+obstacle_keypoint_offset: 0.1
+obstacle_heading_threshold: 0.45
+visualize_hc_graph: False
+
+ 

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/include/slamware_ros_sdk/slamware_ros_sdk_client.h

@@ -0,0 +1,137 @@
+
+#pragma once
+
+#include <ros/ros.h>
+#include <tf/message_filter.h>
+
+#include "utils.h"
+
+#include <slamware_ros_sdk/Vec2DInt32.h>
+#include <slamware_ros_sdk/Line2DFlt32Array.h>
+#include <slamware_ros_sdk/RectInt32.h>
+#include <slamware_ros_sdk/RobotDeviceInfo.h>
+#include <slamware_ros_sdk/BasicSensorInfoArray.h>
+#include <slamware_ros_sdk/BasicSensorValueDataArray.h>
+#include <slamware_ros_sdk/RobotBasicState.h>
+#include <slamware_ros_sdk/SyncMapRequest.h>
+#include <slamware_ros_sdk/MoveByDirectionRequest.h>
+#include <slamware_ros_sdk/MoveByThetaRequest.h>
+#include <slamware_ros_sdk/MoveToRequest.h>
+#include <slamware_ros_sdk/MoveToLocationsRequest.h>
+#include <slamware_ros_sdk/RotateToRequest.h>
+#include <slamware_ros_sdk/RotateRequest.h>
+#include <slamware_ros_sdk/RecoverLocalizationRequest.h>
+#include <slamware_ros_sdk/ClearMapRequest.h>
+#include <slamware_ros_sdk/SetMapUpdateRequest.h>
+#include <slamware_ros_sdk/SetMapLocalizationRequest.h>
+#include <slamware_ros_sdk/GoHomeRequest.h>
+#include <slamware_ros_sdk/CancelActionRequest.h>
+#include <slamware_ros_sdk/AddLineRequest.h>
+#include <slamware_ros_sdk/AddLinesRequest.h>
+#include <slamware_ros_sdk/RemoveLineRequest.h>
+#include <slamware_ros_sdk/ClearLinesRequest.h>
+#include <slamware_ros_sdk/MoveLineRequest.h>
+#include <slamware_ros_sdk/MoveLinesRequest.h>
+
+#include <slamware_ros_sdk/SyncGetStcm.h>
+#include <slamware_ros_sdk/SyncSetStcm.h>
+
+#include <boost/filesystem/path.hpp>
+
+namespace slamware_ros_sdk {
+
+    class SlamwareRosSdkClient
+    {
+    public:
+        typedef boost::filesystem::path             fs_path_t;
+
+    public:
+        explicit SlamwareRosSdkClient(ros::NodeHandle& nhRos
+            , const char* serverNodeName = nullptr
+            , const char* msgNamePrefix = nullptr
+            );
+        ~SlamwareRosSdkClient();
+
+    public:
+        //////////////////////////////////////////////////////////////////////////
+
+        void syncMap(const SyncMapRequest& msg) { return pubSyncMap_.publish(msg); }
+        void setPose(const geometry_msgs::Pose& msg) { pubSetPose_.publish(msg); }
+
+        void recoverLocalization(const RecoverLocalizationRequest& msg) { pubRecoverLocalization_.publish(msg); }
+        void clearMap(const ClearMapRequest& msg) { pubClearMap_.publish(msg); }
+        void setMapUpdate(const SetMapUpdateRequest& msg) { pubSetMapUpdate_.publish(msg); }
+        void setMapLocalization(const SetMapLocalizationRequest& msg) { pubSetMapLocalization_.publish(msg); }
+
+        void moveBy(const MoveByDirectionRequest& msg) { pubMoveByDirection_.publish(msg); }
+        void moveBy(const MoveByThetaRequest& msg) { pubMoveByTheta_.publish(msg); }
+        void moveTo(const MoveToRequest& msg) { pubMoveTo_.publish(msg); }
+        void moveTo(const MoveToLocationsRequest& msg) { pubMoveToLocations_.publish(msg); }
+        void rotateTo(const RotateToRequest& msg) { pubRotateTo_.publish(msg); }
+        void rotate(const RotateRequest& msg) { pubRotate_.publish(msg); }
+
+        void goHome(const GoHomeRequest& msg) { pubGoHome_.publish(msg); }
+        void cancelAction(const CancelActionRequest& msg) { pubCancelAction_.publish(msg); }
+
+        void addLine(const AddLineRequest& msg) { pubAddLine_.publish(msg); }
+        void addLines(const AddLinesRequest& msg) { pubAddLines_.publish(msg); }
+        void removeLine(const RemoveLineRequest& msg) { pubRemoveLine_.publish(msg); }
+        void clearLines(const ClearLinesRequest& msg) { pubClearLines_.publish(msg); }
+        void moveLine(const MoveLineRequest& msg) { pubMoveLine_.publish(msg); }
+        void moveLines(const MoveLinesRequest& msg) { pubMoveLines_.publish(msg); }
+
+        //////////////////////////////////////////////////////////////////////////
+
+        bool syncGetStcm(SyncGetStcm& srvMsg) { return scSyncGetStcm_.call(srvMsg); }
+        // get stcm and write to filePath.
+        bool syncGetStcm(std::string& errMsg
+            , const fs_path_t& filePath
+            );
+
+        bool syncSetStcm(SyncSetStcm& srvMsg) { return scSyncSetStcm_.call(srvMsg); }
+        // load stcm from filePath, and upload to slamware.
+        bool syncSetStcm(const fs_path_t& filePath
+            , const geometry_msgs::Pose& robotPose
+            , std::string& errMsg
+            );
+
+        //////////////////////////////////////////////////////////////////////////
+
+    private:
+        std::string genTopicFullName_(const std::string& strName) const { return msgNamePrefix_ + strName; }
+
+    private:
+        ros::NodeHandle* nh_;
+        std::string sdkServerNodeName_;
+        std::string msgNamePrefix_;
+
+        ros::Publisher pubSyncMap_;
+        ros::Publisher pubSetPose_;
+
+        ros::Publisher pubRecoverLocalization_;
+        ros::Publisher pubClearMap_;
+        ros::Publisher pubSetMapUpdate_;
+        ros::Publisher pubSetMapLocalization_;
+
+        ros::Publisher pubMoveByDirection_;
+        ros::Publisher pubMoveByTheta_;
+        ros::Publisher pubMoveTo_;
+        ros::Publisher pubMoveToLocations_;
+        ros::Publisher pubRotateTo_;
+        ros::Publisher pubRotate_;
+
+        ros::Publisher pubGoHome_;
+        ros::Publisher pubCancelAction_;
+
+        ros::Publisher pubAddLine_;
+        ros::Publisher pubAddLines_;
+        ros::Publisher pubRemoveLine_;
+        ros::Publisher pubClearLines_;
+        ros::Publisher pubMoveLine_;
+        ros::Publisher pubMoveLines_;
+
+        ros::ServiceClient scSyncGetStcm_;
+        ros::ServiceClient scSyncSetStcm_;
+    };
+
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/include/slamware_ros_sdk/utils.h

@@ -0,0 +1,37 @@
+
+#pragma once
+
+#include <cstdint>
+
+#include <boost/optional.hpp>
+
+namespace slamware_ros_sdk {
+
+    template<class ValT, class RosMsgT>
+    inline void optionalToRosMsg(const boost::optional<ValT> & optVal, RosMsgT& rosMsg)
+    {
+        if (optVal)
+        {
+            rosMsg.is_valid = true;
+            rosMsg.value = (*optVal);
+        }
+        else
+        {
+            rosMsg = RosMsgT();
+        }
+    }
+
+    template<class ValT, class RosMsgT>
+    inline void rosMsgToOptional(const RosMsgT& rosMsg, boost::optional<ValT> & optVal)
+    {
+        if (rosMsg.is_valid)
+        {
+            optVal = rosMsg.value;
+        }
+        else
+        {
+            optVal.reset();
+        }
+    }
+
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/launch/go1_slamware_bridge.launch

@@ -0,0 +1,42 @@
+<launch>
+    <!-- 参数配置 -->
+    <arg name="slamware_ip" default="192.168.11.1" doc="思岚雷达的IP地址"/>
+    <arg name="data_port" default="8888" doc="数据传输端口"/>
+    <arg name="control_port" default="8889" doc="控制命令端口"/>
+    
+    <!-- 启动思岚雷达ROS SDK服务器节点 -->
+    <include file="$(find slamware_ros_sdk)/launch/slamware_ros_sdk_server_node.launch">
+        <arg name="ip_address" value="$(arg slamware_ip)"/>
+        <arg name="raw_ladar_data" value="false"/>
+    </include>
+    
+    <!-- 启动GO1桥接节点 -->
+    <node name="go1_slamware_bridge" pkg="slamware_ros_sdk" type="go1_slamware_bridge_node" output="screen">
+        <param name="slamware_ip" value="$(arg slamware_ip)"/>
+        <param name="data_port" value="$(arg data_port)"/>
+        <param name="control_port" value="$(arg control_port)"/>
+        
+        <!-- 数据发送频率配置 -->
+        <param name="scan_rate" value="10.0"/>
+        <param name="map_rate" value="5.0"/>
+        <param name="odom_rate" value="20.0"/>
+        <param name="pose_rate" value="20.0"/>
+        
+        <!-- 话题重映射，确保使用slamware_ros_sdk的标准话题 -->
+        <remap from="scan" to="scan"/>
+        <remap from="slamware_map" to="slamware_map"/>
+        <remap from="odom" to="odom"/>
+        <remap from="robot_pose" to="robot_pose"/>
+    </node>
+    
+    <!-- 可选：启动RViz查看GO1端数据 -->
+    <arg name="rviz" default="false"/>
+    <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz" 
+          args="-d $(find slamware_ros_sdk)/rviz/slamware_ros_sdk_server_node.rviz"/>
+          
+    <!-- 日志和状态监控 -->
+    <node name="bridge_monitor" pkg="slamware_ros_sdk" type="bridge_monitor.py" output="log">
+        <param name="monitor_topics" value="scan,slamware_map,odom"/>
+        <param name="log_interval" value="30.0"/>
+    </node>
+</launch> 

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/launch/slamware_ros_sdk_server_and_view.launch

@@ -0,0 +1,10 @@
+<!--
+  Used for visualising rplidar in action.  
+  
+  It requires rplidar.launch.
+ -->
+<launch>
+  <include file="$(find slamware_ros_sdk)/launch/slamware_ros_sdk_server_node.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slamware_ros_sdk)/rviz/slamware_ros_sdk_server_node.rviz" />
+</launch>

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/launch/slamware_ros_sdk_server_node.launch

@@ -0,0 +1,47 @@
+<launch>
+  <arg name="ip_address" default="192.168.11.1" />
+  <arg name="move_base_goal_topic" default="/move_base_simple/goal"/>
+  <arg name="raw_ladar_data" default="false" />
+
+  <node name="slamware_ros_sdk_server_node" pkg="slamware_ros_sdk" type="slamware_ros_sdk_server_node" output="screen">
+
+  <param name = "ip_address"               value = "$(arg ip_address)"/>
+  <param name = "angle_compensate"         value = "true"/>
+  <param name = "fixed_odom_map_tf"        value = "true"/>
+
+  <param name = "robot_frame"              value = "base_link"/>
+  <param name = "odom_frame"               value = "odom"/>
+  <param name = "laser_frame"              value = "laser"/>
+  <param name = "map_frame"                value = "slamware_map"/>
+  <param name = "robot_pose_frame"            value = "robot_pose"/>
+
+  <param name = "odometry_pub_period"      value = "0.05"/>  <!-- pub_accumulate_odometry = false pub robot pose data -->
+  <param name = "robot_pose_pub_period"      value = "0.05"/>
+  <param name = "scan_pub_period"            value = "0.1"/>
+  <param name = "map_pub_period"             value = "0.2"/>
+  <param name = "path_pub_period"            value = "0.05"/>
+  <param name = "imu_raw_data_period"        value = "0.05"/>
+  <param name = "virtual_walls_pub_period"   value = "0.5"/>
+  <param name = "virtual_tracks_pub_period"  value = "0.5"/>
+  <param name = "basic_sensors_values_pub_period"   value = "0.05"/>
+
+  <param name = "vel_control_topic"          value = "cmd_vel"/>
+  <param name = "raw_ladar_data"             value = "$(arg raw_ladar_data)"/> 
+  <param name = "ladar_data_clockwise"       value = "true"/>
+
+  <param name = "pub_accumulate_odometry"     value = "false"/>
+  <param name = "robot_pose_topic"            value = "robot_pose"/>
+
+  <!-- publish topic remap /-->
+  <remap from = "scan"                       to = "scan"/>
+  <remap from = "odom"                       to = "odom"/>
+  <remap from = "map"                        to = "slamware_map"/>
+  <remap from = "map_metadata"               to = "map_metadata"/>
+  <remap from = "global_plan_path"           to = "global_plan_path"/>
+  </node>
+
+  <node name="map2odom" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 slamware_map odom 100"/>
+  <node name="map2robotpose" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 slamware_map robot_pose 100"/>
+  <node name="base2laser" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_link laser 100"/>
+
+</launch>

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/launch/view_slamware_ros_sdk_server_node.launch

@@ -0,0 +1,8 @@
+<!--
+  Used for visualising rplidar in action.  
+  
+  It requires rplidar.launch.
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slamware_ros_sdk)/rviz/slamware_ros_sdk_server_node.rviz" />
+</launch>

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/ActionDirection.msg

@@ -0,0 +1,8 @@
+
+int8 UNKNOWN=-1
+int8 FORWARD=0
+int8 BACKWARD=1
+int8 TURNRIGHT=2
+int8 TURNLEFT=3
+
+int8 direction

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/AddLineRequest.msg

@@ -0,0 +1,3 @@
+
+ArtifactUsage usage
+Line2DFlt32 line

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/AddLinesRequest.msg

@@ -0,0 +1,3 @@
+
+ArtifactUsage usage
+Line2DFlt32[] lines

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/ArtifactUsage.msg

@@ -0,0 +1,6 @@
+
+int8 UNKNOWN=-1
+int8 VIRTUAL_WALL=0
+int8 VIRTUAL_TRACK=1
+
+int8 usage

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/BasicSensorInfo.msg

@@ -0,0 +1,6 @@
+
+int32 id
+SensorType sensor_type
+ImpactType impact_type
+geometry_msgs/Pose install_pose
+float32 refresh_freq

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/BasicSensorInfoArray.msg

@@ -0,0 +1,2 @@
+
+BasicSensorInfo[] sensors_info

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/BasicSensorValue.msg

@@ -0,0 +1,3 @@
+
+bool is_in_impact			# whether this sensor is in impact status
+float32 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/BasicSensorValueData.msg

@@ -0,0 +1,3 @@
+
+BasicSensorInfo info
+BasicSensorValue value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/BasicSensorValueDataArray.msg

@@ -0,0 +1,2 @@
+
+BasicSensorValueData[] values_data

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/CancelActionRequest.msg

@@ -0,0 +1,2 @@
+
+# currently nothing in this message

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/ClearLinesRequest.msg

@@ -0,0 +1,2 @@
+
+ArtifactUsage usage

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/ClearMapRequest.msg

@@ -0,0 +1,2 @@
+
+MapKind kind

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/GoHomeRequest.msg

@@ -0,0 +1,2 @@
+
+# currently nothing in this message

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/ImpactType.msg

@@ -0,0 +1,6 @@
+
+int8 UNKNOWN=-1
+int8 DIGITAL=0
+int8 ANALOG=1
+
+int8 type

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/Line2DFlt32.msg

@@ -0,0 +1,4 @@
+
+uint32 id
+Vec2DFlt32 start
+Vec2DFlt32 end

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/Line2DFlt32Array.msg

@@ -0,0 +1,2 @@
+
+Line2DFlt32[] lines

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/LocalizationMovement.msg

@@ -0,0 +1,7 @@
+
+int8 UNKNOWN=-1
+int8 NO_MOVE=0
+int8 ROTATE_ONLY=1
+int8 ANY=2
+
+int8 type

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/LocalizationOptions.msg

@@ -0,0 +1,3 @@
+
+OptionalInt32 max_time_ms
+OptionalLocalizationMovement mvmt_type

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MapKind.msg

@@ -0,0 +1,9 @@
+
+int8 UNKNOWN=-1
+int8 EXPLORERMAP=0
+int8 SWEEPERMAP=1
+int8 UWBMAP=2
+int8 SLAMMAP=3
+int8 LOCALSLAMMAP=4
+
+int8 kind

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveByDirectionRequest.msg

@@ -0,0 +1,3 @@
+
+ActionDirection direction
+MoveOptions options

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveByThetaRequest.msg

@@ -0,0 +1,3 @@
+
+float32 theta
+MoveOptions options

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveLineRequest.msg

@@ -0,0 +1,3 @@
+
+ArtifactUsage usage
+Line2DFlt32 line

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveLinesRequest.msg

@@ -0,0 +1,3 @@
+
+ArtifactUsage usage
+Line2DFlt32[] lines

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveOptionFlag.msg

@@ -0,0 +1,12 @@
+
+uint32 NONE=0
+uint32 APPENDING=1
+uint32 MILESTONE=2
+uint32 NO_SMOOTH=4
+uint32 KEY_POINTS=8
+uint32 PRECISE=16
+uint32 WITH_YAW=32
+uint32 RETURN_UNREACHABLE_DIRECTLY=64
+uint32 KEY_POINTS_WITH_OA=128
+
+uint32 flags

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveOptions.msg

@@ -0,0 +1,3 @@
+
+MoveOptionFlag opt_flags
+OptionalFlt64 speed_ratio

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveToLocationsRequest.msg

@@ -0,0 +1,4 @@
+
+geometry_msgs/Point[] locations
+MoveOptions options
+float32 yaw

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/MoveToRequest.msg

@@ -0,0 +1,4 @@
+
+geometry_msgs/Point location
+MoveOptions options
+float32 yaw

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalBool.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+bool value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalFlt32.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+float32 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalFlt64.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+float64 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalInt16.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+int16 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalInt32.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+int32 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalInt64.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+int64 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalInt8.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+int8 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalLocalizationMovement.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+LocalizationMovement value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalUInt16.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+uint16 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalUInt32.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+uint32 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalUInt64.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+uint64 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/OptionalUInt8.msg

@@ -0,0 +1,3 @@
+
+bool is_valid
+uint8 value

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RecoverLocalizationRequest.msg

@@ -0,0 +1,3 @@
+
+RectFlt32 area
+LocalizationOptions options

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RectFlt32.msg

@@ -0,0 +1,5 @@
+
+float32 x		# x of position
+float32 y		# y of position
+float32 w		# width
+float32 h		# height

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RectInt32.msg

@@ -0,0 +1,5 @@
+
+int32 x		# x of position
+int32 y		# y of position
+int32 w		# width
+int32 h		# height

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RemoveLineRequest.msg

@@ -0,0 +1,3 @@
+
+ArtifactUsage usage
+uint32 id

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RobotBasicState.msg

@@ -0,0 +1,11 @@
+
+bool is_map_building_enabled
+bool is_localization_enabled
+
+int32 localization_quality
+
+int32 board_temperature
+
+int32 battery_percentage
+bool is_dc_in
+bool is_charging

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RobotDeviceInfo.msg

@@ -0,0 +1,11 @@
+
+string device_id
+int32 model_id
+string model_name
+int32 manufacturer_id
+string manufacturer_name
+string hardware_version
+string software_version
+
+string sdp_version
+string sdk_version

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RotateRequest.msg

@@ -0,0 +1,3 @@
+
+geometry_msgs/Quaternion rotation
+MoveOptions options

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/RotateToRequest.msg

@@ -0,0 +1,3 @@
+
+geometry_msgs/Quaternion orientation
+MoveOptions options

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/SensorType.msg

@@ -0,0 +1,10 @@
+
+int8 UNKNOWN=-1
+int8 BUMPER=0
+int8 CLIFF=1
+int8 SONAR=2
+int8 DEPTH_CAMERA=3
+int8 WALL_SENSOR=4
+int8 MAG_TAPE_DETECTOR=5
+
+int8 type

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/SetMapLocalizationRequest.msg

@@ -0,0 +1,2 @@
+
+bool enabled

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/SetMapUpdateRequest.msg

@@ -0,0 +1,3 @@
+
+bool enabled
+MapKind kind

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/SyncMapRequest.msg

@@ -0,0 +1,2 @@
+
+# currently nothing in this message

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/Vec2DFlt32.msg

@@ -0,0 +1,3 @@
+
+float32 x
+float32 y

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/msg/Vec2DInt32.msg

@@ -0,0 +1,3 @@
+
+int32 x
+int32 y

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>slamware_ros_sdk</name>
+  <version>1.0.0</version>
+  <description>The slamware_ros_sdk package</description>
+
+  <maintainer email="ros@slamtec.com">Slamtec Ros Maintainer</maintainer>
+  <license>BSD</license>
+  <url type="website">http://developer.slamtec.com/docs/slamware/ros-sdk/2.6.0_rtm/</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+</package>

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/rviz/slamware_ros_sdk_server_node.rviz

@@ -0,0 +1,278 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1
+        - /Odometry1
+        - /Path1
+        - /Axes1
+      Splitter Ratio: 0.5
+    Tree Height: 673
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.019999999552965164
+      Style: Flat Squares
+      Topic: /slamware_ros_sdk_server_node/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 10000
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /slamware_ros_sdk_server_node/odom
+      Unreliable: false
+      Value: false
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /slamware_ros_sdk_server_node/map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.009999999776482582
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /slamware_ros_sdk_server_node/global_plan_path
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 0.20000000298023224
+      Name: Axes
+      Radius: 0.05000000074505806
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        laser:
+          Value: true
+        odom:
+          Value: true
+        robot_pose:
+          Value: true
+        slamware_map:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        slamware_map:
+          odom:
+            {}
+          robot_pose:
+            base_link:
+              laser:
+                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: slamware_map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 13.346840858459473
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.5996972322463989
+        Y: 0.1655363291501999
+        Z: 0.24934253096580505
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 4.708580493927002
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 970
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000032cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000032c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065c0000003efc0100000002fb0000000800540069006d006501000000000000065c000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004ec0000032c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1628
+  X: 292
+  Y: 27

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/rviz/slamware_ros_sdk_server_node_datapub.rviz

@@ -0,0 +1,183 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Axes1
+        - /TF1
+        - /TF1/Frames1
+        - /TF1/Tree1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Angle Tolerance: 0.1
+      Class: rviz/Odometry
+      Color: 255; 25; 0
+      Enabled: true
+      Keep: 100
+      Length: 1
+      Name: Odometry
+      Position Tolerance: 0.1
+      Topic: /slamware_ros_sdk_server_node/odom
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Flat Squares
+      Topic: /slamware_ros_sdk_server_node/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 0.5
+      Name: Axes
+      Radius: 0.1
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        laser:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 0.1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        odom:
+          base_link:
+            laser:
+              {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 8.88033
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -3.07536
+        Y: 6.50537
+        Z: -0.1993
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.2198
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.275397
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b600000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1855
+  X: 65
+  Y: 24

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/rviz/slamware_ros_sdk_teb.rviz

@@ -0,0 +1,323 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1
+        - /Goal1
+        - /TebLocalPath1
+        - /TebMarker1
+      Splitter Ratio: 0.5
+    Tree Height: 776
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.100000001
+      Style: Flat Squares
+      Topic: /slamware_ros_sdk_server_node/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 10000
+      Name: RobotOdometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 25; 0
+        Head Length: 0.300000012
+        Head Radius: 0.100000001
+        Shaft Length: 1
+        Shaft Radius: 0.0500000007
+        Value: Arrow
+      Topic: /slamware_ros_sdk_server_node/odom
+      Unreliable: false
+      Value: false
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: SlamwareMap
+      Topic: /slamware_ros_sdk_server_node/map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.300000012
+      Head Radius: 0.100000001
+      Name: Goal
+      Shaft Length: 1
+      Shaft Radius: 0.0500000007
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 170; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.0500000007
+      Name: TebLocalPath
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/TebLocalPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 0.200000003
+      Name: Axes
+      Radius: 0.0500000007
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        laser:
+          Value: true
+        odom:
+          Value: true
+        slamware_map:
+          Value: true
+      Marker Scale: 5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        slamware_map:
+          odom:
+            base_link:
+              laser:
+                {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: GlobalCostMap
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: FootPrint
+      Topic: /move_base/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: LocalCostMap
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 0; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Billboards
+      Line Width: 0.0599999987
+      Name: GlobalPath
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/GlobalPlanner/plan
+      Unreliable: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /move_base/TebLocalPlannerROS/teb_markers
+      Name: TebMarker
+      Namespaces:
+        PointObstacles: true
+        RobotFootprintModel: true
+        TebContainer: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: slamware_map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 7.65116024
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 3.16399431
+        Y: 5.16340542
+        Z: 0.254350036
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.47979617
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.87358189
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075b0000003efc0100000002fb0000000800540069006d006501000000000000075b0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005eb0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1883
+  X: 37
+  Y: 24

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/client/slamware_ros_sdk_client.cpp

@@ -0,0 +1,144 @@
+
+#include <slamware_ros_sdk/slamware_ros_sdk_client.h>
+
+#include <boost/assert.hpp>
+#include <boost/filesystem/fstream.hpp>
+
+namespace slamware_ros_sdk {
+    
+    SlamwareRosSdkClient::SlamwareRosSdkClient(ros::NodeHandle& nhRos
+        , const char* serverNodeName
+        , const char* msgNamePrefix
+        )
+        : nh_(&nhRos)
+    {
+        if (nullptr != serverNodeName)
+            sdkServerNodeName_ = serverNodeName;
+        else
+            sdkServerNodeName_ = "slamware_ros_sdk_server_node";
+
+        if (nullptr != msgNamePrefix)
+        {
+            msgNamePrefix_ = msgNamePrefix;
+        }
+        else if (!sdkServerNodeName_.empty())
+        {
+            if ('/' != sdkServerNodeName_.front())
+                msgNamePrefix_ = "/";
+            msgNamePrefix_ += sdkServerNodeName_;
+            if ('/' != msgNamePrefix_.back())
+                msgNamePrefix_ += "/";
+        }
+
+        // initialize publishers
+        {
+            pubSyncMap_ = nh_->advertise<SyncMapRequest>(genTopicFullName_("sync_map"), 1);
+            pubSetPose_ = nh_->advertise<geometry_msgs::Pose>(genTopicFullName_("set_pose"), 1);
+
+            pubRecoverLocalization_ = nh_->advertise<RecoverLocalizationRequest>(genTopicFullName_("recover_localization"), 1);
+            pubClearMap_ = nh_->advertise<ClearMapRequest>(genTopicFullName_("clear_map"), 1);
+            pubSetMapUpdate_ = nh_->advertise<SetMapUpdateRequest>(genTopicFullName_("set_map_update"), 1);
+            pubSetMapLocalization_ = nh_->advertise<SetMapLocalizationRequest>(genTopicFullName_("set_map_localization"), 1);
+
+            pubMoveByDirection_ = nh_->advertise<MoveByDirectionRequest>(genTopicFullName_("move_by_direction"), 1);
+            pubMoveByTheta_ = nh_->advertise<MoveByThetaRequest>(genTopicFullName_("move_by_theta"), 1);
+            pubMoveTo_ = nh_->advertise<MoveToRequest>(genTopicFullName_("move_to"), 1);
+            pubMoveToLocations_ = nh_->advertise<MoveToLocationsRequest>(genTopicFullName_("move_to_locations"), 1);
+            pubRotateTo_ = nh_->advertise<RotateToRequest>(genTopicFullName_("rotate_to"), 1);
+            pubRotate_ = nh_->advertise<RotateRequest>(genTopicFullName_("rotate"), 1);
+
+            pubGoHome_ = nh_->advertise<GoHomeRequest>(genTopicFullName_("go_home"), 1);
+            pubCancelAction_ = nh_->advertise<CancelActionRequest>(genTopicFullName_("cancel_action"), 1);
+
+            pubAddLine_ = nh_->advertise<AddLineRequest>(genTopicFullName_("add_line"), 1);
+            pubAddLines_ = nh_->advertise<AddLinesRequest>(genTopicFullName_("add_lines"), 1);
+            pubRemoveLine_ = nh_->advertise<RemoveLineRequest>(genTopicFullName_("remove_line"), 1);
+            pubClearLines_ = nh_->advertise<ClearLinesRequest>(genTopicFullName_("clear_lines"), 1);
+            pubMoveLine_ = nh_->advertise<MoveLineRequest>(genTopicFullName_("move_line"), 1);
+            pubMoveLines_ = nh_->advertise<MoveLinesRequest>(genTopicFullName_("move_lines"), 1);
+        }
+
+        // initialize service clients
+        {
+            scSyncGetStcm_ = nh_->serviceClient<SyncGetStcm>(genTopicFullName_("sync_get_stcm"));
+            scSyncSetStcm_ = nh_->serviceClient<SyncSetStcm>(genTopicFullName_("sync_set_stcm"));
+        }
+    }
+
+    SlamwareRosSdkClient::~SlamwareRosSdkClient()
+    {
+        //
+    }
+
+    bool SlamwareRosSdkClient::syncGetStcm(std::string& errMsg
+        , const fs_path_t& filePath
+        )
+    {
+        errMsg.clear();
+
+        SyncGetStcm srvMsg;
+        if (!syncGetStcm(srvMsg))
+        {
+            errMsg = "failed to call syncGetStcm().";
+            return false;
+        }
+
+        {
+            boost::filesystem::ofstream ofs(filePath, (std::ios_base::out | std::ios_base::trunc | std::ios_base::binary));
+            if (!ofs.is_open())
+            {
+                errMsg = "failed to open file.";
+                return false;
+            }
+            ofs.write((const char*)srvMsg.response.raw_stcm.data(), srvMsg.response.raw_stcm.size());
+            if (ofs.fail())
+            {
+                errMsg = "failed to write file";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    bool SlamwareRosSdkClient::syncSetStcm(const fs_path_t& filePath
+        , const geometry_msgs::Pose& robotPose
+        , std::string& errMsg
+        )
+    {
+        errMsg.clear();
+
+        SyncSetStcm srvMsg;
+        srvMsg.request.robot_pose = robotPose;
+        {
+            boost::filesystem::ifstream ifs(filePath, (std::ios_base::in | std::ios_base::binary | std::ios_base::ate));
+            if (!ifs.is_open())
+            {
+                errMsg = "failed to open file";
+                return false;
+            }
+            const auto szDat = ifs.tellg();
+            if (boost::filesystem::ifstream::pos_type(-1) == szDat)
+            {
+                errMsg = "failed to get file size.";
+                return false;
+            }
+            ifs.seekg(0);
+
+            srvMsg.request.raw_stcm.resize(szDat);
+            ifs.read((char*)srvMsg.request.raw_stcm.data(), szDat);
+            if (ifs.gcount() != szDat)
+            {
+                errMsg = "failed to read file data.";
+                return false;
+            }
+        }
+
+        if (!syncSetStcm(srvMsg))
+        {
+            errMsg = "failed to call syncSetStcm().";
+            return false;
+        }
+        return true;
+    }
+
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/go1_slamware_bridge_node.cpp

@@ -0,0 +1,627 @@
+/**
+ * GO1 Slamware Bridge Node
+ * 订阅slamware雷达信息并转发到PC端
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <thread>
+#include <mutex>
+#include <json/json.h>
+
+class GO1SlamwareBridge
+{
+private:
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+    
+    // ROS订阅器 - 只使用PoseStamped类型
+    ros::Subscriber scan_sub_;
+    ros::Subscriber map_sub_;
+    ros::Subscriber odom_sub_;
+    ros::Subscriber pose_stamped_sub_;  // 只订阅PoseStamped
+    
+    // 网络相关
+    int data_server_socket_;
+    std::vector<int> client_sockets_;
+    std::mutex clients_mutex_;
+    
+    // 参数
+    int data_port_;
+    int control_port_;
+    double scan_rate_;
+    double map_rate_;
+    double odom_rate_;
+    double pose_rate_;
+    
+    // 数据缓存
+    sensor_msgs::LaserScan::ConstPtr latest_scan_;
+    nav_msgs::OccupancyGrid::ConstPtr latest_map_;
+    nav_msgs::Odometry::ConstPtr latest_odom_;
+    geometry_msgs::PoseStamped::ConstPtr latest_pose_;
+    
+    std::mutex data_mutex_;
+    bool running_;
+    
+    ros::Timer status_timer_;
+    int scan_received_count_;
+    int map_received_count_;
+    int odom_received_count_;
+    int pose_received_count_;
+    
+public:
+    GO1SlamwareBridge() : private_nh_("~"), running_(false), data_server_socket_(-1),
+                          scan_received_count_(0), map_received_count_(0), 
+                          odom_received_count_(0), pose_received_count_(0)
+    {
+        // 获取网络参数
+        private_nh_.param<int>("data_port", data_port_, 8888);
+        private_nh_.param<int>("control_port", control_port_, 8889);
+        
+        // 根据实际测试结果设置频率，与思岚雷达实际发布频率匹配
+        // 测试结果：scan=10Hz, robot_pose=20Hz, odom=20Hz, map=5Hz
+        scan_rate_ = 10.0;   // 实测 10.0 Hz
+        pose_rate_ = 20.0;   // 实测 20.0 Hz  
+        odom_rate_ = 20.0;   // 实测 20.0 Hz (不是10Hz)
+        map_rate_ = 5.0;     // 实测 5.0 Hz (不是1Hz)
+        
+        // 允许通过参数覆盖（如果需要特殊设置）
+        private_nh_.param<double>("scan_rate", scan_rate_, scan_rate_);
+        private_nh_.param<double>("map_rate", map_rate_, map_rate_);
+        private_nh_.param<double>("odom_rate", odom_rate_, odom_rate_);
+        private_nh_.param<double>("pose_rate", pose_rate_, pose_rate_);
+        
+        ROS_INFO("GO1 Slamware Bridge initializing...");
+        ROS_INFO("Bridge rates (matched to Slamware actual frequencies):");
+        ROS_INFO("  Scan: %.1fHz, Map: %.1fHz, Odom: %.1fHz, Pose: %.1fHz", 
+                 scan_rate_, map_rate_, odom_rate_, pose_rate_);
+        ROS_INFO("Data port: %d, Control port: %d", data_port_, control_port_);
+        
+        // 订阅思岚雷达的话题
+        scan_sub_ = nh_.subscribe("/slamware_ros_sdk_server_node/scan", 10, 
+                                 &GO1SlamwareBridge::scanCallback, this);
+        map_sub_ = nh_.subscribe("/slamware_ros_sdk_server_node/map", 1, 
+                                &GO1SlamwareBridge::mapCallback, this);
+        odom_sub_ = nh_.subscribe("/slamware_ros_sdk_server_node/odom", 10, 
+                                 &GO1SlamwareBridge::odomCallback, this);
+        
+        // 直接订阅PoseStamped类型的robot_pose话题
+        pose_stamped_sub_ = nh_.subscribe("/slamware_ros_sdk_server_node/robot_pose", 10, 
+                                         &GO1SlamwareBridge::poseStampedCallback, this);
+        
+        ROS_INFO("Subscribed to topics:");
+        ROS_INFO("  - /slamware_ros_sdk_server_node/scan (10Hz)");
+        ROS_INFO("  - /slamware_ros_sdk_server_node/map (5Hz)");
+        ROS_INFO("  - /slamware_ros_sdk_server_node/odom (20Hz)");
+        ROS_INFO("  - /slamware_ros_sdk_server_node/robot_pose (20Hz)");
+        
+        // 添加状态监控定时器
+        status_timer_ = nh_.createTimer(ros::Duration(10.0), &GO1SlamwareBridge::statusCallback, this);
+    }
+    
+    ~GO1SlamwareBridge()
+    {
+        stop();
+    }
+    
+    bool start(std::string& errMsg)
+    {
+        // 初始化网络
+        if (!initializeNetwork())
+        {
+            errMsg = "Failed to initialize network";
+            return false;
+        }
+        
+        running_ = true;
+        
+        // 启动网络处理线程
+        std::thread(&GO1SlamwareBridge::connectionHandlerThread, this).detach();
+        std::thread(&GO1SlamwareBridge::dataSendingThread, this).detach();
+        
+        ROS_INFO("GO1 Slamware Bridge started successfully");
+        ROS_INFO("Waiting for slamware_ros_sdk_server_node to publish data...");
+        return true;
+    }
+    
+    void stop()
+    {
+        running_ = false;
+        closeNetwork();
+    }
+    
+    void waitUntilStopped()
+    {
+        while (running_ && ros::ok())
+        {
+            ros::Duration(0.1).sleep();
+        }
+    }
+    
+    // 增强状态回调函数 - 重点监控位置数据更新
+    void statusCallback(const ros::TimerEvent&)
+    {
+        std::lock_guard<std::mutex> lock(clients_mutex_);
+        ROS_INFO("=== GO1 BRIDGE STATUS ===");
+        ROS_INFO("Connected clients: %zu", client_sockets_.size());
+        
+        // 检查思岚服务器连接状态
+        bool slamware_connected = (scan_received_count_ > 0 || map_received_count_ > 0 || 
+                                  odom_received_count_ > 0 || pose_received_count_ > 0);
+        
+        if (!slamware_connected) {
+            ROS_WARN("⚠️  思岚雷达服务器未连接或无数据!");
+            ROS_WARN("请检查:");
+            ROS_WARN("  1. 思岚雷达设备是否开机");
+            ROS_WARN("  2. 网络连接是否正常 (IP: 192.168.11.1)");
+            ROS_WARN("  3. slamware_ros_sdk_server_node是否正常运行");
+        } else {
+            ROS_INFO("✓ 思岚雷达服务器已连接");
+        }
+        
+        ROS_INFO("Bridge forward rates (matched to Slamware):");
+        ROS_INFO("  Scan: %.1fHz, Map: %.1fHz, Odom: %.1fHz, Pose: %.1fHz", 
+                 scan_rate_, map_rate_, odom_rate_, pose_rate_);
+        
+        ROS_INFO("Messages received - Scan: %d, Map: %d, Odom: %d, Pose: %d", 
+                 scan_received_count_, map_received_count_, odom_received_count_, pose_received_count_);
+        ROS_INFO("Latest data available:");
+        ROS_INFO("  Scan: %s", latest_scan_ ? "YES" : "NO");
+        ROS_INFO("  Map: %s", latest_map_ ? "YES" : "NO"); 
+        ROS_INFO("  Odom: %s", latest_odom_ ? "YES" : "NO");
+        ROS_INFO("  Pose: %s", latest_pose_ ? "YES" : "NO");
+        
+        // 重点检查位置数据更新情况
+        static int last_pose_received_count = 0;
+        static ros::Time last_pose_time = ros::Time::now();
+        
+        if (latest_pose_) {
+            ros::Time current_time = ros::Time::now();
+            double time_since_last_pose = (current_time - last_pose_time).toSec();
+            
+            ROS_INFO("  Current Pose: (%.2f, %.2f, %.2f) @ %.1fHz", 
+                    latest_pose_->pose.position.x, 
+                    latest_pose_->pose.position.y, 
+                    latest_pose_->pose.position.z,
+                    pose_rate_);
+            
+            // 检查位置数据是否在更新
+            if (pose_received_count_ == last_pose_received_count) {
+                ROS_WARN("⚠️  位置数据未更新! 上次更新: %.1f秒前", time_since_last_pose);
+            } else {
+                ROS_INFO("✓ 位置数据正常更新");
+                last_pose_time = current_time;
+            }
+            last_pose_received_count = pose_received_count_;
+        } else {
+            ROS_WARN("⚠️  未收到任何位置数据!");
+        }
+        
+        // 计算数据接收率（每10秒统计一次）
+        static int last_scan_count = 0;
+        static int last_pose_count = 0;
+        static int last_odom_count = 0;
+        static int last_map_count = 0;
+        
+        int scan_rate_actual = (scan_received_count_ - last_scan_count) / 10;
+        int pose_rate_actual = (pose_received_count_ - last_pose_count) / 10;
+        int odom_rate_actual = (odom_received_count_ - last_odom_count) / 10;
+        int map_rate_actual = (map_received_count_ - last_map_count) / 10;
+        
+        ROS_INFO("Actual receive rates: Scan=%dHz, Pose=%dHz, Odom=%dHz, Map=%dHz",
+                 scan_rate_actual, pose_rate_actual, odom_rate_actual, map_rate_actual);
+        
+        // 检查位置数据接收频率
+        if (pose_rate_actual < 5) {
+            ROS_WARN("⚠️  位置数据接收频率过低! 期望: 20Hz, 实际: %dHz", pose_rate_actual);
+        }
+        
+        last_scan_count = scan_received_count_;
+        last_pose_count = pose_received_count_;
+        last_odom_count = odom_received_count_;
+        last_map_count = map_received_count_;
+        
+        ROS_INFO("========================");
+    }
+
+private:
+    bool initializeNetwork()
+    {
+        // 创建数据服务器socket
+        data_server_socket_ = socket(AF_INET, SOCK_STREAM, 0);
+        if (data_server_socket_ < 0) {
+            ROS_ERROR("Failed to create data server socket");
+            return false;
+        }
+        
+        int opt = 1;
+        setsockopt(data_server_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+        
+        struct sockaddr_in server_addr;
+        server_addr.sin_family = AF_INET;
+        server_addr.sin_addr.s_addr = INADDR_ANY;
+        server_addr.sin_port = htons(data_port_);
+        
+        if (bind(data_server_socket_, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
+            ROS_ERROR("Failed to bind data server socket to port %d", data_port_);
+            return false;
+        }
+        
+        if (listen(data_server_socket_, 5) < 0) {
+            ROS_ERROR("Failed to listen on data server socket");
+            return false;
+        }
+        
+        ROS_INFO("Data server listening on port %d", data_port_);
+        return true;
+    }
+    
+    void connectionHandlerThread()
+    {
+        while (running_ && ros::ok()) {
+            struct sockaddr_in client_addr;
+            socklen_t client_len = sizeof(client_addr);
+            
+            // 设置accept超时，避免阻塞
+            fd_set readfds;
+            FD_ZERO(&readfds);
+            FD_SET(data_server_socket_, &readfds);
+            
+            struct timeval timeout;
+            timeout.tv_sec = 1;
+            timeout.tv_usec = 0;
+            
+            int activity = select(data_server_socket_ + 1, &readfds, NULL, NULL, &timeout);
+            
+            if (activity > 0 && FD_ISSET(data_server_socket_, &readfds)) {
+                int client_socket = accept(data_server_socket_, (struct sockaddr*)&client_addr, &client_len);
+                if (client_socket >= 0) {
+                    std::lock_guard<std::mutex> lock(clients_mutex_);
+                    client_sockets_.push_back(client_socket);
+                    
+                    char client_ip[INET_ADDRSTRLEN];
+                    inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, INET_ADDRSTRLEN);
+                    ROS_INFO("PC client connected from %s", client_ip);
+                }
+            }
+            
+            // 清理断开的连接
+            cleanupDisconnectedClients();
+        }
+    }
+    
+    void cleanupDisconnectedClients()
+    {
+        std::lock_guard<std::mutex> lock(clients_mutex_);
+        auto it = client_sockets_.begin();
+        while (it != client_sockets_.end()) {
+            char buffer[1];
+            int result = recv(*it, buffer, 1, MSG_PEEK | MSG_DONTWAIT);
+            if (result == 0 || (result < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) {
+                close(*it);
+                it = client_sockets_.erase(it);
+                ROS_INFO("Client disconnected");
+            } else {
+                ++it;
+            }
+        }
+    }
+    
+    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
+    {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        latest_scan_ = msg;
+        scan_received_count_++;
+        
+        if (scan_received_count_ <= 5 || scan_received_count_ % 50 == 0) {
+            ROS_INFO("✓ Scan #%d: %zu points, frame: %s, range: [%.2f-%.2f]m", 
+                     scan_received_count_, msg->ranges.size(), msg->header.frame_id.c_str(),
+                     msg->range_min, msg->range_max);
+        }
+    }
+    
+    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+    {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        latest_map_ = msg;
+        map_received_count_++;
+        
+        ROS_INFO("✓ Map #%d: %dx%d (res: %.3fm), frame: %s", 
+                 map_received_count_, msg->info.width, msg->info.height, 
+                 msg->info.resolution, msg->header.frame_id.c_str());
+    }
+    
+    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+    {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        latest_odom_ = msg;
+        odom_received_count_++;
+        
+        if (odom_received_count_ <= 5 || odom_received_count_ % 100 == 0) {
+            ROS_INFO("✓ Odom #%d: pos(%.2f, %.2f, %.2f), frame: %s->%s", 
+                     odom_received_count_,
+                     msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z,
+                     msg->header.frame_id.c_str(), msg->child_frame_id.c_str());
+        }
+    }
+    
+    void poseStampedCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+    {
+        std::lock_guard<std::mutex> lock(data_mutex_);
+        latest_pose_ = msg;
+        pose_received_count_++;
+        
+        // 更详细的位置数据日志
+        if (pose_received_count_ <= 10 || pose_received_count_ % 100 == 0) {
+            ROS_INFO("✓ Pose #%d: pos(%.3f, %.3f, %.3f), frame: %s, timestamp: %.3f", 
+                     pose_received_count_,
+                     msg->pose.position.x, msg->pose.position.y, msg->pose.position.z,
+                     msg->header.frame_id.c_str(), msg->header.stamp.toSec());
+        }
+    }
+    
+    void dataSendingThread()
+    {
+        // 使用20Hz作为主循环频率（最高的数据频率）
+        double max_rate = 20.0;  // odom和pose都是20Hz
+        ros::Rate loop_rate(max_rate);
+        
+        ROS_INFO("Data sending thread started with main loop rate: %.1fHz", max_rate);
+        ROS_INFO("Frequency matched to Slamware actual rates:");
+        ROS_INFO("  Scan data: %.1fHz (every %.1fms)", scan_rate_, 1000.0 / scan_rate_);
+        ROS_INFO("  Map data: %.1fHz (every %.1fms)", map_rate_, 1000.0 / map_rate_);
+        ROS_INFO("  Odom data: %.1fHz (every %.1fms)", odom_rate_, 1000.0 / odom_rate_);
+        ROS_INFO("  Pose data: %.1fHz (every %.1fms)", pose_rate_, 1000.0 / pose_rate_);
+        
+        int scan_counter = 0;
+        int map_counter = 0;
+        int odom_counter = 0;
+        int pose_counter = 0;
+        
+        // 计算发送间隔（基于20Hz主循环）
+        int scan_interval = static_cast<int>(max_rate / scan_rate_);  // 20/10 = 2
+        int map_interval = static_cast<int>(max_rate / map_rate_);    // 20/5 = 4
+        int odom_interval = static_cast<int>(max_rate / odom_rate_);  // 20/20 = 1
+        int pose_interval = static_cast<int>(max_rate / pose_rate_);  // 20/20 = 1
+        
+        ROS_INFO("Send intervals - Scan: every %d loops, Map: every %d loops, Odom: every %d loops, Pose: every %d loops",
+                 scan_interval, map_interval, odom_interval, pose_interval);
+        
+        while (running_ && ros::ok()) {
+            // 发送激光扫描数据 (10Hz = 每2个循环发送一次)
+            if (scan_counter % scan_interval == 0) {
+                sendScanData();
+            }
+            
+            // 发送地图数据 (5Hz = 每4个循环发送一次)
+            if (map_counter % map_interval == 0) {
+                sendMapData();
+            }
+            
+            // 发送里程计数据 (20Hz = 每个循环都发送)
+            if (odom_counter % odom_interval == 0) {
+                sendOdomData();
+            }
+            
+            // 发送位置数据 (20Hz = 每个循环都发送)
+            if (pose_counter % pose_interval == 0) {
+                sendPoseData();
+            }
+            
+            scan_counter++;
+            map_counter++;
+            odom_counter++;
+            pose_counter++;
+            
+            loop_rate.sleep();
+        }
+    }
+    
+    void sendScanData()
+    {
+        std::lock_guard<std::mutex> data_lock(data_mutex_);
+        if (!latest_scan_) return;
+        
+        Json::Value json_data;
+        json_data["type"] = "scan";
+        json_data["timestamp"] = latest_scan_->header.stamp.toSec();
+        json_data["frame_id"] = latest_scan_->header.frame_id;
+        json_data["angle_min"] = latest_scan_->angle_min;
+        json_data["angle_max"] = latest_scan_->angle_max;
+        json_data["angle_increment"] = latest_scan_->angle_increment;
+        json_data["time_increment"] = latest_scan_->time_increment;
+        json_data["scan_time"] = latest_scan_->scan_time;
+        json_data["range_min"] = latest_scan_->range_min;
+        json_data["range_max"] = latest_scan_->range_max;
+        
+        Json::Value ranges(Json::arrayValue);
+        for (const auto& range : latest_scan_->ranges) {
+            ranges.append(range);
+        }
+        json_data["ranges"] = ranges;
+        
+        Json::Value intensities(Json::arrayValue);
+        for (const auto& intensity : latest_scan_->intensities) {
+            intensities.append(intensity);
+        }
+        json_data["intensities"] = intensities;
+        
+        broadcastData(json_data);
+    }
+    
+    void sendMapData()
+    {
+        std::lock_guard<std::mutex> data_lock(data_mutex_);
+        if (!latest_map_) return;
+        
+        Json::Value json_data;
+        json_data["type"] = "map";
+        json_data["timestamp"] = latest_map_->header.stamp.toSec();
+        json_data["frame_id"] = latest_map_->header.frame_id;
+        json_data["width"] = latest_map_->info.width;
+        json_data["height"] = latest_map_->info.height;
+        json_data["resolution"] = latest_map_->info.resolution;
+        
+        Json::Value origin;
+        origin["position"]["x"] = latest_map_->info.origin.position.x;
+        origin["position"]["y"] = latest_map_->info.origin.position.y;
+        origin["position"]["z"] = latest_map_->info.origin.position.z;
+        origin["orientation"]["x"] = latest_map_->info.origin.orientation.x;
+        origin["orientation"]["y"] = latest_map_->info.origin.orientation.y;
+        origin["orientation"]["z"] = latest_map_->info.origin.orientation.z;
+        origin["orientation"]["w"] = latest_map_->info.origin.orientation.w;
+        json_data["origin"] = origin;
+        
+        Json::Value data_array(Json::arrayValue);
+        for (const auto& cell : latest_map_->data) {
+            data_array.append(cell);
+        }
+        json_data["data"] = data_array;
+        
+        broadcastData(json_data);
+    }
+    
+    void sendOdomData()
+    {
+        std::lock_guard<std::mutex> data_lock(data_mutex_);
+        if (!latest_odom_) return;
+        
+        Json::Value json_data;
+        json_data["type"] = "odom";
+        json_data["timestamp"] = latest_odom_->header.stamp.toSec();
+        json_data["frame_id"] = latest_odom_->header.frame_id;
+        json_data["child_frame_id"] = latest_odom_->child_frame_id;
+        
+        Json::Value pose;
+        pose["position"]["x"] = latest_odom_->pose.pose.position.x;
+        pose["position"]["y"] = latest_odom_->pose.pose.position.y;
+        pose["position"]["z"] = latest_odom_->pose.pose.position.z;
+        pose["orientation"]["x"] = latest_odom_->pose.pose.orientation.x;
+        pose["orientation"]["y"] = latest_odom_->pose.pose.orientation.y;
+        pose["orientation"]["z"] = latest_odom_->pose.pose.orientation.z;
+        pose["orientation"]["w"] = latest_odom_->pose.pose.orientation.w;
+        json_data["pose"] = pose;
+        
+        Json::Value twist;
+        twist["linear"]["x"] = latest_odom_->twist.twist.linear.x;
+        twist["linear"]["y"] = latest_odom_->twist.twist.linear.y;
+        twist["linear"]["z"] = latest_odom_->twist.twist.linear.z;
+        twist["angular"]["x"] = latest_odom_->twist.twist.angular.x;
+        twist["angular"]["y"] = latest_odom_->twist.twist.angular.y;
+        twist["angular"]["z"] = latest_odom_->twist.twist.angular.z;
+        json_data["twist"] = twist;
+        
+        broadcastData(json_data);
+    }
+    
+    void sendPoseData()
+    {
+        std::lock_guard<std::mutex> data_lock(data_mutex_);
+        if (!latest_pose_) return;
+        
+        Json::Value json_data;
+        json_data["type"] = "pose";
+        json_data["timestamp"] = latest_pose_->header.stamp.toSec();
+        json_data["frame_id"] = latest_pose_->header.frame_id;
+        json_data["send_rate"] = pose_rate_;  // 20Hz
+        
+        Json::Value pose;
+        pose["position"]["x"] = latest_pose_->pose.position.x;
+        pose["position"]["y"] = latest_pose_->pose.position.y;
+        pose["position"]["z"] = latest_pose_->pose.position.z;
+        pose["orientation"]["x"] = latest_pose_->pose.orientation.x;
+        pose["orientation"]["y"] = latest_pose_->pose.orientation.y;
+        pose["orientation"]["z"] = latest_pose_->pose.orientation.z;
+        pose["orientation"]["w"] = latest_pose_->pose.orientation.w;
+        json_data["pose"] = pose;
+        
+        broadcastData(json_data);
+        
+        // 高频位置数据日志（限制频率避免日志过多）
+        ROS_INFO_THROTTLE(2, "📤 Sending pose data @ %.1fHz: (%.2f, %.2f, %.2f)", 
+                 pose_rate_,
+                 latest_pose_->pose.position.x, 
+                 latest_pose_->pose.position.y, 
+                 latest_pose_->pose.position.z);
+    }
+    
+    void broadcastData(const Json::Value& data)
+    {
+        std::lock_guard<std::mutex> lock(clients_mutex_);
+        if (client_sockets_.empty()) return;
+        
+        Json::StreamWriterBuilder builder;
+        builder["indentation"] = "";  // 紧凑格式，减少网络传输
+        std::string message = Json::writeString(builder, data) + "\n";
+        
+        // 根据数据类型调整日志频率
+        if (data["type"].asString() == "scan") {
+            ROS_INFO_THROTTLE(5, "📤 Sending scan data @ 10Hz (%zu bytes) to %zu clients", 
+                              message.length(), client_sockets_.size());
+        } else if (data["type"].asString() == "pose") {
+            ROS_INFO_THROTTLE(3, "📤 Sending pose data @ 20Hz (%zu bytes) to %zu clients", 
+                              message.length(), client_sockets_.size());
+        } else if (data["type"].asString() == "odom") {
+            ROS_INFO_THROTTLE(3, "📤 Sending odom data @ 20Hz (%zu bytes) to %zu clients", 
+                              message.length(), client_sockets_.size());
+        } else if (data["type"].asString() == "map") {
+            ROS_INFO_THROTTLE(10, "📤 Sending map data @ 5Hz (%zu bytes) to %zu clients", 
+                              message.length(), client_sockets_.size());
+        }
+        
+        auto it = client_sockets_.begin();
+        while (it != client_sockets_.end()) {
+            int result = send(*it, message.c_str(), message.length(), MSG_NOSIGNAL);
+            if (result < 0) {
+                close(*it);
+                it = client_sockets_.erase(it);
+                ROS_WARN("Client disconnected during data send");
+            } else {
+                ++it;
+            }
+        }
+    }
+    
+    void closeNetwork()
+    {
+        std::lock_guard<std::mutex> lock(clients_mutex_);
+        for (int socket : client_sockets_) {
+            close(socket);
+        }
+        client_sockets_.clear();
+        
+        if (data_server_socket_ >= 0) {
+            close(data_server_socket_);
+            data_server_socket_ = -1;
+        }
+    }
+};
+
+int main(int argc, char** argv)
+{
+    std::string errMsg;
+    ros::init(argc, argv, "go1_slamware_bridge_node");
+    
+    GO1SlamwareBridge bridge;
+    
+    if (!bridge.start(errMsg))
+    {
+        ROS_ERROR("Failed to start GO1 Slamware Bridge: %s", errMsg.c_str());
+        return -1;
+    }
+    
+    ros::spin();
+    
+    bridge.stop();
+    
+    return 0;
+} 

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/msg_convert.cpp

@@ -0,0 +1,455 @@
+
+#include "msg_convert.h"
+
+#include <stdexcept>
+
+namespace slamware_ros_sdk {
+
+	//////////////////////////////////////////////////////////////////////////
+
+    void MsgConvert<MapKind, rpos::features::location_provider::MapKind>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        switch (sltcVal)
+        {
+        case rpos::features::location_provider::EXPLORERMAP:
+            rosMsg.kind = ros_msg_t::EXPLORERMAP;
+            break;
+        case rpos::features::location_provider::SWEEPERMAP:
+            rosMsg.kind = ros_msg_t::SWEEPERMAP;
+            break;
+        case rpos::features::location_provider::UWBMAP:
+            rosMsg.kind = ros_msg_t::UWBMAP;
+            break;
+        case rpos::features::location_provider::SLAMMAP:
+            rosMsg.kind = ros_msg_t::SLAMMAP;
+            break;
+        case rpos::features::location_provider::LOCALSLAMMAP:
+            rosMsg.kind = ros_msg_t::LOCALSLAMMAP;
+            break;
+        default:
+            rosMsg.kind = ros_msg_t::UNKNOWN;
+            break;
+        }
+    }
+
+    void MsgConvert<MapKind, rpos::features::location_provider::MapKind>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        switch (rosMsg.kind)
+        {
+        case ros_msg_t::EXPLORERMAP:
+            sltcVal = rpos::features::location_provider::EXPLORERMAP;
+            break;
+        case ros_msg_t::SWEEPERMAP:
+            sltcVal = rpos::features::location_provider::SWEEPERMAP;
+            break;
+        case ros_msg_t::UWBMAP:
+            sltcVal = rpos::features::location_provider::UWBMAP;
+            break;
+        case ros_msg_t::SLAMMAP:
+            sltcVal = rpos::features::location_provider::SLAMMAP;
+            break;
+        case ros_msg_t::LOCALSLAMMAP:
+            sltcVal = rpos::features::location_provider::LOCALSLAMMAP;
+            break;
+        default:
+            //sltcVal = ((rpos::features::location_provider::MapKind)-1);
+            throw std::runtime_error("no MapKind::UNKNOWN in RPOS.");
+            break;
+        }
+    }
+
+    void MsgConvert<ArtifactUsage, rpos::features::artifact_provider::ArtifactUsage>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        switch (sltcVal)
+        {
+        case rpos::features::artifact_provider::ArtifactUsageVirtualWall:
+            rosMsg.usage = ros_msg_t::VIRTUAL_WALL;
+            break;
+        case rpos::features::artifact_provider::ArtifactUsageVirtualTrack:
+            rosMsg.usage = ros_msg_t::VIRTUAL_TRACK;
+            break;
+        default:
+            rosMsg.usage = ros_msg_t::UNKNOWN;
+            break;
+        }
+    }
+
+    void MsgConvert<ArtifactUsage, rpos::features::artifact_provider::ArtifactUsage>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        switch (rosMsg.usage)
+        {
+        case ros_msg_t::VIRTUAL_WALL:
+            sltcVal = rpos::features::artifact_provider::ArtifactUsageVirtualWall;
+            break;
+        case ros_msg_t::VIRTUAL_TRACK:
+            sltcVal = rpos::features::artifact_provider::ArtifactUsageVirtualTrack;
+            break;
+        default:
+            //sltcVal = ((rpos::features::artifact_provider::ArtifactUsage)-1);
+            throw std::runtime_error("no ArtifactUsageUnknown in RPOS.");
+            break;
+        }
+    }
+
+    void MsgConvert<SensorType, rpos::core::SensorType>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+    	switch (sltcVal)
+        {
+        case rpos::core::SensorTypeBumper:
+            rosMsg.type = ros_msg_t::BUMPER;
+            break;
+        case rpos::core::SensorTypeCliff:
+            rosMsg.type = ros_msg_t::CLIFF;
+            break;
+        case rpos::core::SensorTypeSonar:
+            rosMsg.type = ros_msg_t::SONAR;
+            break;
+        case rpos::core::SensorTypeDepthCamera:
+            rosMsg.type = ros_msg_t::DEPTH_CAMERA;
+            break;
+        case rpos::core::SensorTypeWallSensor:
+            rosMsg.type = ros_msg_t::WALL_SENSOR;
+            break;
+        case rpos::core::SensorTypeMagTapeDetector:
+            rosMsg.type = ros_msg_t::MAG_TAPE_DETECTOR;
+            break;
+        default:
+            rosMsg.type = ros_msg_t::UNKNOWN;
+            break;
+        }
+    }
+
+    void MsgConvert<SensorType, rpos::core::SensorType>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+    	switch (rosMsg.type)
+    	{
+    	case ros_msg_t::BUMPER:
+    		sltcVal = rpos::core::SensorTypeBumper;
+    		break;
+    	case ros_msg_t::CLIFF:
+    		sltcVal = rpos::core::SensorTypeCliff;
+    		break;
+    	case ros_msg_t::SONAR:
+    		sltcVal = rpos::core::SensorTypeSonar;
+    		break;
+    	case ros_msg_t::DEPTH_CAMERA:
+    		sltcVal = rpos::core::SensorTypeDepthCamera;
+    		break;
+    	case ros_msg_t::WALL_SENSOR:
+    		sltcVal = rpos::core::SensorTypeWallSensor;
+    		break;
+    	case ros_msg_t::MAG_TAPE_DETECTOR:
+    		sltcVal = rpos::core::SensorTypeMagTapeDetector;
+    		break;
+    	default:
+    		sltcVal = rpos::core::SensorTypeUnknown;
+    		break;
+    	}
+    }
+
+    void MsgConvert<ImpactType, rpos::features::impact_sensor::ImpactSensorType>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+    	switch (sltcVal)
+        {
+        case rpos::features::impact_sensor::ImpactSensorTypeDigital:
+            rosMsg.type = ros_msg_t::DIGITAL;
+            break;
+        case rpos::features::impact_sensor::ImpactSensorTypeAnalog:
+            rosMsg.type = ros_msg_t::ANALOG;
+            break;
+        default:
+            rosMsg.type = ros_msg_t::UNKNOWN;
+            break;
+        }
+    }
+
+    void MsgConvert<ImpactType, rpos::features::impact_sensor::ImpactSensorType>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+    	switch (rosMsg.type)
+    	{
+    	case ros_msg_t::DIGITAL:
+    		sltcVal = rpos::features::impact_sensor::ImpactSensorTypeDigital;
+    		break;
+    	case ros_msg_t::ANALOG:
+    		sltcVal = rpos::features::impact_sensor::ImpactSensorTypeAnalog;
+    		break;
+    	default:
+    		//sltcVal = rpos::features::impact_sensor::ImpactSensorTypeUnknown;
+            throw std::runtime_error("no ImpactSensorTypeUnknown in RPOS.");
+    		break;
+    	}
+    }
+
+    void MsgConvert<BasicSensorInfo, rpos::features::impact_sensor::ImpactSensorInfo>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.id = sltcVal.id;
+        sltcToRosMsg(sltcVal.coreSensorType, rosMsg.sensor_type);
+        sltcToRosMsg(sltcVal.type, rosMsg.impact_type);
+        sltcToRosMsg(sltcVal.pose, rosMsg.install_pose);
+        rosMsg.refresh_freq = sltcVal.refreshFreq;
+    }
+
+    void MsgConvert<BasicSensorInfo, rpos::features::impact_sensor::ImpactSensorInfo>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal.id = rosMsg.id;
+        rosMsgToSltc(rosMsg.sensor_type, sltcVal.coreSensorType);
+        rosMsgToSltc(rosMsg.impact_type, sltcVal.type);
+        rosMsgToSltc(rosMsg.install_pose, sltcVal.pose);
+        sltcVal.refreshFreq = rosMsg.refresh_freq;
+    }
+
+    void MsgConvert<ActionDirection, rpos::core::ACTION_DIRECTION>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+    	switch (sltcVal)
+    	{
+    	case rpos::core::FORWARD:
+    		rosMsg.direction = ros_msg_t::FORWARD;
+    		break;
+    	case rpos::core::BACKWARD:
+    		rosMsg.direction = ros_msg_t::BACKWARD;
+    		break;
+    	case rpos::core::TURNRIGHT:
+    		rosMsg.direction = ros_msg_t::TURNRIGHT;
+    		break;
+    	case rpos::core::TURNLEFT:
+    		rosMsg.direction = ros_msg_t::TURNLEFT;
+    		break;
+    	default:
+    		rosMsg.direction = ros_msg_t::UNKNOWN;
+    		break;
+    	}
+    }
+
+    void MsgConvert<ActionDirection, rpos::core::ACTION_DIRECTION>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+    	switch (rosMsg.direction)
+    	{
+    	case ros_msg_t::FORWARD:
+    		sltcVal = rpos::core::FORWARD;
+    		break;
+    	case ros_msg_t::BACKWARD:
+    		sltcVal = rpos::core::BACKWARD;
+    		break;
+    	case ros_msg_t::TURNRIGHT:
+    		sltcVal = rpos::core::TURNRIGHT;
+    		break;
+    	case ros_msg_t::TURNLEFT:
+    		sltcVal = rpos::core::TURNLEFT;
+    		break;
+    	default:
+    		//sltcVal = ((rpos::core::ACTION_DIRECTION)-1);
+    		sltcVal = rpos::core::INVALIDDIRECTION;
+    		break;
+    	}
+    }
+
+    void MsgConvert<MoveOptionFlag, rpos::features::motion_planner::MoveOptionFlag>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+    	// Note: currently just use the SAME interger values.
+    	rosMsg.flags = static_cast<std::uint32_t>(sltcVal);
+    }
+
+    void MsgConvert<MoveOptionFlag, rpos::features::motion_planner::MoveOptionFlag>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+    	// Note: currently just use the SAME interger values.
+    	sltcVal = static_cast<rpos::features::motion_planner::MoveOptionFlag>(rosMsg.flags);
+    }
+
+    void MsgConvert<MoveOptions, rpos::features::motion_planner::MoveOptions>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+    	sltcToRosMsg(sltcVal.flag, rosMsg.opt_flags);
+    	optionalToRosMsg(sltcVal.speed_ratio, rosMsg.speed_ratio);
+    }
+
+    void MsgConvert<MoveOptions, rpos::features::motion_planner::MoveOptions>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+    	rosMsgToSltc(rosMsg.opt_flags, sltcVal.flag);
+    	rosMsgToOptional(rosMsg.speed_ratio, sltcVal.speed_ratio);
+    }
+
+    void MsgConvert<geometry_msgs::Point, rpos::core::Location>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+    	rosMsg.x = sltcVal.x();
+    	rosMsg.y = sltcVal.y();
+    	rosMsg.z = sltcVal.z();
+    }
+
+    void MsgConvert<geometry_msgs::Point, rpos::core::Location>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+    	sltcVal.x() = rosMsg.x;
+    	sltcVal.y() = rosMsg.y;
+    	sltcVal.z() = rosMsg.z;
+    }
+
+    void MsgConvert<geometry_msgs::Quaternion, rpos::core::Rotation>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg = tf::createQuaternionMsgFromRollPitchYaw(sltcVal.roll(), sltcVal.pitch(), sltcVal.yaw());
+    }
+
+    void MsgConvert<geometry_msgs::Quaternion, rpos::core::Rotation>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        if (0.0 == rosMsg.x && 0.0 == rosMsg.y && 0.0 == rosMsg.z && 0.0 == rosMsg.w)
+        {
+            sltcVal = sltc_val_t(0.0, 0.0, 0.0);
+            return;
+        }
+
+        const tf::Quaternion tq(rosMsg.x, rosMsg.y, rosMsg.z, rosMsg.w);
+        const tf::Matrix3x3 tMat(tq);
+        tMat.getRPY(sltcVal.roll(), sltcVal.pitch(), sltcVal.yaw());
+
+        if (std::isnan(sltcVal.roll()))
+        {
+            ROS_WARN("Quaternion to RPY, roll is nan, set to zero.");
+            sltcVal.roll() = 0.0;
+        }
+        if (std::isnan(sltcVal.pitch()))
+        {
+            ROS_WARN("Quaternion to RPY, pitch is nan, set to zero.");
+            sltcVal.pitch() = 0.0;
+        }
+        if (std::isnan(sltcVal.yaw()))
+        {
+            ROS_WARN("Quaternion to RPY, yaw is nan, set to zero.");
+            sltcVal.yaw() = 0.0;
+        }
+    }
+
+    void MsgConvert<geometry_msgs::Pose, rpos::core::Pose>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        sltcToRosMsg(sltcVal.location(), rosMsg.position);
+        sltcToRosMsg(sltcVal.rotation(), rosMsg.orientation);
+    }
+
+    void MsgConvert<geometry_msgs::Pose, rpos::core::Pose>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        rosMsgToSltc(rosMsg.position, sltcVal.location());
+        rosMsgToSltc(rosMsg.orientation, sltcVal.rotation());
+    }
+
+    void MsgConvert<Vec2DInt32, rpos::core::Vector2i>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.x = sltcVal.x();
+        rosMsg.y = sltcVal.y();
+    }
+
+    void MsgConvert<Vec2DInt32, rpos::core::Vector2i>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal = sltc_val_t(rosMsg.x, rosMsg.y);
+    }
+
+    void MsgConvert<Vec2DFlt32, rpos::core::Vector2f>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.x = sltcVal.x();
+        rosMsg.y = sltcVal.y();
+    }
+
+    void MsgConvert<Vec2DFlt32, rpos::core::Vector2f>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal = sltc_val_t(rosMsg.x, rosMsg.y);
+    }
+
+    void MsgConvert<Vec2DFlt32, rpos::core::Point>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.x = sltcVal.x();
+        rosMsg.y = sltcVal.y();
+    }
+
+    void MsgConvert<Vec2DFlt32, rpos::core::Point>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal = sltc_val_t(rosMsg.x, rosMsg.y);
+    }
+
+    void MsgConvert<Line2DFlt32, rpos::core::Line>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.id = sltcVal.id();
+        sltcToRosMsg(sltcVal.startP(), rosMsg.start);
+        sltcToRosMsg(sltcVal.endP(), rosMsg.end);
+    }
+
+    void MsgConvert<Line2DFlt32, rpos::core::Line>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal.id() = rosMsg.id;
+        rosMsgToSltc(rosMsg.start, sltcVal.startP());
+        rosMsgToSltc(rosMsg.end, sltcVal.endP());
+    }
+
+    void MsgConvert<RectInt32, rpos::core::RectangleI>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.x = sltcVal.x();
+        rosMsg.y = sltcVal.y();
+        rosMsg.w = sltcVal.width();
+        rosMsg.h = sltcVal.height();
+    }
+
+    void MsgConvert<RectInt32, rpos::core::RectangleI>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal = sltc_val_t(rosMsg.x, rosMsg.y, rosMsg.w, rosMsg.h);
+    }
+
+    void MsgConvert<RectFlt32, rpos::core::RectangleF>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        rosMsg.x = sltcVal.x();
+        rosMsg.y = sltcVal.y();
+        rosMsg.w = sltcVal.width();
+        rosMsg.h = sltcVal.height();
+    }
+
+    void MsgConvert<RectFlt32, rpos::core::RectangleF>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        sltcVal = sltc_val_t(rosMsg.x, rosMsg.y, rosMsg.w, rosMsg.h);
+    }
+
+    void MsgConvert<LocalizationMovement, rpos::features::motion_planner::RecoverLocalizationMovement>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        switch (sltcVal)
+        {
+        case rpos::features::motion_planner::RecoverLocalizationMovementNoMove:
+            rosMsg.type = ros_msg_t::NO_MOVE;
+            break;
+        case rpos::features::motion_planner::RecoverLocalizationMovementRotateOnly:
+            rosMsg.type = ros_msg_t::ROTATE_ONLY;
+            break;
+        case rpos::features::motion_planner::RecoverLocalizationMovementAny:
+            rosMsg.type = ros_msg_t::ANY;
+            break;
+        default:
+            rosMsg.type = ros_msg_t::UNKNOWN;
+            break;
+        }
+    }
+
+    void MsgConvert<LocalizationMovement, rpos::features::motion_planner::RecoverLocalizationMovement>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        switch (rosMsg.type)
+        {
+        case ros_msg_t::NO_MOVE:
+            sltcVal = rpos::features::motion_planner::RecoverLocalizationMovementNoMove;
+            break;
+        case ros_msg_t::ROTATE_ONLY:
+            sltcVal = rpos::features::motion_planner::RecoverLocalizationMovementRotateOnly;
+            break;
+        case ros_msg_t::ANY:
+            sltcVal = rpos::features::motion_planner::RecoverLocalizationMovementAny;
+            break;
+        default:
+            sltcVal = rpos::features::motion_planner::RecoverLocalizationMovementUnknown;
+            break;
+        }
+    }
+
+    void MsgConvert<LocalizationOptions, rpos::features::motion_planner::RecoverLocalizationOptions>::toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg)
+    {
+        optionalToRosMsg(sltcVal.maxRecoverTimeInMilliSeconds, rosMsg.max_time_ms);
+        toRosOptionalMsg(sltcVal.recoverMovementType, rosMsg.mvmt_type);
+    }
+
+    void MsgConvert<LocalizationOptions, rpos::features::motion_planner::RecoverLocalizationOptions>::toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal)
+    {
+        rosMsgToOptional(rosMsg.max_time_ms, sltcVal.maxRecoverTimeInMilliSeconds);
+        fromRosOptionalMsg(rosMsg.mvmt_type, sltcVal.recoverMovementType);
+    }
+
+    //////////////////////////////////////////////////////////////////////////
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/msg_convert.h

@@ -0,0 +1,377 @@
+
+#pragma once
+
+#include <ros/ros.h>
+#include <tf/message_filter.h>
+#include <sensor_msgs/Imu.h>
+#include <ros/time.h>
+
+#include <slamware_ros_sdk/utils.h>
+
+#include <slamware_ros_sdk/Vec2DInt32.h>
+#include <slamware_ros_sdk/Line2DFlt32Array.h>
+#include <slamware_ros_sdk/RectInt32.h>
+#include <slamware_ros_sdk/RobotDeviceInfo.h>
+#include <slamware_ros_sdk/BasicSensorInfoArray.h>
+#include <slamware_ros_sdk/BasicSensorValueDataArray.h>
+#include <slamware_ros_sdk/RobotBasicState.h>
+#include <slamware_ros_sdk/SyncMapRequest.h>
+#include <slamware_ros_sdk/MoveByDirectionRequest.h>
+#include <slamware_ros_sdk/MoveByThetaRequest.h>
+#include <slamware_ros_sdk/MoveToRequest.h>
+#include <slamware_ros_sdk/MoveToLocationsRequest.h>
+#include <slamware_ros_sdk/RotateToRequest.h>
+#include <slamware_ros_sdk/RotateRequest.h>
+#include <slamware_ros_sdk/RecoverLocalizationRequest.h>
+#include <slamware_ros_sdk/ClearMapRequest.h>
+#include <slamware_ros_sdk/SetMapUpdateRequest.h>
+#include <slamware_ros_sdk/SetMapLocalizationRequest.h>
+#include <slamware_ros_sdk/GoHomeRequest.h>
+#include <slamware_ros_sdk/CancelActionRequest.h>
+#include <slamware_ros_sdk/AddLineRequest.h>
+#include <slamware_ros_sdk/AddLinesRequest.h>
+#include <slamware_ros_sdk/RemoveLineRequest.h>
+#include <slamware_ros_sdk/ClearLinesRequest.h>
+#include <slamware_ros_sdk/MoveLineRequest.h>
+#include <slamware_ros_sdk/MoveLinesRequest.h>
+
+#include <rpos/core/geometry.h>
+#include <rpos/features/artifact_provider.h>
+#include <rpos/features/location_provider.h>
+#include <rpos/features/motion_planner.h>
+#include <rpos/features/system_resource.h>
+#include <rpos/features/impact_sensor_feature.h>
+
+#include <vector>
+#include <map>
+
+#define M_PI       3.14159265358979323846   // pi
+
+namespace slamware_ros_sdk {
+
+    //////////////////////////////////////////////////////////////////////////
+    // Important Notes:
+    //      Generally, MsgConvert just overwrites known fields;
+    //          unknown fields, which are new added and their codes are
+    //          not added into MsgConvert, will be unchanged.
+    //////////////////////////////////////////////////////////////////////////
+
+    //////////////////////////////////////////////////////////////////////////
+    
+    template<class RosMsgT, class SltcValT>
+    struct MsgConvert;
+
+    template<class RosMsgT, class SltcValT>
+    inline void sltcToRosMsg(const SltcValT& sltcVal, RosMsgT& rosMsg)
+    {
+        MsgConvert<RosMsgT, SltcValT>::toRos(sltcVal, rosMsg);
+    }
+    template<class RosMsgT, class SltcValT>
+    inline void rosMsgToSltc(const RosMsgT& rosMsg, SltcValT& sltcVal)
+    {
+        MsgConvert<RosMsgT, SltcValT>::toSltc(rosMsg, sltcVal);
+    }
+
+    //////////////////////////////////////////////////////////////////////////
+
+    template<class ValT, class RosMsgT>
+    inline void toRosOptionalMsg(const boost::optional<ValT> & optVal, RosMsgT& rosMsg)
+    {
+        if (optVal)
+        {
+            rosMsg.is_valid = true;
+            sltcToRosMsg(*optVal, rosMsg.value);
+        }
+        else
+        {
+            rosMsg = RosMsgT();
+        }
+    }
+
+    template<class ValT, class RosMsgT>
+    inline void fromRosOptionalMsg(const RosMsgT& rosMsg, boost::optional<ValT> & optVal)
+    {
+        if (rosMsg.is_valid)
+        {
+            optVal = ValT();
+            rosMsgToSltc(rosMsg.value, *optVal);
+        }
+        else
+        {
+            optVal.reset();
+        }
+    }
+
+    //////////////////////////////////////////////////////////////////////////
+
+    template<>
+    struct MsgConvert<MapKind, rpos::features::location_provider::MapKind>
+    {
+    public:
+        typedef MapKind                                        ros_msg_t;
+        typedef rpos::features::location_provider::MapKind     sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<ArtifactUsage, rpos::features::artifact_provider::ArtifactUsage>
+    {
+    public:
+        typedef ArtifactUsage                                         ros_msg_t;
+        typedef rpos::features::artifact_provider::ArtifactUsage      sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<SensorType, rpos::core::SensorType>
+    {
+    public:
+        typedef SensorType                  ros_msg_t;
+        typedef rpos::core::SensorType      sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<ImpactType, rpos::features::impact_sensor::ImpactSensorType>
+    {
+    public:
+        typedef ImpactType                                          ros_msg_t;
+        typedef rpos::features::impact_sensor::ImpactSensorType     sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<BasicSensorInfo, rpos::features::impact_sensor::ImpactSensorInfo>
+    {
+    public:
+        typedef BasicSensorInfo                                     ros_msg_t;
+        typedef rpos::features::impact_sensor::ImpactSensorInfo     sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<ActionDirection, rpos::core::ACTION_DIRECTION>
+    {
+    public:
+        typedef ActionDirection                     ros_msg_t;
+        typedef rpos::core::ACTION_DIRECTION        sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<MoveOptionFlag, rpos::features::motion_planner::MoveOptionFlag>
+    {
+    public:
+        typedef MoveOptionFlag                                  ros_msg_t;
+        typedef rpos::features::motion_planner::MoveOptionFlag  sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<MoveOptions, rpos::features::motion_planner::MoveOptions>
+    {
+    public:
+        typedef MoveOptions                                     ros_msg_t;
+        typedef rpos::features::motion_planner::MoveOptions     sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<geometry_msgs::Point, rpos::core::Location>
+    {
+    public:
+        typedef geometry_msgs::Point                ros_msg_t;
+        typedef rpos::core::Location                sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<geometry_msgs::Quaternion, rpos::core::Rotation>
+    {
+    public:
+        typedef geometry_msgs::Quaternion           ros_msg_t;
+        typedef rpos::core::Rotation                sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<geometry_msgs::Pose, rpos::core::Pose>
+    {
+    public:
+        typedef geometry_msgs::Pose                 ros_msg_t;
+        typedef rpos::core::Pose                    sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<Vec2DInt32, rpos::core::Vector2i>
+    {
+    public:
+        typedef Vec2DInt32                        ros_msg_t;
+        typedef rpos::core::Vector2i              sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    // Vec2DFlt32 <<======>> rpos::core::Vector2f
+    template<>
+    struct MsgConvert<Vec2DFlt32, rpos::core::Vector2f>
+    {
+    public:
+        typedef Vec2DFlt32                        ros_msg_t;
+        typedef rpos::core::Vector2f              sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+    // Vec2DFlt32 <<======>> rpos::core::Point
+    template<>
+    struct MsgConvert<Vec2DFlt32, rpos::core::Point>
+    {
+    public:
+        typedef Vec2DFlt32                        ros_msg_t;
+        typedef rpos::core::Point                 sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<Line2DFlt32, rpos::core::Line>
+    {
+    public:
+        typedef Line2DFlt32                       ros_msg_t;
+        typedef rpos::core::Line                  sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<RectInt32, rpos::core::RectangleI>
+    {
+    public:
+        typedef RectInt32                           ros_msg_t;
+        typedef rpos::core::RectangleI              sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<RectFlt32, rpos::core::RectangleF>
+    {
+    public:
+        typedef RectFlt32                           ros_msg_t;
+        typedef rpos::core::RectangleF              sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<LocalizationMovement, rpos::features::motion_planner::RecoverLocalizationMovement>
+    {
+    public:
+        typedef LocalizationMovement                                            ros_msg_t;
+        typedef rpos::features::motion_planner::RecoverLocalizationMovement     sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    template<>
+    struct MsgConvert<LocalizationOptions, rpos::features::motion_planner::RecoverLocalizationOptions>
+    {
+    public:
+        typedef LocalizationOptions                                            ros_msg_t;
+        typedef rpos::features::motion_planner::RecoverLocalizationOptions     sltc_val_t;
+    public:
+        static void toRos(const sltc_val_t& sltcVal, ros_msg_t& rosMsg);
+        static void toSltc(const ros_msg_t& rosMsg, sltc_val_t& sltcVal);
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    template<class RosMsgT, class SltcValT, class RosVecAllocT, class SltcVecAllocT>
+    struct MsgConvert< std::vector<RosMsgT, RosVecAllocT> , std::vector<SltcValT, SltcVecAllocT> >
+    {
+    public:
+        typedef std::vector<RosMsgT, RosVecAllocT>              ros_msg_t;
+        typedef std::vector<SltcValT, SltcVecAllocT>            sltc_val_t;
+
+    public:
+        static void toRos(const sltc_val_t& vSltcVal, ros_msg_t& vRosMsg)
+        {
+            const size_t szCnt = vSltcVal.size();
+            vRosMsg.resize(szCnt);
+            for (size_t t = 0; t < szCnt; ++t)
+            {
+                sltcToRosMsg(vSltcVal[t], vRosMsg[t]);
+            }
+        }
+
+        static void toSltc(const ros_msg_t& vRosMsg, sltc_val_t& vSltcVal)
+        {
+            const size_t szCnt = vRosMsg.size();
+            vSltcVal.resize(szCnt);
+            for (size_t t = 0; t < szCnt; ++t)
+            {
+                rosMsgToSltc(vRosMsg[t], vSltcVal[t]);
+            }
+        }
+    };
+
+    template<class RosKeyT, class SltcKeyT, class RosMsgT, class SltcValT, class RosCmpT, class SltcCmpT, class RosAllocT, class SltcAllocT>
+    struct MsgConvert< std::map<RosKeyT, RosMsgT, RosCmpT, RosAllocT>, std::map<SltcKeyT, SltcValT, SltcCmpT, SltcAllocT> >
+    {
+    public:
+        typedef std::map<RosKeyT, RosMsgT, RosCmpT, RosAllocT>              ros_msg_t;
+        typedef std::map<SltcKeyT, SltcValT, SltcCmpT, SltcAllocT>          sltc_val_t;
+
+    public:
+        static void toRos(const sltc_val_t& mapSltcVal, ros_msg_t& mapRosMsg)
+        {
+            mapRosMsg.clear();
+            for (auto cit = mapSltcVal.cbegin(), citEnd = mapSltcVal.cend(); citEnd != cit; ++cit)
+            {
+                sltcToRosMsg(cit->second, mapRosMsg[cit->first]);
+            }
+        }
+
+        static void toSltc(const ros_msg_t& mapRosMsg, sltc_val_t& mapSltcVal)
+        {
+            mapSltcVal.clear();
+            for (auto cit = mapRosMsg.cbegin(), citEnd = mapRosMsg.cend(); citEnd != cit; ++cit)
+            {
+                rosMsgToSltc(cit->second, mapSltcVal[cit->first]);
+            }
+        }
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_map_holder.cpp

@@ -0,0 +1,480 @@
+
+#include "server_map_holder.h"
+
+#include <boost/assert.hpp>
+
+#include <cmath>
+#include <cstring>
+#include <stdexcept>
+
+namespace slamware_ros_sdk {
+
+    const float ServerMapHolder::C_DEFAULT_RESOLUTION = 0.05f;
+    const std::uint32_t ServerMapHolder::C_MAP_DATA_SIZE_ALIGNMENT = 16u;
+    const int ServerMapHolder::C_DEFAULT_MORE_CELL_CNT_TO_EXTEND = 32;
+
+    ServerMapHolder::ServerMapHolder()
+        : moreCellCntToExtend_(C_DEFAULT_MORE_CELL_CNT_TO_EXTEND)
+        , resolution_(C_DEFAULT_RESOLUTION)
+    {
+        //
+    }
+
+    ServerMapHolder::~ServerMapHolder()
+    {
+        //
+    }
+
+    void ServerMapHolder::clear()
+    {
+        availCellIdxRect_ = rpos::core::RectangleI();
+        availMapArea_ = rpos::core::RectangleF();
+        validCellIdxRect_ = rpos::core::RectangleI();
+        validMapArea_ = rpos::core::RectangleF();
+        map_data_t().swap(mapDat_);
+    }
+
+    void ServerMapHolder::reinit(float resolution)
+    {
+        sfCheckResolutionValidity_(resolution);
+
+        clear();
+        resolution_ = resolution;
+    }
+
+    void ServerMapHolder::setMoreCellCountToExtend(int moreCellCntToExtend)
+    {
+        moreCellCntToExtend_ = (0 <= moreCellCntToExtend ? moreCellCntToExtend : C_DEFAULT_MORE_CELL_CNT_TO_EXTEND);
+    }
+
+    void ServerMapHolder::reserveByCellIdxRect(const rpos::core::RectangleI& cellIdxRect)
+    {
+        if (sfIsCellIdxRectEmpty(cellIdxRect))
+            return;
+        if (sfDoesCellIdxRectContain(availCellIdxRect_, cellIdxRect))
+            return;
+
+        auto newAvailCellIdxRect = sfMergeCellIdxRect(availCellIdxRect_, cellIdxRect);
+        newAvailCellIdxRect = sfCalcAdjustedCellIdxRect_(newAvailCellIdxRect);
+
+        const size_t newDatSize = newAvailCellIdxRect.width() * newAvailCellIdxRect.height();
+        map_data_t newMapDat(newDatSize);
+        std::memset(newMapDat.data(), 0, (newDatSize * sizeof(cell_value_t)));
+
+        if (!sfIsCellIdxRectEmpty(validCellIdxRect_))
+        {
+            const int destOffX = validCellIdxRect_.x() - newAvailCellIdxRect.x();
+            BOOST_ASSERT(0 <= destOffX && destOffX < newAvailCellIdxRect.width());
+            BOOST_ASSERT(destOffX + validCellIdxRect_.width() <= newAvailCellIdxRect.width());
+
+            const int destOffY = validCellIdxRect_.y() - newAvailCellIdxRect.y();
+            BOOST_ASSERT(0 <= destOffY && destOffY < newAvailCellIdxRect.height());
+            BOOST_ASSERT(destOffY + validCellIdxRect_.height() <= newAvailCellIdxRect.height());
+
+            const size_t srcBytesPerLine = validCellIdxRect_.width() * sizeof(cell_value_t);
+            size_t srcCellOffset = (validCellIdxRect_.y() - availCellIdxRect_.y()) * availCellIdxRect_.width() + (validCellIdxRect_.x() - availCellIdxRect_.x());
+            size_t destCellOffset = destOffY * newAvailCellIdxRect.width() + destOffX;
+            for (int j = 0; j < validCellIdxRect_.height(); ++j)
+            {
+                std::memcpy(&newMapDat[destCellOffset], &mapDat_[srcCellOffset], srcBytesPerLine);
+
+                srcCellOffset += availCellIdxRect_.width();
+                destCellOffset += newAvailCellIdxRect.width();
+            }
+        }
+
+        availCellIdxRect_ = newAvailCellIdxRect;
+        availMapArea_ = calcAreaByCellIdxRect(availCellIdxRect_);
+        mapDat_.swap(newMapDat);
+
+    #if 0
+        ROS_INFO("ServerMapHolder::reserveByCellIdxRect(), avail rect: ((%d, %d), (%d, %d)), avail area: ((%f, %f), (%f, %f))."
+            , availCellIdxRect_.x(), availCellIdxRect_.y(), availCellIdxRect_.width(), availCellIdxRect_.height()
+            , availMapArea_.x(), availMapArea_.y(), availMapArea_.width(), availMapArea_.height()
+            );
+    #endif
+    }
+
+    void ServerMapHolder::reserveByArea(const rpos::core::RectangleF& reqArea)
+    {
+        if (reqArea.empty())
+            return;
+
+        const auto minBoundingCellIdxRect = calcMinBoundingCellIdxRect(reqArea);
+        reserveByCellIdxRect(minBoundingCellIdxRect);
+    }
+
+    void ServerMapHolder::setMapData(float x, float y, float resolution, int dimensionX, int dimensionY, const cell_value_t* srcDat)
+    {
+        if (dimensionX < 1 || dimensionY < 1)
+            return;
+        BOOST_ASSERT(nullptr != srcDat);
+
+        sfCheckResolutionEquality_(resolution, resolution_);
+
+        const rpos::core::RectangleI cellIdxRectToUp(static_cast<int>(std::round(x / resolution_))
+            , static_cast<int>(std::round(y / resolution_))
+            , dimensionX
+            , dimensionY
+            );
+        checkToExtendMap_(cellIdxRectToUp);
+
+        const int destOffX = cellIdxRectToUp.x() - availCellIdxRect_.x();
+        BOOST_ASSERT(0 <= destOffX && destOffX < availCellIdxRect_.width());
+        BOOST_ASSERT(destOffX + dimensionX <= availCellIdxRect_.width());
+
+        const int destOffY = cellIdxRectToUp.y() - availCellIdxRect_.y();
+        BOOST_ASSERT(0 <= destOffY && destOffY < availCellIdxRect_.height());
+        BOOST_ASSERT(destOffY + dimensionY <= availCellIdxRect_.height());
+
+        const size_t srcBytesPerLine = dimensionX * sizeof(cell_value_t);
+        size_t srcCellOffset = 0;
+        size_t destCellOffset = destOffY * availCellIdxRect_.width() + destOffX;
+        for (int j = 0; j < dimensionY; ++j)
+        {
+            std::memcpy(&mapDat_[destCellOffset], &srcDat[srcCellOffset], srcBytesPerLine);
+
+            srcCellOffset += dimensionX;
+            destCellOffset += availCellIdxRect_.width();
+        }
+
+        validCellIdxRect_ = sfMergeCellIdxRect(validCellIdxRect_, cellIdxRectToUp);
+        validMapArea_ = calcAreaByCellIdxRect(validCellIdxRect_);
+    }
+
+    void ServerMapHolder::setMapData(const rpos::features::location_provider::Map& hMap)
+    {
+        if (!hMap)
+            return;
+
+        const auto& mapPosition = hMap.getMapPosition();
+        const auto& mapDimension = hMap.getMapDimension();
+        const auto& mapResolution = hMap.getMapResolution();
+        const auto& mapDat = hMap.getMapData();
+
+        if (!rpos::system::types::fequal(mapResolution.x(), mapResolution.y()))
+            throw std::runtime_error("map resolution is different on x and y.");
+        if (static_cast<int>(mapDat.size()) < (mapDimension.x() * mapDimension.y()))
+            throw std::runtime_error("map data size is less than (dimensionX * dimensionY).");
+
+        setMapData(mapPosition.x(), mapPosition.y(), mapResolution.x(), mapDimension.x(), mapDimension.y(), mapDat.data());
+    }
+
+    rpos::core::RectangleI ServerMapHolder::fillRosMapMsg(const rpos::core::RectangleI& reqIdxRect, nav_msgs::GetMap::Response& msgMap) const
+    {
+        const auto resIdxRect = reqIdxRect;
+        const auto resArea = calcAreaByCellIdxRect(resIdxRect);
+
+        const auto intersecRect = sfIntersectionOfCellIdxRect(validCellIdxRect_, resIdxRect);
+
+        msgMap.map.info.resolution = resolution_;
+        msgMap.map.info.origin.position.x = resArea.x();
+        msgMap.map.info.origin.position.y = resArea.y();
+        msgMap.map.info.origin.position.z = 0.0;
+        msgMap.map.info.origin.orientation.x = 0.0;
+        msgMap.map.info.origin.orientation.y = 0.0;
+        msgMap.map.info.origin.orientation.z = 0.0;
+        msgMap.map.info.origin.orientation.w = 1.0;
+
+        msgMap.map.info.width = 0;
+        msgMap.map.info.height = 0;
+        msgMap.map.data.clear();
+        if (!sfIsCellIdxRectEmpty(resIdxRect))
+        {
+            msgMap.map.info.width = resIdxRect.width();
+            msgMap.map.info.height = resIdxRect.height();
+            const size_t destDatSize = msgMap.map.info.width * msgMap.map.info.height;
+            msgMap.map.data.resize(destDatSize, -1);
+            //
+            if (!sfIsCellIdxRectEmpty(intersecRect))
+            {
+                const int srcOffsetX = intersecRect.x() - availCellIdxRect_.x();
+                const int srcOffsetY = intersecRect.y() - availCellIdxRect_.y();
+                size_t srcRowOffset = srcOffsetY * availCellIdxRect_.width() + srcOffsetX;
+                //
+                const int destOffsetX = intersecRect.x() - resIdxRect.x();
+                const int destOffsetY = intersecRect.y() - resIdxRect.y();
+                size_t destRowOffset = destOffsetY * resIdxRect.width() + destOffsetX;
+                //
+                for (int j = 0; j < intersecRect.height(); ++j)
+                {
+                    size_t tSrcIdx = srcRowOffset;
+                    size_t tDestIdx = destRowOffset;
+                    for (int i = 0; i < intersecRect.width(); ++i)
+                    {
+                        const auto tVal = mapDat_[tSrcIdx];
+                        if (0 == tVal)
+                            msgMap.map.data[tDestIdx] = -1;
+                        else if (tVal <= 127)
+                            msgMap.map.data[tDestIdx] = 0;
+                        else if (127 < tVal)
+                            msgMap.map.data[tDestIdx] = 100;
+                        //
+                        ++tSrcIdx;
+                        ++tDestIdx;
+                    }
+                    //
+                    srcRowOffset += availCellIdxRect_.width();
+                    destRowOffset += resIdxRect.width();
+                }
+            }
+        }
+
+        return resIdxRect;
+    }
+
+    rpos::core::RectangleI ServerMapHolder::fillRosMapMsg(const rpos::core::RectangleF& reqArea, nav_msgs::GetMap::Response& msgMap) const
+    {
+        rpos::core::RectangleI reqIdxRect;
+        if (!reqArea.empty())
+            reqIdxRect = calcMinBoundingCellIdxRect(reqArea);
+        return fillRosMapMsg(reqIdxRect, msgMap);
+    }
+
+    rpos::core::RectangleI ServerMapHolder::fillRosMapMsg(nav_msgs::GetMap::Response& msgMap) const
+    {
+        return fillRosMapMsg(validCellIdxRect_, msgMap);
+    }
+
+    bool ServerMapHolder::sfDoesCellIdxRectContain(const rpos::core::RectangleI& cellIdxRect, const rpos::core::RectangleI& objRect)
+    {
+        if (sfIsCellIdxRectEmpty(cellIdxRect))
+            return false;
+        const int xEnd = cellIdxRect.x() + cellIdxRect.width();
+        const int yEnd = cellIdxRect.y() + cellIdxRect.height();
+
+        const int objXEnd = (0 < objRect.width() ? (objRect.x() + objRect.width()) : objRect.x());
+        const int objYEnd = (0 < objRect.height() ? (objRect.y() + objRect.height()) : objRect.y());
+        return (cellIdxRect.x() <= objRect.x() && objRect.x() < xEnd
+            && objXEnd <= xEnd
+            && cellIdxRect.y() <= objRect.y() && objRect.y() < yEnd
+            && objYEnd <= yEnd
+            );
+    }
+
+    rpos::core::RectangleI ServerMapHolder::sfMergeCellIdxRect(const rpos::core::RectangleI& idxRectA, const rpos::core::RectangleI& idxRectB)
+    {
+        if (sfIsCellIdxRectEmpty(idxRectB))
+            return idxRectA;
+        if (sfIsCellIdxRectEmpty(idxRectA))
+            return idxRectB;
+
+        const int xMin = std::min<int>(idxRectA.x(), idxRectB.x());
+        const int yMin = std::min<int>(idxRectA.y(), idxRectB.y());
+
+        const int aXEnd = idxRectA.x() + idxRectA.width();
+        const int aYEnd = idxRectA.y() + idxRectA.height();
+        const int bXEnd = idxRectB.x() + idxRectB.width();
+        const int bYEnd = idxRectB.y() + idxRectB.height();
+        const int xEnd = std::max<int>(aXEnd, bXEnd);
+        const int yEnd = std::max<int>(aYEnd, bYEnd);
+
+        BOOST_ASSERT(xMin < xEnd);
+        BOOST_ASSERT(yMin < yEnd);
+        return rpos::core::RectangleI(xMin
+            , yMin
+            , (xEnd - xMin)
+            , (yEnd - yMin)
+            );
+    }
+
+    rpos::core::RectangleI ServerMapHolder::sfIntersectionOfCellIdxRect(const rpos::core::RectangleI& idxRectA, const rpos::core::RectangleI& idxRectB)
+    {
+        if (sfIsCellIdxRectEmpty(idxRectA)
+            || sfIsCellIdxRectEmpty(idxRectB)
+            )
+        {
+            return rpos::core::RectangleI();
+        }
+
+        const int xMin = std::max<int>(idxRectA.x(), idxRectB.x());
+        const int yMin = std::max<int>(idxRectA.y(), idxRectB.y());
+
+        const int aXEnd = idxRectA.x() + idxRectA.width();
+        const int aYEnd = idxRectA.y() + idxRectA.height();
+        const int bXEnd = idxRectB.x() + idxRectB.width();
+        const int bYEnd = idxRectB.y() + idxRectB.height();
+        const int xEnd = std::min<int>(aXEnd, bXEnd);
+        const int yEnd = std::min<int>(aYEnd, bYEnd);
+
+        return rpos::core::RectangleI(xMin
+            , yMin
+            , (xMin < xEnd ? (xEnd - xMin) : 0)
+            , (yMin < yEnd ? (yEnd - yMin) : 0)
+            );
+    }
+
+    rpos::core::RectangleF ServerMapHolder::sfCalcAreaByCellIdxRect(float resolution, const rpos::core::RectangleI& cellIdxRect)
+    {
+        BOOST_ASSERT(FLT_EPSILON < resolution);
+        return rpos::core::RectangleF(resolution * cellIdxRect.x()
+            , resolution * cellIdxRect.y()
+            , resolution * cellIdxRect.width()
+            , resolution * cellIdxRect.height()
+            );
+    }
+
+    rpos::core::RectangleI ServerMapHolder::sfCalcMinBoundingCellIdxRect(float resolution, const rpos::core::RectangleF& reqArea)
+    {
+        BOOST_ASSERT(FLT_EPSILON < resolution);
+        
+        int idxXMin = static_cast<int>(std::floor(reqArea.x() / resolution));
+        if (reqArea.x() < resolution * idxXMin)
+            --idxXMin;
+
+        int idxYMin = static_cast<int>(std::floor(reqArea.y() / resolution));
+        if (reqArea.y() < resolution * idxYMin)
+            --idxYMin;
+
+        const float srcXMax = (0.0f < reqArea.width() ? (reqArea.x() + reqArea.width()) : reqArea.x());
+        int idxXMax = static_cast<int>(std::floor(srcXMax / resolution));
+        if (resolution * (idxXMax + 1) < srcXMax)
+            ++idxXMax;
+
+        const float srcYMax = (0.0f < reqArea.height() ? (reqArea.y() + reqArea.height()) : reqArea.y());
+        int idxYMax = static_cast<int>(std::floor(srcYMax / resolution));
+        if (resolution * (idxYMax + 1) < srcYMax)
+            ++idxYMax;
+
+        BOOST_ASSERT(idxXMin <= idxXMax);
+        BOOST_ASSERT(idxYMin <= idxYMax);
+        return rpos::core::RectangleI(idxXMin
+            , idxYMin
+            , (idxXMax - idxXMin + 1)
+            , (idxYMax - idxYMin + 1)
+            );
+    }
+
+    rpos::core::RectangleF ServerMapHolder::sfCalcMinBoundingArea(float resolution, const rpos::core::RectangleF& reqArea)
+    {
+        BOOST_ASSERT(FLT_EPSILON < resolution);
+        const auto minBoundingCellIdxRect = sfCalcMinBoundingCellIdxRect(resolution, reqArea);
+        return sfCalcAreaByCellIdxRect(resolution, minBoundingCellIdxRect);
+    }
+
+    rpos::core::RectangleI ServerMapHolder::sfCalcRoundedCellIdxRect(float resolution, const rpos::core::RectangleF& reqArea)
+    {
+        BOOST_ASSERT(FLT_EPSILON < resolution);
+        return rpos::core::RectangleI(static_cast<int>(std::round(reqArea.x() / resolution))
+            , static_cast<int>(std::round(reqArea.y() / resolution))
+            , static_cast<int>(std::round(reqArea.width() / resolution))
+            , static_cast<int>(std::round(reqArea.height() / resolution))
+            );
+    }
+
+    rpos::core::RectangleF ServerMapHolder::sfCalcRoundedArea(float resolution, const rpos::core::RectangleF& reqArea)
+    {
+        BOOST_ASSERT(FLT_EPSILON < resolution);
+        const auto roundedCellIdxRect = sfCalcRoundedCellIdxRect(resolution, reqArea);
+        return sfCalcAreaByCellIdxRect(resolution, roundedCellIdxRect);
+    }
+
+    void ServerMapHolder::checkToExtendMap_(const rpos::core::RectangleI& cellIdxRectToUp)
+    {
+        BOOST_ASSERT(0 <= moreCellCntToExtend_);
+        BOOST_ASSERT(0 <= availCellIdxRect_.width());
+        BOOST_ASSERT(0 <= availCellIdxRect_.height());
+
+        if (sfIsCellIdxRectEmpty(cellIdxRectToUp))
+            return;
+        if (sfDoesCellIdxRectContain(availCellIdxRect_, cellIdxRectToUp))
+            return;
+
+        int tXMin = availCellIdxRect_.x();
+        if (cellIdxRectToUp.x() < tXMin)
+            tXMin = cellIdxRectToUp.x() - moreCellCntToExtend_;
+
+        int tYMin = availCellIdxRect_.y();
+        if (cellIdxRectToUp.y() < tYMin)
+            tYMin = cellIdxRectToUp.y() - moreCellCntToExtend_;
+
+        const int upXEnd = cellIdxRectToUp.x() + cellIdxRectToUp.width();
+        int tXEnd = availCellIdxRect_.x() + availCellIdxRect_.width();
+        if (tXEnd < upXEnd)
+            tXEnd = upXEnd + moreCellCntToExtend_;
+
+        const int upYEnd = cellIdxRectToUp.y() + cellIdxRectToUp.height();
+        int tYEnd = availCellIdxRect_.y() + availCellIdxRect_.height();
+        if (tYEnd < upYEnd)
+            tYEnd = upYEnd + moreCellCntToExtend_;
+
+        BOOST_ASSERT(tXMin < tXEnd);
+        BOOST_ASSERT(tYMin < tYEnd);
+        const auto tCellIdxRect = rpos::core::RectangleI(tXMin
+            , tYMin
+            , (tXEnd - tXMin)
+            , (tYEnd - tYMin)
+            );
+        reserveByCellIdxRect(tCellIdxRect);
+    }
+
+    void ServerMapHolder::sfCheckResolutionValidity_(float resolution)
+    {
+        if (resolution <= FLT_EPSILON)
+            throw std::runtime_error("invalid resolution.");
+    }
+
+    void ServerMapHolder::sfCheckResolutionEquality_(float resA, float resB)
+    {
+        if (!rpos::system::types::fequal(resA, resB))
+            throw std::runtime_error("inconsistent resolution.");
+
+        if (resA <= FLT_EPSILON)
+            throw std::runtime_error("invalid resolution.");
+        if (resB <= FLT_EPSILON)
+            throw std::runtime_error("invalid resolution.");
+    }
+
+    int ServerMapHolder::sfGetAlignedFloorCellIdx_(int inCellIdx)
+    {
+        if (inCellIdx < 0)
+        {
+            std::uint32_t tmpIdx = static_cast<std::uint32_t>(-inCellIdx);
+            --tmpIdx;
+            tmpIdx /= C_MAP_DATA_SIZE_ALIGNMENT;
+            ++tmpIdx;
+            tmpIdx *= C_MAP_DATA_SIZE_ALIGNMENT;
+            return (-static_cast<int>(tmpIdx));
+        }
+        else
+        {
+            std::uint32_t tmpIdx = static_cast<std::uint32_t>(inCellIdx);
+            tmpIdx /= C_MAP_DATA_SIZE_ALIGNMENT;
+            tmpIdx *= C_MAP_DATA_SIZE_ALIGNMENT;
+            return static_cast<int>(tmpIdx);
+        }
+    }
+
+    int ServerMapHolder::sfGetAlignedCeilSize_(int inSize)
+    {
+        if (inSize < 0)
+            throw std::runtime_error("invalid input size.");
+
+        std::uint32_t tmpSize = static_cast<std::uint32_t>(inSize);
+        tmpSize += (C_MAP_DATA_SIZE_ALIGNMENT - 1);
+        tmpSize /= C_MAP_DATA_SIZE_ALIGNMENT;
+        tmpSize *= C_MAP_DATA_SIZE_ALIGNMENT;
+        return static_cast<int>(tmpSize);
+    }
+    
+    rpos::core::RectangleI ServerMapHolder::sfCalcAdjustedCellIdxRect_(const rpos::core::RectangleI& idxRect)
+    {
+        if (sfIsCellIdxRectEmpty(idxRect))
+            return rpos::core::RectangleI();
+
+        const int xMin = sfGetAlignedFloorCellIdx_(idxRect.x());
+        BOOST_ASSERT(xMin <= idxRect.x());
+        const int yMin = sfGetAlignedFloorCellIdx_(idxRect.y());
+        BOOST_ASSERT(yMin <= idxRect.y());
+
+        const int reqXEnd = idxRect.x() + idxRect.width();
+        const int reqYEnd = idxRect.y() + idxRect.height();
+        const int tWidth = sfGetAlignedCeilSize_(reqXEnd - xMin);
+        BOOST_ASSERT(reqXEnd <= xMin + tWidth);
+        const int tHeight = sfGetAlignedCeilSize_(reqYEnd - yMin);
+        BOOST_ASSERT(reqYEnd <= yMin + tHeight);
+
+        return rpos::core::RectangleI(xMin, yMin, tWidth, tHeight);
+    }
+
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_map_holder.h

@@ -0,0 +1,113 @@
+
+#pragma once
+
+#include <ros/ros.h>
+#include <nav_msgs/GetMap.h>
+
+#include <rpos/core/geometry.h>
+#include <rpos/features/location_provider/map.h>
+
+#include <cstdint>
+
+namespace slamware_ros_sdk {
+
+    class ServerMapHolder
+    {
+    public:
+        static const float C_DEFAULT_RESOLUTION;
+        static const std::uint32_t C_MAP_DATA_SIZE_ALIGNMENT;
+        static const int C_DEFAULT_MORE_CELL_CNT_TO_EXTEND;
+
+        typedef std::uint8_t                    cell_value_t;
+        typedef std::vector<cell_value_t>       map_data_t;
+
+    public:
+        ServerMapHolder();
+        ~ServerMapHolder();
+
+        float resolution() const { return resolution_; }
+        bool isMapDataEmpty() const { return sfIsCellIdxRectEmpty(validCellIdxRect_); }
+
+        // when extending rect of map data, it will extend more count of cells to prevent memory allocation frequently.
+        int getMoreCellCountToExtend() const { return moreCellCntToExtend_; }
+
+        void clear();
+        void reinit(float resolution);
+
+        void setMoreCellCountToExtend(int moreCellCntToExtend);
+
+        void reserveByCellIdxRect(const rpos::core::RectangleI& cellIdxRect);
+        void reserveByArea(const rpos::core::RectangleF& reqArea);
+
+        void setMapData(float x, float y, float resolution, int dimensionX, int dimensionY, const cell_value_t* srcDat);
+        void setMapData(const rpos::features::location_provider::Map& hMap);
+
+    public:
+        const rpos::core::RectangleI& getValidCellIdxRect() const { return validCellIdxRect_; }
+        const rpos::core::RectangleF& getValidMapArea() const { return validMapArea_; }
+
+        // returns the cell index rect of actually filled cells
+        rpos::core::RectangleI fillRosMapMsg(const rpos::core::RectangleI& reqIdxRect, nav_msgs::GetMap::Response& msgMap) const;
+        rpos::core::RectangleI fillRosMapMsg(const rpos::core::RectangleF& reqArea, nav_msgs::GetMap::Response& msgMap) const;
+        rpos::core::RectangleI fillRosMapMsg(nav_msgs::GetMap::Response& msgMap) const;
+
+    public:
+        rpos::core::RectangleF calcAreaByCellIdxRect(const rpos::core::RectangleI& cellIdxRect) const { return sfCalcAreaByCellIdxRect(resolution_, cellIdxRect); }
+
+        rpos::core::RectangleI calcMinBoundingCellIdxRect(const rpos::core::RectangleF& reqArea) const { return sfCalcMinBoundingCellIdxRect(resolution_, reqArea); }
+        rpos::core::RectangleF calcMinBoundingArea(const rpos::core::RectangleF& reqArea) const { return sfCalcMinBoundingArea(resolution_, reqArea); }
+
+        rpos::core::RectangleI calcRoundedCellIdxRect(const rpos::core::RectangleF& reqArea) const { return sfCalcRoundedCellIdxRect(resolution_, reqArea); }
+        rpos::core::RectangleF calcRoundedArea(const rpos::core::RectangleF& reqArea) const { return sfCalcRoundedArea(resolution_, reqArea); }
+
+    public:
+        static bool sfIsCellIdxRectEmpty(const rpos::core::RectangleI& cellIdxRect)
+        {
+            return (cellIdxRect.width() <= 0 || cellIdxRect.height() <= 0);
+        }
+        static bool sfDoesCellIdxRectContain(const rpos::core::RectangleI& cellIdxRect, int cellIdxX, int cellIdxY)
+        {
+            return (0 < cellIdxRect.width() && 0 < cellIdxRect.height()
+                && cellIdxRect.x() <= cellIdxX && cellIdxX < (cellIdxRect.x() + cellIdxRect.width())
+                && cellIdxRect.y() <= cellIdxY && cellIdxY < (cellIdxRect.y() + cellIdxRect.height())
+                );
+        }
+        static bool sfDoesCellIdxRectContain(const rpos::core::RectangleI& cellIdxRect, const rpos::core::RectangleI& objRect);
+        static rpos::core::RectangleI sfMergeCellIdxRect(const rpos::core::RectangleI& idxRectA, const rpos::core::RectangleI& idxRectB);
+        static rpos::core::RectangleI sfIntersectionOfCellIdxRect(const rpos::core::RectangleI& idxRectA, const rpos::core::RectangleI& idxRectB);
+
+        static rpos::core::RectangleF sfCalcAreaByCellIdxRect(float resolution, const rpos::core::RectangleI& cellIdxRect);
+
+        static rpos::core::RectangleI sfCalcMinBoundingCellIdxRect(float resolution, const rpos::core::RectangleF& reqArea);
+        static rpos::core::RectangleF sfCalcMinBoundingArea(float resolution, const rpos::core::RectangleF& reqArea);
+
+        static rpos::core::RectangleI sfCalcRoundedCellIdxRect(float resolution, const rpos::core::RectangleF& reqArea);
+        static rpos::core::RectangleF sfCalcRoundedArea(float resolution, const rpos::core::RectangleF& reqArea);
+
+    private:
+        void checkToExtendMap_(const rpos::core::RectangleI& cellIdxRectToUp);
+
+    private:
+        static void sfCheckResolutionValidity_(float resolution);
+        static void sfCheckResolutionEquality_(float resA, float resB);
+
+        // returns aligned floor cell index, may be negative.
+        static int sfGetAlignedFloorCellIdx_(int inCellIdx);
+        // returns aligned ceil size (>= 0).
+        static int sfGetAlignedCeilSize_(int inSize);
+        
+        // returns cell index rect that position is aligned floor and size is aligned ceil.
+        static rpos::core::RectangleI sfCalcAdjustedCellIdxRect_(const rpos::core::RectangleI& idxRect);
+
+    private:
+        int moreCellCntToExtend_;
+
+        float resolution_;
+        rpos::core::RectangleI availCellIdxRect_;
+        rpos::core::RectangleF availMapArea_;
+        rpos::core::RectangleI validCellIdxRect_;
+        rpos::core::RectangleF validMapArea_;
+        map_data_t mapDat_;
+    };
+
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_params.cpp

@@ -0,0 +1,129 @@
+
+#include "server_params.h"
+
+#include <cmath>
+
+namespace slamware_ros_sdk {
+
+    //////////////////////////////////////////////////////////////////////////
+
+    const float C_FLT_PI = ((float)M_PI);
+    const float C_FLT_2PI = (C_FLT_PI * 2);
+
+    //////////////////////////////////////////////////////////////////////////
+
+    ServerParams::ServerParams()
+    {
+        resetToDefault();
+    }
+
+    void ServerParams::resetToDefault()
+    {
+        ip_address = "192.168.11.1";
+        robot_port = 1445;
+        reconn_wait_ms = (1000U * 3U);
+
+        angle_compensate = true;
+        fixed_odom_map_tf = true;
+
+        robot_frame = "base_link";
+        laser_frame = "laser";
+        map_frame = "map";
+        odom_frame = "odom";
+        robot_pose_frame = "robot_pose";
+
+        odometry_pub_period = 0.05f;
+        robot_pose_pub_period = 0.05f;
+        scan_pub_period = 0.1f;
+        map_update_period = 0.2f;
+        map_pub_period = 0.2f;
+        basic_sensors_info_update_period = 7.0f;
+        basic_sensors_values_pub_period = 0.05f;
+        path_pub_period = 0.05f;
+        robot_basic_state_pub_period = 1.0f;
+        virtual_walls_pub_period = 0.5f;
+        virtual_tracks_pub_period = 0.5f;
+
+        map_sync_once_get_max_wh = 100.f;
+        map_update_near_robot_half_wh = 8.0f;
+
+        imu_raw_data_period = 0.05f;
+        event_period = 1.0f;
+
+        scan_topic = "scan";
+        odom_topic = "odom";
+        robot_pose_topic = "robot_pose";
+        map_topic = "map";
+        map_info_topic = "map_metadata";
+        basic_sensors_info_topic = "basic_sensors_info";
+        basic_sensors_values_topic = "basic_sensors_values";
+        path_topic = "global_plan_path";
+
+        vel_control_topic = "/cmd_vel";
+        goal_topic = "/move_base_simple/goal";
+
+        imu_raw_data_topic = "imu_raw_data";
+        imu_raw_mag_data_topic = "imu_raw_mag_data";
+
+        raw_ladar_data = false;
+        ladar_data_clockwise = true;
+
+        pub_accumulate_odometry = false;
+    }
+
+    void ServerParams::setBy(const ros::NodeHandle& nhRos)
+    {
+        nhRos.getParam("ip_address", ip_address);
+        nhRos.getParam("robot_port", robot_port);
+        nhRos.getParam("reconn_wait_ms", reconn_wait_ms);
+
+        nhRos.getParam("angle_compensate", angle_compensate);
+        nhRos.getParam("fixed_odom_map_tf", fixed_odom_map_tf);
+
+        nhRos.getParam("robot_frame", robot_frame);
+        nhRos.getParam("laser_frame", laser_frame);
+        nhRos.getParam("map_frame", map_frame);
+        nhRos.getParam("odom_frame", odom_frame);
+        nhRos.getParam("robot_pose_frame", robot_pose_frame);
+
+        nhRos.getParam("odometry_pub_period", odometry_pub_period);
+        nhRos.getParam("robot_pose_pub_period", robot_pose_pub_period);
+        nhRos.getParam("scan_pub_period", scan_pub_period);
+        nhRos.getParam("map_update_period", map_update_period);
+        nhRos.getParam("map_pub_period", map_pub_period);
+        nhRos.getParam("basic_sensors_info_update_period", basic_sensors_info_update_period);
+        nhRos.getParam("basic_sensors_values_pub_period", basic_sensors_values_pub_period);
+        nhRos.getParam("path_pub_period", path_pub_period);
+        nhRos.getParam("robot_basic_state_pub_period", robot_basic_state_pub_period);
+        nhRos.getParam("virtual_walls_pub_period", virtual_walls_pub_period);
+        nhRos.getParam("virtual_tracks_pub_period", virtual_tracks_pub_period);
+        nhRos.getParam("imu_raw_data_period", imu_raw_data_period);
+        nhRos.getParam("event_period", event_period);
+
+        nhRos.getParam("map_sync_once_get_max_wh", map_sync_once_get_max_wh);
+        nhRos.getParam("map_update_near_robot_half_wh", map_update_near_robot_half_wh);
+
+        nhRos.getParam("scan_topic", scan_topic);
+        nhRos.getParam("odom_topic", odom_topic);
+        nhRos.getParam("robot_pose_topic", robot_pose_topic);
+        nhRos.getParam("map_topic", map_topic);
+        nhRos.getParam("map_info_topic", map_info_topic);
+        nhRos.getParam("basic_sensors_info_topic", basic_sensors_info_topic);
+        nhRos.getParam("basic_sensors_values_topic", basic_sensors_values_topic);
+        nhRos.getParam("path_topic", path_topic);
+
+        nhRos.getParam("vel_control_topic", vel_control_topic);
+        nhRos.getParam("goal_topic", goal_topic);
+
+        nhRos.getParam("imu_raw_data_topic", imu_raw_data_topic);
+        nhRos.getParam("imu_raw_mag_data_topic", imu_raw_mag_data_topic);
+
+        nhRos.getParam("raw_ladar_data", raw_ladar_data);
+        nhRos.getParam("ladar_data_clockwise", ladar_data_clockwise);
+
+        nhRos.getParam("pub_accumulate_odometry", pub_accumulate_odometry);
+    }
+
+    //////////////////////////////////////////////////////////////////////////
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_params.h

@@ -0,0 +1,75 @@
+
+#pragma once
+
+#include <ros/ros.h>
+
+#include <slamware_ros_sdk/utils.h>
+
+#include <boost/shared_ptr.hpp>
+
+namespace slamware_ros_sdk {
+
+    extern const float C_FLT_PI;
+    extern const float C_FLT_2PI;
+
+    struct ServerParams
+    {
+        std::string ip_address;
+        int robot_port;
+        int reconn_wait_ms;
+
+        bool angle_compensate;
+        bool fixed_odom_map_tf;
+
+        std::string robot_frame;
+        std::string laser_frame;
+        std::string map_frame;
+        std::string odom_frame;
+        std::string robot_pose_frame;
+
+        float odometry_pub_period;
+
+        float robot_pose_pub_period;
+        float scan_pub_period;
+        float map_update_period;
+        float map_pub_period;
+        float basic_sensors_info_update_period;
+        float basic_sensors_values_pub_period;
+        float path_pub_period;
+        float robot_basic_state_pub_period;
+        float virtual_walls_pub_period;
+        float virtual_tracks_pub_period;
+
+        float map_sync_once_get_max_wh;
+        float map_update_near_robot_half_wh;
+
+        float imu_raw_data_period;
+        float event_period;
+
+        std::string scan_topic;
+        std::string odom_topic;
+        std::string robot_pose_topic;
+        std::string map_topic;
+        std::string map_info_topic;
+        std::string basic_sensors_info_topic;
+        std::string basic_sensors_values_topic;
+        std::string path_topic;
+
+        std::string vel_control_topic;
+        std::string goal_topic;
+
+        std::string imu_raw_data_topic;
+        std::string imu_raw_mag_data_topic;
+
+        bool raw_ladar_data;
+        bool ladar_data_clockwise;
+
+        bool pub_accumulate_odometry;
+
+        ServerParams();
+
+        void resetToDefault();
+        void setBy(const ros::NodeHandle& nhRos);
+    };
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_work_data.cpp

@@ -0,0 +1,18 @@
+
+#include "server_work_data.h"
+
+#include <boost/assert.hpp>
+
+namespace slamware_ros_sdk {
+
+    //////////////////////////////////////////////////////////////////////////
+
+    ServerWorkData::ServerWorkData()
+    	: syncMapRequested(true)
+    {
+    	//
+    }
+
+    //////////////////////////////////////////////////////////////////////////
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_work_data.h

@@ -0,0 +1,47 @@
+
+#pragma once
+
+#include <ros/ros.h>
+
+#include "server_map_holder.h"
+#include "msg_convert.h"
+
+#include <boost/shared_ptr.hpp>
+#include <boost/atomic.hpp>
+
+namespace slamware_ros_sdk {
+
+    struct ServerWorkData
+    {
+    public:
+        typedef rpos::features::impact_sensor::ImpactSensorInfo                 sensor_info_t;
+        typedef rpos::features::impact_sensor::ImpactSensorValue                sensor_value_t;
+        typedef std::map<int, sensor_info_t>                                    sensors_info_map_t;
+        typedef std::map<int, sensor_value_t>                                   sensors_values_map_t;
+
+        typedef slamware_ros_sdk::BasicSensorInfo                               ros_basic_sensor_info_t;
+        typedef std::map<int, ros_basic_sensor_info_t>                          ros_basic_sensor_info_map_t;
+
+    public:
+        RobotDeviceInfo robotDeviceInfo;
+
+        rpos::core::Pose robotPose;
+
+        boost::atomic<bool> syncMapRequested;
+        ServerMapHolder exploreMapHolder;
+
+        sensors_info_map_t sensorsInfo;
+        ros_basic_sensor_info_map_t rosBasicSensorsInfo;
+
+        RobotBasicState  robotState;
+    public:
+        ServerWorkData();
+
+    public:
+        static inline bool sfIsDigitalSensorValueImpact(float fVal) { return fVal < FLT_EPSILON; }
+    };
+
+    typedef boost::shared_ptr<ServerWorkData>               ServerWorkData_Ptr;
+    typedef boost::shared_ptr<const ServerWorkData>         ServerWorkData_ConstPtr;
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_worker_base.cpp

@@ -0,0 +1,75 @@
+
+#include "server_worker_base.h"
+#include "slamware_ros_sdk_server.h"
+
+#include <boost/assert.hpp>
+
+namespace slamware_ros_sdk {
+
+    ServerWorkerBase::ServerWorkerBase(SlamwareRosSdkServer* pRosSdkServer
+        , const std::string& wkName
+        , const boost::chrono::milliseconds& triggerInterval
+        )
+        : rosSdkServer_(pRosSdkServer)
+        , workerName_(wkName)
+        , isWorkLoopInitOk_(false)
+        , triggerInterval_(triggerInterval)
+    {
+        BOOST_ASSERT(nullptr != rosSdkServer_);
+    }
+
+    ServerWorkerBase::~ServerWorkerBase()
+    {
+        //
+    }
+
+    void ServerWorkerBase::resetOnWorkLoopBegin()
+    {
+        isWorkLoopInitOk_ = false;
+    }
+
+    bool ServerWorkerBase::reinitWorkLoop(slamware_platform_t& /*pltfm*/)
+    {
+        nextTimepointToTrigger_ = time_point_t();
+
+        isWorkLoopInitOk_ = true;
+        return isWorkLoopInitOk_;
+    }
+
+    void ServerWorkerBase::checkToPerform(slamware_platform_t& pltfm)
+    {
+        const auto tpNow = clock_t::now();
+        if (nextTimepointToTrigger_ <= tpNow)
+        {
+            nextTimepointToTrigger_ = tpNow + triggerInterval_;
+
+            doPerform(pltfm);
+        }
+    }
+    
+    ros::NodeHandle& ServerWorkerBase::rosNodeHandle() const
+    {
+        return rosSdkServer_->rosNodeHandle_();
+    }
+
+    const ServerParams& ServerWorkerBase::serverParams() const
+    {
+        return rosSdkServer_->serverParams_();
+    }
+
+    tf::TransformBroadcaster& ServerWorkerBase::tfBroadcaster() const
+    {
+        return rosSdkServer_->tfBroadcaster_();
+    }
+
+    ServerWorkData_ConstPtr ServerWorkerBase::workData() const
+    {
+        return rosSdkServer_->safeGetWorkData_();
+    }
+    
+    ServerWorkData_Ptr ServerWorkerBase::mutableWorkData()
+    {
+        return rosSdkServer_->safeGetMutableWorkData_();
+    }
+
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_worker_base.h

@@ -0,0 +1,67 @@
+
+#pragma once
+
+#include "server_params.h"
+#include "server_work_data.h"
+
+#include <tf/transform_broadcaster.h>
+
+#include <rpos/robot_platforms/slamware_core_platform.h>
+
+#include <boost/chrono.hpp>
+
+namespace slamware_ros_sdk {
+
+    class SlamwareRosSdkServer;
+
+    class ServerWorkerBase
+    {
+    public:
+        typedef boost::chrono::steady_clock         clock_t;
+        typedef clock_t::time_point                 time_point_t;
+
+        typedef rpos::robot_platforms::SlamwareCorePlatform     slamware_platform_t;
+
+    public:
+        ServerWorkerBase(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerWorkerBase();
+
+        const std::string& getWorkerName() const { return workerName_; }
+
+        time_point_t getNextTimepointToTrigger() const { return nextTimepointToTrigger_; }
+        
+        bool isWorkLoopInitOk() const { return isWorkLoopInitOk_; }
+        virtual void resetOnWorkLoopBegin();
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+        virtual void checkToPerform(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm) = 0;
+
+    protected:
+        SlamwareRosSdkServer* rosSdkServer() const { return rosSdkServer_; }
+
+        ros::NodeHandle& rosNodeHandle() const;
+        const ServerParams& serverParams() const;
+        tf::TransformBroadcaster& tfBroadcaster() const;
+
+        ServerWorkData_ConstPtr workData() const;
+        ServerWorkData_Ptr mutableWorkData();
+
+    private:
+        SlamwareRosSdkServer* rosSdkServer_;
+        std::string workerName_;
+
+    protected:
+        bool isWorkLoopInitOk_;
+        boost::chrono::milliseconds triggerInterval_;
+        time_point_t nextTimepointToTrigger_;
+    };
+
+    typedef boost::shared_ptr<ServerWorkerBase>         ServerWorkerBase_Ptr;
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/server_workers.h

@@ -0,0 +1,403 @@
+
+#pragma once
+
+#include "server_worker_base.h"
+
+#include <sensor_msgs/LaserScan.h>
+#include <std_srvs/Empty.h>
+#include <std_msgs/Float64.h>
+#include <nav_msgs/GetMap.h>
+#include <nav_msgs/Path.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include "std_msgs/String.h"
+
+namespace slamware_ros_sdk {
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerRobotDeviceInfoWorker: public ServerWorkerBase
+    {
+    public:
+        typedef ServerWorkerBase          super_t;
+
+    public:
+        ServerRobotDeviceInfoWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerRobotDeviceInfoWorker();
+
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubRobotDeviceInfo_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerOdometryWorker: public ServerWorkerBase
+    {
+    public:
+        ServerOdometryWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerOdometryWorker();
+
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubOdometry_;
+        bool isOdoPoseFeatureSupported_;
+        bool isSpeedFeatureSupported_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerRobotPoseWorker: public ServerWorkerBase
+    {
+    public:
+        ServerRobotPoseWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerRobotPoseWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubRobotPose_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerExploreMapUpdateWorker: public ServerWorkerBase
+    {
+    public:
+        typedef ServerWorkerBase          super_t;
+
+    public:
+        ServerExploreMapUpdateWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerExploreMapUpdateWorker();
+
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        rpos::features::location_provider::Map getMapByPltfm_(slamware_platform_t& pltfm, const rpos::core::RectangleF& area) const;
+
+        void requestReinitMap_();
+        bool checkToReinitMap_(slamware_platform_t& pltfm, const ServerWorkData_Ptr& wkDat);
+
+        bool checkRecvResolution_(float recvResolution, const ServerWorkData_Ptr& wkDat);
+
+        bool updateMapInCellIdxRect_(slamware_platform_t& pltfm
+            , const rpos::core::RectangleI& reqIdxRect
+            , const ServerWorkData_Ptr& wkDat
+            );
+
+        bool syncWholeMap_(const ServerParams& srvParams
+            , slamware_platform_t& pltfm
+            , const ServerWorkData_Ptr& wkDat
+            );
+
+        bool updateMapNearRobot_(const ServerParams& srvParams
+            , slamware_platform_t& pltfm
+            , const ServerWorkData_Ptr& wkDat
+            );
+
+    private:
+        bool shouldReinitMap_;
+        bool enableUpdateMap_;
+        time_point_t lastMapUpdateTime_; 
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerExploreMapPublishWorker: public ServerWorkerBase
+    {
+    public:
+        ServerExploreMapPublishWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerExploreMapPublishWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubMapDat_;
+        ros::Publisher pubMapInfo_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerLaserScanWorker: public ServerWorkerBase
+    {
+    public:
+        ServerLaserScanWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerLaserScanWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        void fillRangeMinMaxInMsg_(const std::vector<rpos::core::LaserPoint>& laserPoints
+            , sensor_msgs::LaserScan& msgScan
+            ) const;
+
+        float calcAngleInNegativePiToPi_(float angle) const;
+        
+        std::uint32_t calcCompensateDestIndexBySrcAngle_(float srcAngle
+            , bool isAnglesReverse
+            ) const;
+        bool isSrcAngleMoreCloseThanOldSrcAngle_(float srcAngle, float destAngle, float oldSrcAngle) const;
+        void compensateAndfillRangesInMsg_(const std::vector<rpos::core::LaserPoint>& laserPoints
+            , bool isClockwise
+            , bool isLaserDataReverse
+            , sensor_msgs::LaserScan& msgScan
+            ) const;
+        void compensateAndfillRangeInMsg_(const rpos::core::LaserPoint& laserPoint
+            , bool isClockwise
+            , sensor_msgs::LaserScan& msgScan
+            , std::vector<float>& tmpSrcAngles
+            ) const;
+        void fillOriginalRangesInMsg_(const std::vector<rpos::core::LaserPoint>& laserPoints
+            , bool isLaserDataReverse
+            , sensor_msgs::LaserScan& msgScan
+            ) const;
+        void fillOriginalRangeInMsg_(const rpos::core::LaserPoint& laserPoint
+            , int index
+            , sensor_msgs::LaserScan& msgScan
+            ) const;
+
+    private:
+        std::uint32_t compensatedAngleCnt_;
+        float absAngleIncrement_;
+
+        ros::Publisher pubLaserScan_;
+        std::uint64_t latestLidarStartTimestamp_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerBasicSensorsInfoWorker: public ServerWorkerBase
+    {
+    public:
+        ServerBasicSensorsInfoWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerBasicSensorsInfoWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        typedef ServerWorkData::sensors_info_map_t          sensors_info_map_t;
+
+        bool getSensorsInfo_(slamware_platform_t& pltfm, sensors_info_map_t& sensorsInfo) const;
+        bool isSensorsInfoAsTheSame_(const sensors_info_map_t& sensorsInfoA, const sensors_info_map_t& sensorsInfoB) const;
+
+    private:
+        ros::Publisher pubSensorsInfo_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerBasicSensorsValuesWorker: public ServerWorkerBase
+    {
+    public:
+        ServerBasicSensorsValuesWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerBasicSensorsValuesWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        typedef ServerWorkData::sensor_value_t                sensor_value_t;
+        typedef ServerWorkData::sensors_values_map_t          sensors_values_map_t;
+
+        bool getSensorsValues_(slamware_platform_t& pltfm, sensors_values_map_t& sensorsValues) const;
+
+        bool isSensorValueImpact_(const BasicSensorInfo& basicInfo, const sensor_value_t& sensorVal) const;
+
+    private:
+        ros::Publisher pubSensorsValues_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerPlanPathWorker: public ServerWorkerBase
+    {
+    public:
+        ServerPlanPathWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerPlanPathWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubPlanPath_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerRobotBasicStateWorker: public ServerWorkerBase
+    {
+    public:
+        ServerRobotBasicStateWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerRobotBasicStateWorker();
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubRobotBasicState_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerArtifactLinesWorker: public ServerWorkerBase
+    {
+    public:
+        typedef ServerWorkerBase          super_t;
+
+        struct params_t
+        {
+            ArtifactUsage usage;
+            std::string topic;
+            std::uint32_t queueSize;
+            bool latch;
+
+            params_t()
+                : queueSize(1u)
+                , latch(true)
+            {
+                usage.usage = ArtifactUsage::UNKNOWN;
+            }
+        };
+
+    public:
+        ServerArtifactLinesWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            , const params_t& rcParams
+            );
+        virtual ~ServerArtifactLinesWorker();
+
+        virtual void resetOnWorkLoopBegin();
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        params_t params_;
+        rpos::features::artifact_provider::ArtifactUsage sltcUsage_;
+
+        bool isFeatureSupported_;
+        ros::Publisher pubArtifactLines_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+
+    class ServerReadEventsWorker: public ServerWorkerBase
+    {
+    public:
+        typedef ServerWorkerBase          super_t;
+	
+    public:
+        ServerReadEventsWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerReadEventsWorker();
+
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        boost::shared_ptr<rpos::robot_platforms::objects::SystemEventProvider> systemEventProvider_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+    
+    class ServerImuRawDataWorker: public ServerWorkerBase
+    {
+    public:
+        typedef ServerWorkerBase          super_t;
+	
+    public:
+        ServerImuRawDataWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~ServerImuRawDataWorker();
+
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        bool isFeatureSupported_;
+        ros::Publisher pubImuRawData_;
+        ros::Publisher pubImuRawMagData_;
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+    class RosConnectWorker: public ServerWorkerBase
+    {
+    public:
+        typedef ServerWorkerBase super_t;
+        
+    public:
+        RosConnectWorker(SlamwareRosSdkServer* pRosSdkServer
+            , const std::string& wkName
+            , const boost::chrono::milliseconds& triggerInterval
+            );
+        virtual ~RosConnectWorker();
+
+        virtual bool reinitWorkLoop(slamware_platform_t& pltfm);
+
+    protected:
+        virtual void doPerform(slamware_platform_t& pltfm);
+
+    private:
+        ros::Publisher pubRosConnect_;
+        
+    };
+
+    //////////////////////////////////////////////////////////////////////////
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/slamware_ros_sdk_server.h

@@ -0,0 +1,227 @@
+/**
+ kint.zhao  huasheng_zyh@163.com
+   2017.0721
+
+  modified by yun.li@slamtec.com, 2019.
+*/
+
+#pragma once
+
+#include "server_workers.h"
+
+#include <slamware_ros_sdk/SyncGetStcm.h>
+#include <slamware_ros_sdk/SyncSetStcm.h>
+
+#include <message_filters/subscriber.h>
+
+#include <boost/atomic.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/lock_guard.hpp>
+
+#include <boost/function.hpp>
+
+namespace slamware_ros_sdk {
+
+    class SlamwareRosSdkServer
+    {
+    private:
+        friend class ServerWorkerBase;
+
+    public:
+        SlamwareRosSdkServer();
+        ~SlamwareRosSdkServer();
+
+        bool startRun(std::string& errMsg);
+        void requestStop();
+        void waitUntilStopped(); // not thread-safe
+
+    public:
+        void requestSyncMap();
+
+    private:
+        enum ServerState
+        {
+            ServerStateNotInit
+            , ServerStateRunning
+            , ServerStateStopped
+        };
+
+        typedef rpos::robot_platforms::SlamwareCorePlatform     slamware_platform_t;
+
+        template<class MsgT>
+        struct msg_cb_help_t
+        {
+            typedef MsgT                                    msg_t;
+            typedef typename msg_t::ConstPtr                const_msg_shared_ptr;
+            typedef void (SlamwareRosSdkServer::*msg_cb_perform_fun_t)(slamware_platform_t&, const const_msg_shared_ptr&);
+            typedef boost::function< void(const const_msg_shared_ptr&) >        ros_cb_fun_t; // callback function for ROS.
+        };
+
+        template<class SrvMsgT>
+        struct srv_cb_help_t
+        {
+            typedef SrvMsgT                             srv_msg_t;
+            typedef typename srv_msg_t::Request         request_t;
+            typedef typename srv_msg_t::Response        response_t;
+            typedef bool (SlamwareRosSdkServer::*srv_cb_perform_fun_t)(slamware_platform_t&, request_t&, response_t&);
+            typedef boost::function< bool(request_t&, response_t&) >            ros_cb_fun_t; // callback function for ROS.
+        };
+
+    private:
+        static boost::chrono::milliseconds sfConvFloatSecToBoostMs_(float fSec);
+
+        bool isRunning_() const { return ServerStateRunning == state_.load(); }
+        bool shouldContinueRunning_() const;
+
+        const ros::NodeHandle& rosNodeHandle_() const { return nh_; }
+        ros::NodeHandle& rosNodeHandle_() { return nh_; }
+
+        const ServerParams& serverParams_() const { return params_; }
+        
+        const tf::TransformBroadcaster& tfBroadcaster_() const { return tfBrdcstr_; }
+        tf::TransformBroadcaster& tfBroadcaster_() { return tfBrdcstr_; }
+
+        ServerWorkData_ConstPtr safeGetWorkData_() const;
+        ServerWorkData_Ptr safeGetMutableWorkData_();
+
+        bool safeIsSlamwarePlatformConnected_() const;
+        slamware_platform_t safeGetSlamwarePlatform_() const;
+        void safeSetSlamwarePlatform_(const slamware_platform_t& pltfm);
+        void safeReleaseSlamwarePlatform_();
+        slamware_platform_t connectSlamwarePlatform_(const std::string& ip, int port) const;
+        void disconnectSlamwarePlatform_(slamware_platform_t& pltfm) const;
+
+        bool init_(std::string& errMsg);
+        void cleanup_();
+
+        void workThreadFun_();
+
+        void roughSleepWait_(std::uint32_t maxSleepMs, std::uint32_t onceSleepMs);
+        void loopTryConnectToSlamwarePlatform_();
+
+        bool reinitWorkLoop_(slamware_platform_t& pltfm);
+        void loopWork_();
+
+        //////////////////////////////////////////////////////////////////////////
+        // subscribed messages
+        //////////////////////////////////////////////////////////////////////////
+
+        template<class MsgT>
+        void msgCbWrapperFun_T_(typename msg_cb_help_t<MsgT>::msg_cb_perform_fun_t mfpCbPerform
+            , const std::string& msgTopic
+            , const typename msg_cb_help_t<MsgT>::const_msg_shared_ptr & msg
+            );
+        template<class MsgT>
+        ros::Subscriber subscribe_T_(const std::string& msgTopic
+            , std::uint32_t queueSize
+            , typename msg_cb_help_t<MsgT>::msg_cb_perform_fun_t mfpCbPerform
+            );
+
+        void msgCbRobotControl_(slamware_platform_t& pltfm, const geometry_msgs::Twist::ConstPtr& msg);
+        void msgCbRobotControlNoLimit_(slamware_platform_t& pltfm, const geometry_msgs::Twist::ConstPtr& msg);
+        void msgCbMoveToGoal_(slamware_platform_t& pltfm, const geometry_msgs::PoseStamped::ConstPtr& msg);
+        
+        void msgCbSyncMap_(slamware_platform_t& pltfm, const SyncMapRequest::ConstPtr& msg);
+        void msgCbSetPose_(slamware_platform_t& pltfm, const geometry_msgs::Pose::ConstPtr& msg);
+
+        void msgCbRecoverLocalization_(slamware_platform_t& pltfm, const RecoverLocalizationRequest::ConstPtr& msg);
+        void msgCbClearMap_(slamware_platform_t& pltfm, const ClearMapRequest::ConstPtr& msg);
+        void msgCbSetMapUpdate_(slamware_platform_t& pltfm, const SetMapUpdateRequest::ConstPtr& msg);
+        void msgCbSetMapLocalization_(slamware_platform_t& pltfm, const SetMapLocalizationRequest::ConstPtr& msg);
+
+        void msgCbMoveByDirection_(slamware_platform_t& pltfm, const MoveByDirectionRequest::ConstPtr& msg);
+        void msgCbMoveByTheta_(slamware_platform_t& pltfm, const MoveByThetaRequest::ConstPtr& msg);
+        void msgCbMoveTo_(slamware_platform_t& pltfm, const MoveToRequest::ConstPtr& msg);
+        void msgCbMoveToLocations_(slamware_platform_t& pltfm, const MoveToLocationsRequest::ConstPtr& msg);
+        void msgCbRotateTo_(slamware_platform_t& pltfm, const RotateToRequest::ConstPtr& msg);
+        void msgCbRotate_(slamware_platform_t& pltfm, const RotateRequest::ConstPtr& msg);
+
+        void msgCbGoHome_(slamware_platform_t& pltfm, const GoHomeRequest::ConstPtr& msg);
+        void msgCbCancelAction_(slamware_platform_t& pltfm, const CancelActionRequest::ConstPtr& msg);
+
+        void msgCbAddLine_(slamware_platform_t& pltfm, const AddLineRequest::ConstPtr& msg);
+        void msgCbAddLines_(slamware_platform_t& pltfm, const AddLinesRequest::ConstPtr& msg);
+        void msgCbRemoveLine_(slamware_platform_t& pltfm, const RemoveLineRequest::ConstPtr& msg);
+        void msgCbClearLines_(slamware_platform_t& pltfm, const ClearLinesRequest::ConstPtr& msg);
+        void msgCbMoveLine_(slamware_platform_t& pltfm, const MoveLineRequest::ConstPtr& msg);
+        void msgCbMoveLines_(slamware_platform_t& pltfm, const MoveLinesRequest::ConstPtr& msg);
+
+        //////////////////////////////////////////////////////////////////////////
+        // advertised services
+        //////////////////////////////////////////////////////////////////////////
+
+        template<class SrvMsgT>
+        bool srvCbWrapperFun_T_(typename srv_cb_help_t<SrvMsgT>::srv_cb_perform_fun_t mfpCbPerform
+            , const std::string& srvMsgTopic
+            , typename srv_cb_help_t<SrvMsgT>::request_t& req
+            , typename srv_cb_help_t<SrvMsgT>::response_t& resp
+            );
+        template<class SrvMsgT>
+        ros::ServiceServer advertiseService_T_(const std::string& srvMsgTopic
+            , typename srv_cb_help_t<SrvMsgT>::srv_cb_perform_fun_t mfpCbPerform
+            );
+
+        bool srvCbSyncGetStcm_(slamware_platform_t& pltfm, SyncGetStcm::Request& req, SyncGetStcm::Response& resp);
+        bool srvCbSyncSetStcm_(slamware_platform_t& pltfm, SyncSetStcm::Request& req, SyncSetStcm::Response& resp);
+
+        //////////////////////////////////////////////////////////////////////////
+
+    private:
+        boost::atomic<ServerState> state_;
+        boost::atomic<bool> isStopRequested_;
+        
+        ros::NodeHandle nh_;
+        ServerParams params_;
+
+        tf::TransformBroadcaster tfBrdcstr_;
+
+        mutable boost::mutex workDatLock_;
+        ServerWorkData_Ptr workDat_;
+
+        std::vector<ServerWorkerBase_Ptr> serverWorkers_;
+
+        // subscriptions
+        ros::Subscriber subRobotControl_;
+        ros::Subscriber subRobotControlNoLimit_;
+        ros::Subscriber subMoveToGoal_;
+        
+        ros::Subscriber subSyncMap_;
+        ros::Subscriber subSetPose_;
+
+        ros::Subscriber subRecoverLocalization_;
+        ros::Subscriber subClearMap_;
+        ros::Subscriber subSetMapUpdate_;
+        ros::Subscriber subSetMapLocalization_;
+
+        ros::Subscriber subMoveByDirection_;
+        ros::Subscriber subMoveByTheta_;
+        ros::Subscriber subMoveTo_;
+        ros::Subscriber subMoveToLocations_;
+        ros::Subscriber subRotateTo_;
+        ros::Subscriber subRotate_;
+
+        ros::Subscriber subGoHome_;
+        ros::Subscriber subCancelAction_;
+
+        ros::Subscriber subAddLine_;
+        ros::Subscriber subAddLines_;
+        ros::Subscriber subRemoveLine_;
+        ros::Subscriber subClearLines_;
+        ros::Subscriber subMoveLine_;
+        ros::Subscriber subMoveLines_;
+        
+        rpos::actions::VelocityControlMoveAction velocityControllAction_;
+        
+        // services
+        ros::ServiceServer srvSyncGetStcm_;
+        ros::ServiceServer srvSyncSetStcm_;
+
+        boost::thread workThread_;
+
+        mutable boost::mutex slamwarePltfmLock_;
+        slamware_platform_t slamwarePltfm_;
+
+    };
+    
+}

src/catkin_dog/src/slamware_ros_sdk_linux-x86_64-gcc9/src/slamware_ros_sdk/src/server/slamware_ros_sdk_server_node.cpp

@@ -0,0 +1,30 @@
+/**
+ kint.zhao  huasheng_zyh@163.com
+   2017.0721
+
+  modified by yun.li@slamtec.com, 2019.
+*/
+
+#include <ros/ros.h>
+
+#include  "slamware_ros_sdk_server.h"
+
+int main(int argc, char **argv)
+{
+  std::string errMsg;
+  ros::init(argc, argv, "slamware_ros_sdk_server_node");
+
+  slamware_ros_sdk::SlamwareRosSdkServer rosSdkServer;
+  if (!rosSdkServer.startRun(errMsg))
+  {
+    ROS_ERROR("failed to start slamware ros sdk server: %s.", errMsg.c_str());
+    return -1;
+  }
+
+  ros::spin();
+
+  rosSdkServer.requestStop();
+  rosSdkServer.waitUntilStopped();
+
+  return 0;
+}