Skip to content

Commit

Permalink
Fix header namespaces (#408)
Browse files Browse the repository at this point in the history
* Fix include header path
* Put everything inside package namespace
* Install planner_cspace public headers
  • Loading branch information
at-wat committed Jan 2, 2020
1 parent 09669a3 commit 2f7c632
Show file tree
Hide file tree
Showing 35 changed files with 178 additions and 33 deletions.
7 changes: 4 additions & 3 deletions costmap_cspace/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,9 @@ include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(costmap_3d src/costmap_3d.cpp src/costmap_3d_layers.cpp)
target_link_libraries(costmap_3d ${catkin_LIBRARIES})
add_dependencies(costmap_3d ${catkin_EXPORTED_TARGETS})
set_property(TARGET costmap_3d PROPERTY PUBLIC_HEADER
include/costmap_cspace/pointcloud_accumulator.h
)

add_executable(laserscan_to_map src/laserscan_to_map.cpp)
target_link_libraries(laserscan_to_map ${catkin_LIBRARIES})
Expand Down Expand Up @@ -63,7 +66,5 @@ install(TARGETS
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}
PUBLIC_HEADER DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
Original file line number Diff line number Diff line change
Expand Up @@ -27,13 +27,15 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_ACCUMULATOR_H
#define COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_ACCUMULATOR_H
#ifndef COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_H
#define COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_H

#include <ros/ros.h>

#include <list>

namespace costmap_cspace
{
template <typename T>
class PointcloudAccumurator
{
Expand Down Expand Up @@ -106,5 +108,6 @@ class PointcloudAccumurator
ros::Duration time_to_hold_;
std::list<Points> points_;
};
} // namespace costmap_cspace

#endif // COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_ACCUMULATOR_H
#endif // COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_H
6 changes: 3 additions & 3 deletions costmap_cspace/src/laserscan_to_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@

#include <string>

#include <costmap_cspace/pointcloud_accumulator/accumulator.h>
#include <costmap_cspace/pointcloud_accumulator.h>
#include <neonavigation_common/compatibility.h>

class LaserscanToMapNode
Expand All @@ -65,7 +65,7 @@ class LaserscanToMapNode
float origin_x_;
float origin_y_;

PointcloudAccumurator<sensor_msgs::PointCloud2> accum_;
costmap_cspace::PointcloudAccumurator<sensor_msgs::PointCloud2> accum_;

public:
LaserscanToMapNode()
Expand Down Expand Up @@ -121,7 +121,7 @@ class LaserscanToMapNode
{
ROS_WARN("%s", e.what());
}
accum_.push(PointcloudAccumurator<sensor_msgs::PointCloud2>::Points(
accum_.push(costmap_cspace::PointcloudAccumurator<sensor_msgs::PointCloud2>::Points(
cloud_global, cloud_global.header.stamp));

ros::Time now = scan->header.stamp;
Expand Down
6 changes: 3 additions & 3 deletions costmap_cspace/src/pointcloud2_to_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
#include <string>
#include <vector>

#include <costmap_cspace/pointcloud_accumulator/accumulator.h>
#include <costmap_cspace/pointcloud_accumulator.h>
#include <neonavigation_common/compatibility.h>

class Pointcloud2ToMapNode
Expand All @@ -65,7 +65,7 @@ class Pointcloud2ToMapNode
float origin_x_;
float origin_y_;

std::vector<PointcloudAccumurator<sensor_msgs::PointCloud2>> accums_;
std::vector<costmap_cspace::PointcloudAccumurator<sensor_msgs::PointCloud2>> accums_;

public:
Pointcloud2ToMapNode()
Expand Down Expand Up @@ -130,7 +130,7 @@ class Pointcloud2ToMapNode
tf2::doTransform(*cloud, cloud_global, trans);

const int buffer = singleshot ? 1 : 0;
accums_[buffer].push(PointcloudAccumurator<sensor_msgs::PointCloud2>::Points(
accums_[buffer].push(costmap_cspace::PointcloudAccumurator<sensor_msgs::PointCloud2>::Points(
cloud_global, cloud_global.header.stamp));

ros::Time now = cloud->header.stamp;
Expand Down
11 changes: 11 additions & 0 deletions planner_cspace/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPENDS})
find_package(Boost REQUIRED COMPONENTS chrono)
find_package(OpenMP REQUIRED)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS ${CATKIN_DEPENDS}
)

Expand All @@ -45,6 +46,15 @@ add_executable(planner_3d
)
target_link_libraries(planner_3d ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS})
add_dependencies(planner_3d ${catkin_EXPORTED_TARGETS})
set_property(TARGET planner_3d PROPERTY PUBLIC_HEADER
include/planner_cspace/bbf.h
include/planner_cspace/blockmem_gridmap.h
include/planner_cspace/cyclic_vec.h
include/planner_cspace/grid_astar.h
include/planner_cspace/grid_astar_model.h
include/planner_cspace/jump_detector.h
include/planner_cspace/reservable_priority_queue.h
)

add_executable(planner_2dof_serial_joints
src/planner_2dof_serial_joints.cpp
Expand Down Expand Up @@ -80,4 +90,5 @@ install(TARGETS
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
PUBLIC_HEADER DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
3 changes: 3 additions & 0 deletions planner_cspace/include/planner_cspace/bbf.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
#ifndef PLANNER_CSPACE_BBF_H
#define PLANNER_CSPACE_BBF_H

namespace planner_cspace
{
namespace bbf
{
constexpr float oddsToProbability(const float& o)
Expand Down Expand Up @@ -81,5 +83,6 @@ class BinaryBayesFilter
}
};
}; // namespace bbf
} // namespace planner_cspace

#endif // PLANNER_CSPACE_BBF_H
3 changes: 3 additions & 0 deletions planner_cspace/include/planner_cspace/blockmem_gridmap.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@

#include <planner_cspace/cyclic_vec.h>

namespace planner_cspace
{
template <class T, int DIM, int NONCYCLIC>
class BlockMemGridmapBase
{
Expand Down Expand Up @@ -215,5 +217,6 @@ class BlockMemGridmap : public BlockMemGridmapBase<T, DIM, NONCYCLIC>
return *this;
}
};
} // namespace planner_cspace

