Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixes for Windows #439

Open
wants to merge 4 commits into
base: foxy-devel
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -136,33 +136,54 @@ target_link_libraries(ceres_solver_plugin ${CERES_LIBRARIES}
rosidl_target_interfaces(ceres_solver_plugin ${PROJECT_NAME} "rosidl_typesupport_cpp")
pluginlib_export_plugin_description_file(slam_toolbox solver_plugins.xml)

if(MSVC)
add_compile_definitions(_USE_MATH_DEFINES)
endif()

#### Tool lib for mapping
add_library(toolbox_common src/slam_toolbox_common.cpp src/map_saver.cpp src/loop_closure_assistant.cpp src/laser_utils.cpp src/slam_mapper.cpp)
ament_target_dependencies(toolbox_common
${dependencies}
)
target_link_libraries(toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
set_target_properties(toolbox_common PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
rosidl_target_interfaces(toolbox_common ${PROJECT_NAME} "rosidl_typesupport_cpp")

#### Mapping executibles
add_library(async_slam_toolbox src/slam_toolbox_async.cpp)
set_target_properties(async_slam_toolbox PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
add_executable(async_slam_toolbox_node src/slam_toolbox_async_node.cpp)
if(MSVC)
target_compile_options(async_slam_toolbox_node PRIVATE "/F 40000000")
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not familiar with this /F 40000000, can you explain me this?

endif()
target_link_libraries(async_slam_toolbox_node async_slam_toolbox)

add_library(sync_slam_toolbox src/slam_toolbox_sync.cpp)
set_target_properties(sync_slam_toolbox PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
target_link_libraries(sync_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
add_executable(sync_slam_toolbox_node src/slam_toolbox_sync_node.cpp)
if(MSVC)
target_compile_options(sync_slam_toolbox_node PRIVATE "/F 40000000")
endif()
target_link_libraries(sync_slam_toolbox_node sync_slam_toolbox)

add_library(localization_slam_toolbox src/slam_toolbox_localization.cpp)
set_target_properties(localization_slam_toolbox PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
target_link_libraries(localization_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
add_executable(localization_slam_toolbox_node src/slam_toolbox_localization_node.cpp)
if(MSVC)
target_compile_options(localization_slam_toolbox_node PRIVATE "/F 40000000")
endif()
target_link_libraries(localization_slam_toolbox_node localization_slam_toolbox)

add_library(lifelong_slam_toolbox src/experimental/slam_toolbox_lifelong.cpp)
set_target_properties(lifelong_slam_toolbox PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${Boost_LIBRARIES})
add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp)
if(MSVC)
target_compile_options(lifelong_slam_toolbox_node PRIVATE "/F 40000000")
endif()
target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox)

#### Merging maps tool
Expand Down
2 changes: 2 additions & 0 deletions include/slam_toolbox/merge_maps_kinematic.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,9 @@

#include <thread>
#include <sys/stat.h>
#ifndef _WIN32
#include <unistd.h>
#endif
#include <string>
#include <map>
#include <memory>
Expand Down
2 changes: 2 additions & 0 deletions include/slam_toolbox/slam_toolbox_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,9 @@
#ifndef SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_
#define SLAM_TOOLBOX__SLAM_TOOLBOX_COMMON_HPP_

#ifndef _WIN32
#include <sys/resource.h>
#endif
#include <boost/thread.hpp>
#include <string>
#include <map>
Expand Down
5 changes: 4 additions & 1 deletion lib/karto_sdk/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,9 @@ if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0")
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0")
endif()
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")

find_package(ament_cmake REQUIRED)
Expand All @@ -29,6 +31,7 @@ add_definitions(${EIGEN3_DEFINITIONS})

include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp)
target_compile_definitions(kartoSlamToolbox PRIVATE KARTO_DYNAMIC)
ament_target_dependencies(kartoSlamToolbox ${dependencies})
target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES})

Expand Down
8 changes: 4 additions & 4 deletions lib/karto_sdk/src/Mapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -405,7 +405,7 @@ void MapperSensorManager::SetRunningScanBufferSize(kt_int32u rScanBufferSize)
m_RunningBufferMaximumSize = rScanBufferSize;

std::vector<Name> names = GetSensorNames();
for (uint i = 0; i != names.size(); i++) {
for (unsigned int i = 0; i != names.size(); i++) {
GetScanManager(names[i])->SetRunningScanBufferSize(rScanBufferSize);
}
}
Expand All @@ -415,7 +415,7 @@ void MapperSensorManager::SetRunningScanBufferMaximumDistance(kt_double rScanBuf
m_RunningBufferMaximumDistance = rScanBufferMaxDistance;

std::vector<Name> names = GetSensorNames();
for (uint i = 0; i != names.size(); i++) {
for (unsigned int i = 0; i != names.size(); i++) {
GetScanManager(names[i])->SetRunningScanBufferMaximumDistance(rScanBufferMaxDistance);
}
}
Expand Down Expand Up @@ -1867,7 +1867,7 @@ std::vector<Vertex<LocalizedRangeScan> *> MapperGraph::FindNearByVertices(

std::vector<Vertex<LocalizedRangeScan> *> rtn_vertices;
rtn_vertices.reserve(ret_matches.size());
for (uint i = 0; i != ret_matches.size(); i++) {
for (unsigned int i = 0; i != ret_matches.size(); i++) {
rtn_vertices.push_back(vertices_to_search[ret_matches[i].first]);
}
return rtn_vertices;
Expand Down Expand Up @@ -2941,7 +2941,7 @@ void Mapper::ClearLocalizationBuffer()
}

std::vector<Name> names = m_pMapperSensorManager->GetSensorNames();
for (uint i = 0; i != names.size(); i++)
for (unsigned int i = 0; i != names.size(); i++)
{
m_pMapperSensorManager->ClearRunningScans(names[i]);
m_pMapperSensorManager->ClearLastScan(names[i]);
Expand Down
1 change: 1 addition & 0 deletions rviz_plugin/slam_toolbox_rviz_plugin.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include <QLabel>
#include <QFrame>
#include <QRadioButton>
#undef NO_ERROR
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
// STL
#include <thread>
#include <chrono>
Expand Down
4 changes: 4 additions & 0 deletions src/experimental/slam_toolbox_lifelong_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,9 @@ int main(int argc, char ** argv)
auto temp_node = std::make_shared<rclcpp::Node>("slam_toolbox");
temp_node->declare_parameter("stack_size_to_use");
if (temp_node->get_parameter("stack_size_to_use", stack_size)) {
#ifdef _WIN32
RCLCPP_WARN(temp_node->get_logger(), "Can't dynamically change stack size on Windows. Node using stack size 40000000");
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there another API to change the stack size then? This is kind of an important element for serialization / deserialization of large maps

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From my research, I wasn't able to find a way to dynamically change the stack size of the program at runtime on Windows. However, I was able to find a way to change the stack size when a thread is created here.

Do you think it would be possible to utilize this functionality instead? Or, instead of serializing and deserializing maps on the stack, is it possible to use dynamic memory, just for Windows?

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It uses boost serialization, so that's totally out of our control where that memory is located for serialization.

So are you suggesting we serialize/deserialize in another thread that in instantiation we set the max size on? That would need some testing on your end to validate, but I could approve that.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Instead of putting only the serialization/deserialization on a separate thread, it might be easier to just put the entire node in a separate thread. Do you think that would be fine too?

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would not like to do that, that would add an extra thread for everyone for all of run-time, versus only an extra thread on serialization/deserialization processes

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see, that makes sense! Would I just have to modify SlamToolbox::serializePoseGraphCallback and SlamToolbox::loadSerializedPoseGraph? Are there any other functions that do the serialization and deserialization?

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup, that's right

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you have any large serialized maps I can use for testing? In the README, I saw a mention about a map from Circuit Launch. Would it be possible to give me access to that or another map?

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

None that I can share publicly

#else
RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
Expand All @@ -37,6 +40,7 @@ int main(int argc, char ** argv)
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
#endif
}
}

Expand Down
4 changes: 4 additions & 0 deletions src/merge_maps_kinematic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,11 @@ bool MergeMapsKinematic::addSubmapCallback(
"/map_" + std::to_string(num_submaps_), rclcpp::QoS(1)));
sstmS_.push_back(this->create_publisher<nav_msgs::msg::MapMetaData>(
"/map_metadata_" + std::to_string(num_submaps_), rclcpp::QoS(1)));
#ifdef _WIN32
Sleep(1000);
#else
sleep(1.0);
#endif

nav_msgs::srv::GetMap::Response map;
nav_msgs::msg::OccupancyGrid & og = map.map;
Expand Down
4 changes: 4 additions & 0 deletions src/slam_toolbox_async_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,9 @@ int main(int argc, char ** argv)
auto temp_node = std::make_shared<rclcpp::Node>("slam_toolbox");
temp_node->declare_parameter("stack_size_to_use");
if (temp_node->get_parameter("stack_size_to_use", stack_size)) {
#ifdef _WIN32
RCLCPP_WARN(temp_node->get_logger(), "Can't dynamically change stack size on Windows. Using stack size 40000000");
#else
RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
Expand All @@ -37,6 +40,7 @@ int main(int argc, char ** argv)
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
#endif
}
}

Expand Down
4 changes: 4 additions & 0 deletions src/slam_toolbox_localization_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,9 @@ int main(int argc, char ** argv)
auto temp_node = std::make_shared<rclcpp::Node>("slam_toolbox");
temp_node->declare_parameter("stack_size_to_use");
if (temp_node->get_parameter("stack_size_to_use", stack_size)) {
#ifdef _WIN32
RCLCPP_WARN(temp_node->get_logger(), "Can't dynamically change stack size on Windows. Node using stack size 40000000");
#else
RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
Expand All @@ -37,6 +40,7 @@ int main(int argc, char ** argv)
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
#endif
}
}

Expand Down
4 changes: 4 additions & 0 deletions src/slam_toolbox_sync_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,9 @@ int main(int argc, char ** argv)
auto temp_node = std::make_shared<rclcpp::Node>("slam_toolbox");
temp_node->declare_parameter("stack_size_to_use");
if (temp_node->get_parameter("stack_size_to_use", stack_size)) {
#ifdef _WIN32
RCLCPP_WARN(temp_node->get_logger(), "Can't dynamically change stack size on Windows. Node using stack size 40000000");
#else
RCLCPP_INFO(temp_node->get_logger(), "Node using stack size %i", (int)stack_size);
const rlim_t max_stack_size = stack_size;
struct rlimit stack_limit;
Expand All @@ -37,6 +40,7 @@ int main(int argc, char ** argv)
stack_limit.rlim_cur = stack_size;
}
setrlimit(RLIMIT_STACK, &stack_limit);
#endif
}
}

Expand Down