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 #2595

Merged
merged 6 commits into from
Oct 14, 2021
Merged
Show file tree
Hide file tree
Changes from 5 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
7 changes: 7 additions & 0 deletions nav2_map_server/src/map_io.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,13 @@ char * dirname(char * path)
return path;
}

/* Replace all "\" with "/" */
char * c = path;
while (*c != '\0') {
if (*c == '\\') *c = '/';
++c;
}

/* Find last '/'. */
last_slash = path != NULL ? strrchr(path, '/') : NULL;

Expand Down
1 change: 1 addition & 0 deletions nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <QtWidgets>
#include <QBasicTimer>
#undef NO_ERROR

#include <memory>
#include <string>
Expand Down
2 changes: 2 additions & 0 deletions nav2_util/src/dump_params.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,9 @@

#include <iomanip>
#include <iostream>
#ifndef _WIN32
#include <libgen.h>
#endif
#include <memory>
#include <sstream>
#include <string>
Expand Down
12 changes: 12 additions & 0 deletions nav2_util/test/test_lifecycle_cli_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,10 @@
#include "nav2_util/node_thread.hpp"
#include "rclcpp/rclcpp.hpp"

#ifdef _WIN32
#include <windows.h>
#endif

class DummyNode : public nav2_util::LifecycleNode
{
public:
Expand Down Expand Up @@ -80,7 +84,11 @@ TEST(LifeycleCLI, fails_no_node_name)
Handle handle;
auto rc = system("ros2 run nav2_util lifecycle_bringup");
(void)rc;
#ifdef _WIN32
Sleep(1000);
#else
sleep(1);
#endif
// check node didn't mode
EXPECT_EQ(handle.node->activated, false);
SUCCEED();
Expand All @@ -90,7 +98,11 @@ TEST(LifeycleCLI, succeeds_node_name)
{
Handle handle;
auto rc = system("ros2 run nav2_util lifecycle_bringup nav2_test_cli");
#ifdef _WIN32
Sleep(3000);
#else
sleep(3);
#endif
// check node moved
(void)rc;
EXPECT_EQ(handle.node->activated, true);
Expand Down
7 changes: 6 additions & 1 deletion smac_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,9 @@ endif()

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")

add_compile_options(-O3 -Wextra -Wdeprecated -fPIC)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-O3 -Wextra -Wdeprecated -fPIC)
endif()

include_directories(
include
Expand All @@ -41,6 +43,9 @@ include_directories(
)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
if(MSVC)
add_compile_definitions(_USE_MATH_DEFINES)
endif()

set(library_name smac_planner)

Expand Down
9 changes: 7 additions & 2 deletions smac_planner/include/smac_planner/smoother.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -105,8 +105,13 @@ class Smoother
{
_options.max_solver_time_in_seconds = params.max_time;

#ifdef _MSC_VER
std::vector<double> parametersVec(path.size() * 2);
double* parameters = parametersVec.data();
Ace314159 marked this conversation as resolved.
Show resolved Hide resolved
#else
double parameters[path.size() * 2]; // NOLINT
for (uint i = 0; i != path.size(); i++) {
#endif
for (unsigned int i = 0; i != path.size(); i++) {
parameters[2 * i] = path[i][0];
parameters[2 * i + 1] = path[i][1];
}
Expand All @@ -123,7 +128,7 @@ class Smoother
return false;
}

for (uint i = 0; i != path.size(); i++) {
for (unsigned int i = 0; i != path.size(); i++) {
path[i][0] = parameters[2 * i];
path[i][1] = parameters[2 * i + 1];
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
Eigen::Vector2d xi;
Eigen::Vector2d xi_p1;
Eigen::Vector2d xi_m1;
uint x_index, y_index;
unsigned int x_index, y_index;
cost[0] = 0.0;
double cost_raw = 0.0;
double grad_x_raw = 0.0;
Expand Down
8 changes: 4 additions & 4 deletions smac_planner/src/costmap_downsampler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,8 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(
}

// Assign costs
for (uint i = 0; i < _downsampled_size_x; ++i) {
for (uint j = 0; j < _downsampled_size_y; ++j) {
for (unsigned int i = 0; i < _downsampled_size_x; ++i) {
for (unsigned int j = 0; j < _downsampled_size_y; ++j) {
setCostOfCell(i, j);
}
}
Expand Down Expand Up @@ -122,12 +122,12 @@ void CostmapDownsampler::setCostOfCell(
unsigned int x_offset = new_mx * _downsampling_factor;
unsigned int y_offset = new_my * _downsampling_factor;

for (uint i = 0; i < _downsampling_factor; ++i) {
for (unsigned int i = 0; i < _downsampling_factor; ++i) {
mx = x_offset + i;
if (mx >= _size_x) {
continue;
}
for (uint j = 0; j < _downsampling_factor; ++j) {
for (unsigned int j = 0; j < _downsampling_factor; ++j) {
my = y_offset + j;
if (my >= _size_y) {
continue;
Expand Down
2 changes: 1 addition & 1 deletion smac_planner/src/smac_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -340,7 +340,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(

// populate final path
// TODO(stevemacenski): set orientation to tangent of path
for (uint i = 0; i != path_world.size(); i++) {
for (unsigned int i = 0; i != path_world.size(); i++) {
pose.pose.position.x = path_world[i][0];
pose.pose.position.y = path_world[i][1];
plan.poses[i] = pose;
Expand Down
2 changes: 1 addition & 1 deletion smac_planner/src/smac_planner_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -303,7 +303,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
removeHook(path_world);

// populate final path
for (uint i = 0; i != path_world.size(); i++) {
for (unsigned int i = 0; i != path_world.size(); i++) {
pose.pose.position.x = path_world[i][0];
pose.pose.position.y = path_world[i][1];
plan.poses[i] = pose;
Expand Down