#endif // PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
4 changes: 4 additions & 0 deletions planner_cspace/include/planner_cspace/cyclic_vec.h
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@

#include <planner_cspace/reservable_priority_queue.h>

namespace planner_cspace
{
namespace cyclic_vec_type_conversion_rule
{
template <typename T>
Expand Down Expand Up @@ -334,4 +336,6 @@ using CyclicVecInt = CyclicVecBase<DIM, NONCYCLIC, int>;
template <int DIM, int NONCYCLIC>
using CyclicVecFloat = CyclicVecBase<DIM, NONCYCLIC, float>;

} // namespace planner_cspace

#endif // PLANNER_CSPACE_CYCLIC_VEC_H
3 changes: 3 additions & 0 deletions planner_cspace/include/planner_cspace/grid_astar.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@

#include <omp.h>

namespace planner_cspace
{
template <int DIM = 3, int NONCYCLIC = 2>
class GridAstar
{
Expand Down Expand Up @@ -382,5 +384,6 @@ class GridAstar
size_t queue_size_limit_;
size_t search_task_num_;
};
} // namespace planner_cspace

#endif // PLANNER_CSPACE_GRID_ASTAR_H
3 changes: 3 additions & 0 deletions planner_cspace/include/planner_cspace/grid_astar_model.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,8 @@

#include <planner_cspace/cyclic_vec.h>

namespace planner_cspace
{
template <int DIM = 3, int NONCYCLIC = 2>
class GridAstarModelBase
{
Expand Down Expand Up @@ -62,5 +64,6 @@ class GridAstarModelBase
virtual const std::vector<Vec>& searchGrids(
const Vec& cur, const std::vector<VecWithCost>& start, const Vec& goal) const = 0;
};
} // namespace planner_cspace

#endif // PLANNER_CSPACE_GRID_ASTAR_MODEL_H
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef PLANNER_CSPACE_PLANNER_3D_JUMP_DETECTOR_H
#define PLANNER_CSPACE_PLANNER_3D_JUMP_DETECTOR_H
#ifndef PLANNER_CSPACE_JUMP_DETECTOR_H
#define PLANNER_CSPACE_JUMP_DETECTOR_H

#include <cmath>
#include <string>
Expand All @@ -39,6 +39,10 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

namespace planner_cspace
{
namespace planner_3d
{
class JumpDetector
{
protected:
Expand Down Expand Up @@ -117,5 +121,7 @@ class JumpDetector
return false;
}
};
} // namespace planner_3d
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_3D_JUMP_DETECTOR_H
#endif // PLANNER_CSPACE_JUMP_DETECTOR_H
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,10 @@
#include <planner_cspace/grid_astar_model.h>
#include <planner_cspace/blockmem_gridmap.h>

namespace planner_cspace
{
namespace planner_2dof_serial_joints
{
class CostCoeff
{
public:
Expand Down Expand Up @@ -77,5 +81,7 @@ class GridAstarModel2DoFSerialJoint : public GridAstarModelBase<2, 0>
const std::vector<VecWithCost>& ss,
const Vec& es) const override;
};
} // namespace planner_2dof_serial_joints
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_2DOF_SERIAL_JOINTS_GRID_ASTAR_MODEL_H
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,10 @@
#include <planner_cspace/grid_astar_model.h>
#include <planner_cspace/blockmem_gridmap.h>

namespace planner_cspace
{
namespace planner_3d
{
class CostCoeff
{
public:
Expand Down Expand Up @@ -137,5 +141,7 @@ class GridAstarModel2D : public GridAstarModelBase<3, 2>
const std::vector<Vec>& searchGrids(
const Vec& cur, const std::vector<VecWithCost>& start, const Vec& goal) const final;
};
} // namespace planner_3d
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_3D_GRID_ASTAR_MODEL_H
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,10 @@

#include <planner_cspace/cyclic_vec.h>

namespace planner_cspace
{
namespace planner_3d
{
namespace grid_metric_converter
{
template <typename T>
Expand Down Expand Up @@ -97,5 +101,7 @@ void grid2MetricPath(
}
}
} // namespace grid_metric_converter
} // namespace planner_3d
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_3D_GRID_METRIC_CONVERTER_H
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,10 @@

#include <planner_cspace/cyclic_vec.h>

namespace planner_cspace
{
namespace planner_3d
{
class MotionCache
{
public:
Expand Down Expand Up @@ -96,5 +100,7 @@ class MotionCache
int page_size_;
CyclicVecInt<3, 2> max_range_;
};
} // namespace planner_3d
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,10 @@
#include <planner_cspace/cyclic_vec.h>
#include <planner_cspace/planner_3d/rotation_cache.h>

namespace planner_cspace
{
namespace planner_3d
{
class PathInterpolator
{
private:
Expand All @@ -56,5 +60,7 @@ class PathInterpolator
const float interval,
const int local_range) const;
};
} // namespace planner_3d
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_3D_PATH_INTERPOLATOR_H
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,10 @@

#include <planner_cspace/cyclic_vec.h>

namespace planner_cspace
{
namespace planner_3d
{
class RotationCache
{
private:
Expand Down Expand Up @@ -99,5 +103,7 @@ class RotationCache
const float interval,
const int local_range) const;
};
} // namespace planner_3d
} // namespace planner_cspace

#endif // PLANNER_CSPACE_PLANNER_3D_ROTATION_CACHE_H
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@

#include <queue>

namespace planner_cspace
{
template <class T>
class reservable_priority_queue : public std::priority_queue<T>
{
Expand All @@ -58,5 +60,6 @@ class reservable_priority_queue : public std::priority_queue<T>
this->c.pop_back();
}
};
} // namespace planner_cspace

#endif // PLANNER_CSPACE_RESERVABLE_PRIORITY_QUEUE_H
7 changes: 6 additions & 1 deletion planner_cspace/src/grid_astar_model_2dof_joints.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,10 @@

#include <planner_cspace/planner_2dof_serial_joints/grid_astar_model.h>

namespace planner_cspace
{
namespace planner_2dof_serial_joints
{
GridAstarModel2DoFSerialJoint::GridAstarModel2DoFSerialJoint(
const Vecf& euclid_cost_coef,
const int resolution,
Expand Down Expand Up @@ -117,4 +121,5 @@ const std::vector<GridAstarModel2DoFSerialJoint::Vec>& GridAstarModel2DoFSerialJ
{
return search_list_;
}

} // namespace planner_2dof_serial_joints
} // namespace planner_cspace
Loading

0 comments on commit 2f7c632

Please sign in to comment.