123
1 week ago
commit
1bfbbd5122
@ -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"
|
||||
}
|
||||
}
|
||||
Binary file not shown.
@ -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"
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
@ -0,0 +1 @@
|
||||
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
|
||||
@ -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}
|
||||
)
|
||||
@ -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.
|
||||
@ -0,0 +1 @@
|
||||
# slamware_ros_sdk
|
||||
@ -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"
|
||||
|
||||
@ -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"}
|
||||
@ -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"}
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -0,0 +1,8 @@
|
||||
|
||||
int8 UNKNOWN=-1
|
||||
int8 FORWARD=0
|
||||
int8 BACKWARD=1
|
||||
int8 TURNRIGHT=2
|
||||
int8 TURNLEFT=3
|
||||
|
||||
int8 direction
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
ArtifactUsage usage
|
||||
Line2DFlt32 line
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
ArtifactUsage usage
|
||||
Line2DFlt32[] lines
|
||||
@ -0,0 +1,6 @@
|
||||
|
||||
int8 UNKNOWN=-1
|
||||
int8 VIRTUAL_WALL=0
|
||||
int8 VIRTUAL_TRACK=1
|
||||
|
||||
int8 usage
|
||||
@ -0,0 +1,6 @@
|
||||
|
||||
int32 id
|
||||
SensorType sensor_type
|
||||
ImpactType impact_type
|
||||
geometry_msgs/Pose install_pose
|
||||
float32 refresh_freq
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
BasicSensorInfo[] sensors_info
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_in_impact # whether this sensor is in impact status
|
||||
float32 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
BasicSensorInfo info
|
||||
BasicSensorValue value
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
BasicSensorValueData[] values_data
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
# currently nothing in this message
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
ArtifactUsage usage
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
MapKind kind
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
# currently nothing in this message
|
||||
@ -0,0 +1,6 @@
|
||||
|
||||
int8 UNKNOWN=-1
|
||||
int8 DIGITAL=0
|
||||
int8 ANALOG=1
|
||||
|
||||
int8 type
|
||||
@ -0,0 +1,4 @@
|
||||
|
||||
uint32 id
|
||||
Vec2DFlt32 start
|
||||
Vec2DFlt32 end
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
Line2DFlt32[] lines
|
||||
@ -0,0 +1,7 @@
|
||||
|
||||
int8 UNKNOWN=-1
|
||||
int8 NO_MOVE=0
|
||||
int8 ROTATE_ONLY=1
|
||||
int8 ANY=2
|
||||
|
||||
int8 type
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
OptionalInt32 max_time_ms
|
||||
OptionalLocalizationMovement mvmt_type
|
||||
@ -0,0 +1,9 @@
|
||||
|
||||
int8 UNKNOWN=-1
|
||||
int8 EXPLORERMAP=0
|
||||
int8 SWEEPERMAP=1
|
||||
int8 UWBMAP=2
|
||||
int8 SLAMMAP=3
|
||||
int8 LOCALSLAMMAP=4
|
||||
|
||||
int8 kind
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
ActionDirection direction
|
||||
MoveOptions options
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
float32 theta
|
||||
MoveOptions options
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
ArtifactUsage usage
|
||||
Line2DFlt32 line
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
ArtifactUsage usage
|
||||
Line2DFlt32[] lines
|
||||
@ -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
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
MoveOptionFlag opt_flags
|
||||
OptionalFlt64 speed_ratio
|
||||
@ -0,0 +1,4 @@
|
||||
|
||||
geometry_msgs/Point[] locations
|
||||
MoveOptions options
|
||||
float32 yaw
|
||||
@ -0,0 +1,4 @@
|
||||
|
||||
geometry_msgs/Point location
|
||||
MoveOptions options
|
||||
float32 yaw
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
bool value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
float32 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
float64 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
int16 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
int32 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
int64 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
int8 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
LocalizationMovement value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
uint16 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
uint32 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
uint64 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool is_valid
|
||||
uint8 value
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
RectFlt32 area
|
||||
LocalizationOptions options
|
||||
@ -0,0 +1,5 @@
|
||||
|
||||
float32 x # x of position
|
||||
float32 y # y of position
|
||||
float32 w # width
|
||||
float32 h # height
|
||||
@ -0,0 +1,5 @@
|
||||
|
||||
int32 x # x of position
|
||||
int32 y # y of position
|
||||
int32 w # width
|
||||
int32 h # height
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
ArtifactUsage usage
|
||||
uint32 id
|
||||
@ -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
|
||||
@ -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
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
geometry_msgs/Quaternion rotation
|
||||
MoveOptions options
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
geometry_msgs/Quaternion orientation
|
||||
MoveOptions options
|
||||
@ -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
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
bool enabled
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
bool enabled
|
||||
MapKind kind
|
||||
@ -0,0 +1,2 @@
|
||||
|
||||
# currently nothing in this message
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
float32 x
|
||||
float32 y
|
||||
@ -0,0 +1,3 @@
|
||||
|
||||
int32 x
|
||||
int32 y
|
||||
@ -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>
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
}
|
||||
@ -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]);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
}
|
||||
@ -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);
|
||||
};
|
||||
|
||||
}
|
||||
@ -0,0 +1,18 @@
|
||||
|
||||
#include "server_work_data.h"
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
namespace slamware_ros_sdk {
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
ServerWorkData::ServerWorkData()
|
||||
: syncMapRequested(true)
|
||||
{
|
||||
//
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
}
|
||||
@ -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;
|
||||
|
||||
}
|
||||
@ -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_();
|
||||
}
|
||||
|
||||
}
|
||||
@ -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;
|
||||
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@ -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_;
|
||||
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@ -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_;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue