diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..97529bb86 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "src/octomap_msgs"] + path = src/octomap_msgs + url = https://github.com/OctoMap/octomap_msgs + branch = ros2 diff --git a/global_planner/CMakeLists.txt b/global_planner/CMakeLists.txt deleted file mode 100644 index 8f2af83ea..000000000 --- a/global_planner/CMakeLists.txt +++ /dev/null @@ -1,246 +0,0 @@ -cmake_minimum_required(VERSION 2.8.12) -project(global_planner) - -add_definitions(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - dynamic_reconfigure - message_generation - tf - pcl_ros - mavlink - mavros_msgs - mavros - mavros_extras - avoidance -) -find_package(PCL 1.7 REQUIRED) -find_package(octomap REQUIRED) - -if(DISABLE_SIMULATION) - message(STATUS "Building avoidance without Gazebo Simulation") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDISABLE_SIMULATION") -endif() - -################################################ -## Gazebo Simulation -################################################ - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - PathWithRiskMsg.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -generate_dynamic_reconfigure_options( - cfg/GlobalPlannerNode.cfg -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs sensor_msgs message_runtime tf -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## CMake Setup -# Build in Release mode if nothing is specified -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif(NOT CMAKE_BUILD_TYPE) - - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIR} -) -link_libraries(${OCTOMAP_LIBRARIES}) - -## Declare a C++ library -add_library(global_planner - src/library/node.cpp - src/library/cell.cpp - src/library/global_planner.cpp - src/nodes/global_planner_node.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(global_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -add_executable(global_planner_node src/nodes/global_planner_node_main.cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(global_planner_node - global_planner ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS avoidance avoidance_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -# Add gtest based cpp test target and link libraries -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp - test/test_example.cpp) - if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${YAML_CPP_LIBRARIES}) - endif() - - - if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage") - SET(CMAKE_CXX_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage --coverage") - SET(CMAKE_C_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage --coverage") - SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") - - add_custom_target(${PROJECT_NAME}-test_coverage - COMMAND lcov --zerocounters --directory ${PROJECT_BINARY_DIR} - COMMAND lcov --capture --initial --no-external --directory ${PROJECT_BINARY_DIR} --base-directory ${${PROJECT_NAME}_SOURCE_DIR} --output-file base_coverage.info --rc lcov_branch_coverage=1 - COMMAND ${PROJECT_NAME}-test - COMMAND lcov --capture --no-external --directory ${PROJECT_BINARY_DIR} --base-directory ${${PROJECT_NAME}_SOURCE_DIR} --output-file test_coverage.info --rc lcov_branch_coverage=1 - COMMAND lcov -a base_coverage.info -a test_coverage.info -o coverage.info --rc lcov_branch_coverage=1 - COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info - WORKING_DIRECTORY . - DEPENDS ${PROJECT_NAME}-test - ) - add_custom_target(${PROJECT_NAME}-test_coverage_html - COMMAND genhtml coverage.info --output-directory out --branch-coverage - COMMAND x-www-browser out/index.html - WORKING_DIRECTORY . - DEPENDS ${PROJECT_NAME}-test_coverage - ) - endif() -endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/global_planner/include/global_planner/global_planner_node.h b/global_planner/include/global_planner/global_planner_node.h deleted file mode 100644 index 6020f8afb..000000000 --- a/global_planner/include/global_planner/global_planner_node.h +++ /dev/null @@ -1,153 +0,0 @@ -#ifndef GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H -#define GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "avoidance/avoidance_node.h" -#include "global_planner/global_planner.h" - -#ifndef DISABLE_SIMULATION -#include -#endif - -namespace global_planner { - -struct cameraData { - ros::Subscriber pointcloud_sub_; -}; - -class GlobalPlannerNode { - public: - // TODO: Deque instead of vector - GlobalPlanner global_planner_; - std::vector waypoints_; // Intermediate goals, from file, mavros - // mission or intermediate goals - GlobalPlannerNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); - ~GlobalPlannerNode(); - - private: - std::mutex mutex_; - - ros::NodeHandle nh_; - ros::NodeHandle nh_private_; - - // Subscribers - ros::Subscriber octomap_sub_; - ros::Subscriber octomap_full_sub_; - ros::Subscriber ground_truth_sub_; - ros::Subscriber velocity_sub_; - ros::Subscriber clicked_point_sub_; - ros::Subscriber move_base_simple_sub_; - ros::Subscriber laser_sensor_sub_; - ros::Subscriber fcu_input_sub_; - - // Publishers - ros::Publisher global_temp_path_pub_; - ros::Publisher smooth_path_pub_; - ros::Publisher actual_path_pub_; - ros::Publisher explored_cells_pub_; - ros::Publisher global_goal_pub_; - ros::Publisher global_temp_goal_pub_; - ros::Publisher mavros_obstacle_free_path_pub_; - ros::Publisher mavros_waypoint_publisher_; - ros::Publisher current_waypoint_publisher_; - ros::Publisher pointcloud_pub_; - - ros::Time start_time_; - ros::Time last_wp_time_; - - ros::Timer cmdloop_timer_; - ros::Timer plannerloop_timer_; - ros::CallbackQueue cmdloop_queue_; - ros::CallbackQueue plannerloop_queue_; - std::unique_ptr cmdloop_spinner_; - std::unique_ptr plannerloop_spinner_; - - tf::TransformListener listener_; - dynamic_reconfigure::Server server_; - - nav_msgs::Path actual_path_; - geometry_msgs::Point start_pos_; - geometry_msgs::PoseStamped current_goal_; - geometry_msgs::PoseStamped last_goal_; - geometry_msgs::PoseStamped last_pos_; - - std::vector last_clicked_points; - std::vector path_; - std::vector cameras_; - - int num_octomap_msg_ = 0; - int num_pos_msg_ = 0; - double cmdloop_dt_; - double plannerloop_dt_; - double mapupdate_dt_; - double min_speed_; - double speed_ = min_speed_; - double start_yaw_; - bool position_received_; - std::string frame_id_; - - // Dynamic Reconfiguration - double clicked_goal_alt_; - double clicked_goal_radius_; - bool hover_; - int simplify_iterations_; - double simplify_margin_; - - avoidance::AvoidanceNode avoidance_node_; -#ifndef DISABLE_SIMULATION - std::unique_ptr world_visualizer_; -#endif - void readParams(); - void initializeCameraSubscribers(std::vector& camera_topics); - void receivePath(const nav_msgs::Path& msg); - void setNewGoal(const GoalCell& goal); - void popNextGoal(); - void planPath(); - void setIntermediateGoal(); - bool isCloseToGoal(); - void setCurrentPath(const std::vector& poses); - void dynamicReconfigureCallback(global_planner::GlobalPlannerNodeConfig& config, uint32_t level); - void velocityCallback(const geometry_msgs::TwistStamped& msg); - void positionCallback(const geometry_msgs::PoseStamped& msg); - void clickedPointCallback(const geometry_msgs::PointStamped& msg); - void moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg); - void octomapFullCallback(const octomap_msgs::Octomap& msg); - void depthCameraCallback(const sensor_msgs::PointCloud2& msg); - void fcuInputGoalCallback(const mavros_msgs::Trajectory& msg); - void cmdLoopCallback(const ros::TimerEvent& event); - void plannerLoopCallback(const ros::TimerEvent& event); - void publishGoal(const GoalCell& goal); - void publishPath(); - void publishSetpoint(); - void printPointInfo(double x, double y, double z); -}; - -} // namespace global_planner - -#endif // GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H diff --git a/global_planner/package.xml b/global_planner/package.xml deleted file mode 100644 index e292ce58b..000000000 --- a/global_planner/package.xml +++ /dev/null @@ -1,79 +0,0 @@ - - - global_planner - 0.0.0 - Avoidance module doing global planning. - - - - - pixhawk - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - dynamic_reconfigure - message_generation - roscpp - rospy - std_msgs - geometry_msgs - sensor_msgs - octomap - octomap_msgs - tf - pcl_ros - mavros - mavros_extras - avoidance - - - dynamic_reconfigure - message_runtime - roscpp - rospy - std_msgs - geometry_msgs - sensor_msgs - octomap - octomap_msgs - tf - pcl_ros - mavros - mavros_extras - avoidance - - - - - - diff --git a/global_planner/src/nodes/global_planner_node.cpp b/global_planner/src/nodes/global_planner_node.cpp deleted file mode 100644 index d724362db..000000000 --- a/global_planner/src/nodes/global_planner_node.cpp +++ /dev/null @@ -1,411 +0,0 @@ -#include "global_planner/global_planner_node.h" - -namespace global_planner { - -GlobalPlannerNode::GlobalPlannerNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) - : nh_(nh), - nh_private_(nh_private), - avoidance_node_(nh, nh_private), - cmdloop_dt_(0.1), - plannerloop_dt_(1.0), - mapupdate_dt_(0.2), - start_yaw_(0.0) { - // Set up Dynamic Reconfigure Server - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&GlobalPlannerNode::dynamicReconfigureCallback, this, _1, _2); - server_.setCallback(f); - -#ifndef DISABLE_SIMULATION - world_visualizer_.reset(new avoidance::WorldVisualizer(nh_, ros::this_node::getName())); -#endif - - avoidance_node_.init(); - // Read Ros parameters - readParams(); - - // Subscribers - octomap_full_sub_ = nh_.subscribe("/octomap_full", 1, &GlobalPlannerNode::octomapFullCallback, this); - ground_truth_sub_ = nh_.subscribe("/mavros/local_position/pose", 1, &GlobalPlannerNode::positionCallback, this); - velocity_sub_ = nh_.subscribe("/mavros/local_position/velocity", 1, &GlobalPlannerNode::velocityCallback, this); - clicked_point_sub_ = nh_.subscribe("/clicked_point", 1, &GlobalPlannerNode::clickedPointCallback, this); - move_base_simple_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &GlobalPlannerNode::moveBaseSimpleCallback, this); - fcu_input_sub_ = nh_.subscribe("/mavros/trajectory/desired", 1, &GlobalPlannerNode::fcuInputGoalCallback, this); - - // Publishers - global_temp_path_pub_ = nh_.advertise("/global_temp_path", 10); - actual_path_pub_ = nh_.advertise("/actual_path", 10); - smooth_path_pub_ = nh_.advertise("/smooth_path", 10); - global_goal_pub_ = nh_.advertise("/global_goal", 10); - global_temp_goal_pub_ = nh_.advertise("/global_temp_goal", 10); - explored_cells_pub_ = nh_.advertise("/explored_cells", 10); - mavros_waypoint_publisher_ = nh_.advertise("/mavros/setpoint_position/local", 10); - mavros_obstacle_free_path_pub_ = nh_.advertise("/mavros/trajectory/generated", 10); - current_waypoint_publisher_ = nh_.advertise("/current_setpoint", 10); - pointcloud_pub_ = nh_.advertise("/cloud_in", 10); - - actual_path_.header.frame_id = frame_id_; - - ros::TimerOptions cmdlooptimer_options(ros::Duration(cmdloop_dt_), - boost::bind(&GlobalPlannerNode::cmdLoopCallback, this, _1), &cmdloop_queue_); - cmdloop_timer_ = nh_.createTimer(cmdlooptimer_options); - - cmdloop_spinner_.reset(new ros::AsyncSpinner(1, &cmdloop_queue_)); - cmdloop_spinner_->start(); - - ros::TimerOptions plannerlooptimer_options(ros::Duration(plannerloop_dt_), - boost::bind(&GlobalPlannerNode::plannerLoopCallback, this, _1), - &plannerloop_queue_); - plannerloop_timer_ = nh_.createTimer(plannerlooptimer_options); - - plannerloop_spinner_.reset(new ros::AsyncSpinner(1, &plannerloop_queue_)); - plannerloop_spinner_->start(); - - current_goal_.header.frame_id = frame_id_; - current_goal_.pose.position = start_pos_; - current_goal_.pose.orientation = tf::createQuaternionMsgFromYaw(start_yaw_); - last_goal_ = current_goal_; - - speed_ = 5.0; - - start_time_ = ros::Time::now(); -} - -GlobalPlannerNode::~GlobalPlannerNode() {} - -// Read Ros parameters -void GlobalPlannerNode::readParams() { - std::vector camera_topics; - - nh_.param("start_pos_x", start_pos_.x, 0.5); - nh_.param("start_pos_y", start_pos_.y, 0.5); - nh_.param("start_pos_z", start_pos_.z, 3.5); - nh_.param("frame_id", frame_id_, "/local_origin"); - nh_.getParam("pointcloud_topics", camera_topics); - - initializeCameraSubscribers(camera_topics); - global_planner_.goal_pos_ = GoalCell(start_pos_.x, start_pos_.y, start_pos_.z); - double robot_radius; - nh_.param("robot_radius", robot_radius, 0.5); - global_planner_.setFrame(frame_id_); - global_planner_.setRobotRadius(robot_radius); -} - -void GlobalPlannerNode::initializeCameraSubscribers(std::vector& camera_topics) { - cameras_.resize(camera_topics.size()); - - for (size_t i = 0; i < camera_topics.size(); i++) { - cameras_[i].pointcloud_sub_ = nh_.subscribe(camera_topics[i], 1, &GlobalPlannerNode::depthCameraCallback, this); - } -} - -// Sets a new goal, plans a path to it and publishes some info -void GlobalPlannerNode::setNewGoal(const GoalCell& goal) { - ROS_INFO("========== Set goal : %s ==========", goal.asString().c_str()); - global_planner_.setGoal(goal); - publishGoal(goal); -} - -// Sets the next waypoint to be the current goal -void GlobalPlannerNode::popNextGoal() { - if (!waypoints_.empty()) { - // Set the first goal in waypoints_ as the new goal - GoalCell new_goal = waypoints_.front(); - waypoints_.erase(waypoints_.begin()); - setNewGoal(new_goal); - } else if (global_planner_.goal_is_blocked_) { - // Goal is blocked but there is no other goal in waypoints_, just stop - ROS_INFO(" STOP "); - global_planner_.stop(); - } -} - -// Plans a new path and publishes it -void GlobalPlannerNode::planPath() { - std::clock_t start_time = std::clock(); - if (global_planner_.octree_) { - ROS_INFO("OctoMap memory usage: %2.3f MB", global_planner_.octree_->memoryUsage() / 1000000.0); - } - - bool found_path = global_planner_.getGlobalPath(); - - if (!found_path) { - // TODO: popNextGoal(), instead of checking if goal_is_blocked in - // positionCallback? - ROS_INFO("Failed to find a path"); - } else if (global_planner_.overestimate_factor_ > 1.05) { - // The path is not good enough, set an intermediate goal on the path - setIntermediateGoal(); - } - printf("Total time: %2.2f ms \n", (std::clock() - start_time) / (double)(CLOCKS_PER_SEC / 1000)); -} - -// Sets a temporary goal on the path to the current goal -void GlobalPlannerNode::setIntermediateGoal() { - int curr_path_length = global_planner_.curr_path_.size(); - if (curr_path_length > 10) { - printf("\n ===== Half-way path ====== \n"); - waypoints_.insert(waypoints_.begin(), global_planner_.goal_pos_); - Cell middle_cell = global_planner_.curr_path_[curr_path_length / 2]; - setNewGoal(GoalCell(middle_cell, curr_path_length / 4, true)); - } -} - -void GlobalPlannerNode::dynamicReconfigureCallback(global_planner::GlobalPlannerNodeConfig& config, uint32_t level) { - // global_planner_ - global_planner_.min_altitude_ = config.min_altitude_; - global_planner_.max_altitude_ = config.max_altitude_; - global_planner_.max_cell_risk_ = config.max_cell_risk_; - global_planner_.smooth_factor_ = config.smooth_factor_; - global_planner_.vert_to_hor_cost_ = config.vert_to_hor_cost_; - global_planner_.risk_factor_ = config.risk_factor_; - global_planner_.neighbor_risk_flow_ = config.neighbor_risk_flow_; - global_planner_.expore_penalty_ = config.expore_penalty_; - global_planner_.up_cost_ = config.up_cost_; - global_planner_.down_cost_ = config.down_cost_; - global_planner_.search_time_ = config.search_time_; - global_planner_.min_overestimate_factor_ = config.min_overestimate_factor_; - global_planner_.max_overestimate_factor_ = config.max_overestimate_factor_; - global_planner_.max_iterations_ = config.max_iterations_; - global_planner_.goal_must_be_free_ = config.goal_must_be_free_; - global_planner_.use_current_yaw_ = config.use_current_yaw_; - global_planner_.use_risk_heuristics_ = config.use_risk_heuristics_; - global_planner_.use_speedup_heuristics_ = config.use_speedup_heuristics_; - - // global_planner_node - clicked_goal_alt_ = config.clicked_goal_alt_; - clicked_goal_radius_ = config.clicked_goal_radius_; - simplify_iterations_ = config.simplify_iterations_; - simplify_margin_ = config.simplify_margin_; - - // cell - if (level == 2) { - CELL_SCALE = config.CELL_SCALE; - } - - // node - if (level == 4) { - SPEEDNODE_RADIUS = config.SPEEDNODE_RADIUS; - global_planner_.default_node_type_ = config.default_node_type_; - } -} - -void GlobalPlannerNode::velocityCallback(const geometry_msgs::TwistStamped& msg) { - global_planner_.curr_vel_ = msg.twist.linear; -} - -// Sets the current position and checks if the current goal has been reached -void GlobalPlannerNode::positionCallback(const geometry_msgs::PoseStamped& msg) { - // Update position - last_pos_ = msg; - global_planner_.setPose(last_pos_); - - // Check if a new goal is needed - if (num_pos_msg_++ % 10 == 0) { - // Keep track of and publish the actual travel trajectory - // ROS_INFO("Travelled path extended"); - last_pos_.header.frame_id = frame_id_; - actual_path_.poses.push_back(last_pos_); - actual_path_pub_.publish(actual_path_); - } - - position_received_ = true; - - // Check if we are close enough to current goal to get the next part of the - // path - if (path_.size() > 0 && isCloseToGoal()) { - // TODO: get yawdiff(yaw1, yaw2) - double yaw1 = tf::getYaw(current_goal_.pose.orientation); - double yaw2 = tf::getYaw(last_pos_.pose.orientation); - double yaw_diff = std::abs(yaw2 - yaw1); - // Transform yaw_diff to [0, 2*pi] - yaw_diff -= std::floor(yaw_diff / (2 * M_PI)) * (2 * M_PI); - double max_yaw_diff = M_PI / 1.0; - if (yaw_diff < max_yaw_diff || yaw_diff > 2 * M_PI - max_yaw_diff) { - // If we are facing the right direction, then pop the first point of the - // path - last_goal_ = current_goal_; - current_goal_ = path_[0]; - path_.erase(path_.begin()); - } - } -} - -void GlobalPlannerNode::clickedPointCallback(const geometry_msgs::PointStamped& msg) { - printPointInfo(msg.point.x, msg.point.y, msg.point.z); - - geometry_msgs::PoseStamped pose; - pose.header = msg.header; - pose.pose.position = msg.point; - pose.pose.position.z = global_planner_.curr_pos_.z; - last_clicked_points.push_back(pose); -} - -void GlobalPlannerNode::moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg) { - setNewGoal(GoalCell(msg.pose.position.x, msg.pose.position.y, clicked_goal_alt_, clicked_goal_radius_)); -} - -void GlobalPlannerNode::fcuInputGoalCallback(const mavros_msgs::Trajectory& msg) { - const GoalCell new_goal = GoalCell(msg.point_2.position.x, msg.point_2.position.y, msg.point_2.position.z, 1.0); - if (msg.point_valid[1] == true && ((std::fabs(global_planner_.goal_pos_.xPos() - new_goal.xPos()) > 0.001) || - (std::fabs(global_planner_.goal_pos_.yPos() - new_goal.yPos()) > 0.001))) { - setNewGoal(new_goal); - } -} - -// Check if the current path is blocked -void GlobalPlannerNode::octomapFullCallback(const octomap_msgs::Octomap& msg) { - std::lock_guard lock(mutex_); - - ros::Time current = ros::Time::now(); - // Update map at a fixed rate. This is useful on setting replanning rates for the planner. - if ((current - last_wp_time_).toSec() < mapupdate_dt_) { - return; - } - last_wp_time_ = ros::Time::now(); - - octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(msg); - - global_planner_.updateFullOctomap(tree); -} - -// Go through obstacle points and store them -void GlobalPlannerNode::depthCameraCallback(const sensor_msgs::PointCloud2& msg) { - try { - // Transform msg from camera frame to world frame - ros::Time now = ros::Time::now(); - listener_.waitForTransform(frame_id_, "/camera_link", now, ros::Duration(5.0)); - tf::StampedTransform transform; - listener_.lookupTransform(frame_id_, "/camera_link", now, transform); - sensor_msgs::PointCloud2 transformed_msg; - pcl_ros::transformPointCloud(frame_id_, transform, msg, transformed_msg); - pcl::PointCloud cloud; // Easier to loop through pcl::PointCloud - pcl::fromROSMsg(transformed_msg, cloud); - - // Store the obstacle points - for (const auto& p : cloud) { - if (!std::isnan(p.x)) { - // TODO: Not all points end up here - Cell occupied_cell(p.x, p.y, p.z); - global_planner_.occupied_.insert(occupied_cell); - } - } - pointcloud_pub_.publish(msg); - } catch (tf::TransformException const& ex) { - ROS_DEBUG("%s", ex.what()); - ROS_WARN("Transformation not available (%s to /camera_link", frame_id_); - } -} - -void GlobalPlannerNode::setCurrentPath(const std::vector& poses) { - path_.clear(); - - if (poses.size() < 2) { - ROS_INFO(" Received empty path\n"); - return; - } - last_goal_ = poses[0]; - current_goal_ = poses[1]; - - for (int i = 2; i < poses.size(); ++i) { - path_.push_back(poses[i]); - } -} - -void GlobalPlannerNode::cmdLoopCallback(const ros::TimerEvent& event) { - hover_ = false; - - // Check if all information was received - ros::Time now = ros::Time::now(); - - ros::Duration since_last_cloud = now - last_wp_time_; - ros::Duration since_start = now - start_time_; - - avoidance_node_.checkFailsafe(since_last_cloud, since_start, hover_); - publishSetpoint(); -} - -void GlobalPlannerNode::plannerLoopCallback(const ros::TimerEvent& event) { - std::lock_guard lock(mutex_); - bool is_in_goal = global_planner_.goal_pos_.withinPositionRadius(global_planner_.curr_pos_); - if (is_in_goal || global_planner_.goal_is_blocked_) { - popNextGoal(); - } - - planPath(); - - // Print and publish info - if (is_in_goal && !waypoints_.empty()) { - ROS_INFO("Reached current goal %s, %d goals left\n\n", global_planner_.goal_pos_.asString().c_str(), - (int)waypoints_.size()); - ROS_INFO("Actual travel distance: %2.2f \t Actual energy usage: %2.2f", pathLength(actual_path_), - pathEnergy(actual_path_, global_planner_.up_cost_)); - } - - publishPath(); -} - -// Publish the position of goal -void GlobalPlannerNode::publishGoal(const GoalCell& goal) { - geometry_msgs::PointStamped pointMsg; - pointMsg.header.frame_id = frame_id_; - pointMsg.point = goal.toPoint(); - - // Always publish as temporary to remove any obsolete temporary path - global_temp_goal_pub_.publish(pointMsg); - if (!goal.is_temporary_) { - global_goal_pub_.publish(pointMsg); - } -} - -// Publish the current path -void GlobalPlannerNode::publishPath() { - auto path_msg = global_planner_.getPathMsg(); - PathWithRiskMsg risk_msg = global_planner_.getPathWithRiskMsg(); - // Always publish as temporary to remove any obsolete temporary path - global_temp_path_pub_.publish(path_msg); - setCurrentPath(path_msg.poses); - smooth_path_pub_.publish(smoothPath(path_msg)); - - auto simple_path = simplifyPath(&global_planner_, global_planner_.curr_path_, simplify_iterations_, simplify_margin_); - auto simple_path_msg = global_planner_.getPathMsg(simple_path); - global_temp_path_pub_.publish(simple_path_msg); - setCurrentPath(simple_path_msg.poses); - smooth_path_pub_.publish(smoothPath(simple_path_msg)); -} - -// Prints information about the point, mostly the risk of the containing cell -void GlobalPlannerNode::printPointInfo(double x, double y, double z) { - // Update explored cells - printPointStats(&global_planner_, x, y, z); -} - -void GlobalPlannerNode::publishSetpoint() { - // Vector pointing from current position to the current goal - tf::Vector3 vec = toTfVector3(subtractPoints(current_goal_.pose.position, last_pos_.pose.position)); - // If we are less than 1.0 away, then we should stop at the goal - double new_len = vec.length() < 1.0 ? vec.length() : speed_; - vec.normalize(); - vec *= new_len; - - auto setpoint = current_goal_; // The intermediate position sent to Mavros - setpoint.pose.position.x = last_pos_.pose.position.x + vec.getX(); - setpoint.pose.position.y = last_pos_.pose.position.y + vec.getY(); - setpoint.pose.position.z = last_pos_.pose.position.z + vec.getZ(); - - // Publish setpoint for vizualization - current_waypoint_publisher_.publish(setpoint); - - // Publish setpoint to Mavros - mavros_waypoint_publisher_.publish(setpoint); - mavros_msgs::Trajectory obst_free_path = {}; - geometry_msgs::Twist velocity_setpoint{}; - velocity_setpoint.linear.x = NAN; - velocity_setpoint.linear.y = NAN; - velocity_setpoint.linear.z = NAN; - avoidance::transformToTrajectory(obst_free_path, setpoint, velocity_setpoint); - mavros_obstacle_free_path_pub_.publish(obst_free_path); -} - -bool GlobalPlannerNode::isCloseToGoal() { return distance(current_goal_, last_pos_) < 1.5; } - -} // namespace global_planner diff --git a/global_planner/src/nodes/global_planner_node_main.cpp b/global_planner/src/nodes/global_planner_node_main.cpp deleted file mode 100644 index 5c892ae5d..000000000 --- a/global_planner/src/nodes/global_planner_node_main.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "global_planner/global_planner_node.h" - -int main(int argc, char** argv) { - ros::init(argc, argv, "global_planner_node"); - - ros::NodeHandle nh("~"); - ros::NodeHandle nh_private(""); - - global_planner::GlobalPlannerNode global_planner_node(nh, nh_private); - - ros::spin(); - return 0; -} diff --git a/global_planner/src/nodes/mock_data_node.cpp b/global_planner/src/nodes/mock_data_node.cpp deleted file mode 100644 index d6a109641..000000000 --- a/global_planner/src/nodes/mock_data_node.cpp +++ /dev/null @@ -1,129 +0,0 @@ -#include "mock_data_node.h" - -namespace global_planner { - -MockDataNode::MockDataNode() { - ros::NodeHandle nh; - - path_sub_ = nh.subscribe("/global_path", 1, &MockDataNode::receivePath, this); - - depth_points_pub_ = nh.advertise("/camera/depth/points", 10); - local_position_pub_ = nh.advertise("/mavros/local_position/pose", 10); - global_goal_pub_ = nh.advertise("/clicked_point", 10); - - createWall(5, 5, 6); - - int num_loops = 0; - ros::Rate rate(1); - while (ros::ok()) { - if (num_loops++ == 10) { - sendClickedPoint(); - } - sendMockData(); - rate.sleep(); - ros::spinOnce(); - } -} - -MockDataNode::~MockDataNode() {} - -void MockDataNode::createWall(int dist, int width, int height) { - points_.clear(); - for (int i = -width; i <= width; ++i) { - for (int j = 0; j <= height; ++j) { - points_.push_back(dist + 0.5); - points_.push_back(i + 0.5); - points_.push_back(j + 0.5); - } - } -} - -void MockDataNode::sendClickedPoint() { - geometry_msgs::PointStamped msg; - msg.header.frame_id = "/world"; - msg.point.x = 8.5; - msg.point.y = 4.5; - msg.point.z = 1.5; - global_goal_pub_.publish(msg); -} - -void MockDataNode::receivePath(const nav_msgs::Path& msg) { - for (auto p : msg.poses) { - double x = p.pose.position.x; - double y = p.pose.position.y; - double z = p.pose.position.z; - printf("(%2.2f, %2.2f, %2.2f) -> ", x, y, z); - } - printf("\n\n"); -} - -void MockDataNode::sendMockData() { - // Create a PointCloud2 - sensor_msgs::PointCloud2 cloud_msg; - cloud_msg.header.frame_id = "/world"; - // Fill some internals of the PoinCloud2 like the header/width/height ... - cloud_msg.height = 1; - cloud_msg.width = 4; - // Set the point fields to xyzrgb and resize the vector with the following - // command 4 is for the number of added fields. Each come in triplet: the name - // of the PointField, the number of occurences of the type in the PointField, - // the type of the PointField - sensor_msgs::PointCloud2Modifier modifier(cloud_msg); - modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - // For convenience and the xyz, rgb, rgba fields, you can also use the - // following overloaded function. You have to be aware that the following - // function does add extra padding for backward compatibility though so it is - // definitely the solution of choice for PointXYZ and PointXYZRGB 2 is for the - // number of fields to add - modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - // You can then reserve / resize as usual - modifier.resize(100); - - // Define some raw data we'll put in the PointCloud2 - int n = points_.size() / 3; - uint8_t color_data[] = {40, 200, 120}; - - // Define the iterators. When doing so, you define the Field you would like to - // iterate upon and the type of you would like returned: it is not necessary - // the type of the PointField as sometimes you pack data in another type (e.g. - // 3 uchar + 1 uchar for RGB are packed in a float) - sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); - // Even though the r,g,b,a fields do not exist (it's usually rgb, rgba), you - // can create iterators for those: they will handle data packing for you (in - // little endian RGB is packed as *,R,G,B in a float and RGBA as A,R,G,B) - sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); - sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); - sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); - // Fill the PointCloud2 - for (size_t i = 0; i < n; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) { - *iter_x = points_[3 * i + 0]; - *iter_y = points_[3 * i + 1]; - *iter_z = points_[3 * i + 2]; - *iter_r = color_data[0]; - *iter_g = color_data[1]; - *iter_b = color_data[2]; - } - - depth_points_pub_.publish(cloud_msg); - - // Send position - geometry_msgs::PoseStamped pos; - pos.header.frame_id = "/world"; - pos.pose.position.x = 0.5; - pos.pose.position.y = 2.5; - pos.pose.position.z = 1.5; - pos.pose.orientation.w = 1.0; - local_position_pub_.publish(pos); -} - -} // namespace global_planner - -int main(int argc, char** argv) { - ros::init(argc, argv, "mock_data_node"); - global_planner::MockDataNode mock_data_node; - ros::spin(); - return 0; -} diff --git a/global_planner/src/nodes/mock_data_node.h b/global_planner/src/nodes/mock_data_node.h deleted file mode 100644 index c5f8885df..000000000 --- a/global_planner/src/nodes/mock_data_node.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef GLOBAL_PLANNER_MOCK_DATA_NODE_H -#define GLOBAL_PLANNER_MOCK_DATA_NODE_H - -#include -#include -#include -#include - -#include -#include - -#include "global_planner/common.h" // hasSameYawAndAltitude - -namespace global_planner { - -class MockDataNode { - public: - MockDataNode(); - ~MockDataNode(); - void createWall(int dist, int width, int height); - void sendClickedPoint(); - void receivePath(const nav_msgs::Path& msg); - void sendMockData(); - - std::vector points_{5.5, -0.5, 0.5, 5.5, 0.5, 0.5, 5.5, 1.5, 0.5, 5.5, -0.5, 1.5, 5.5, 0.5, - 1.5, 5.5, 1.5, 1.5, 5.5, -0.5, 2.5, 5.5, 0.5, 2.5, 5.5, 1.5, 2.5}; - - private: - ros::Subscriber path_sub_; - - ros::Publisher local_position_pub_; - ros::Publisher depth_points_pub_; - ros::Publisher global_goal_pub_; -}; - -} // namespace global_planner - -#endif // GLOBAL_PLANNER_MOCK_DATA_NODE_H diff --git a/safe_landing_planner/COLCON_IGNORE b/safe_landing_planner/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/avoidance/CMakeLists.txt b/src/avoidance/CMakeLists.txt similarity index 100% rename from avoidance/CMakeLists.txt rename to src/avoidance/CMakeLists.txt diff --git a/avoidance/include/avoidance/avoidance_node.h b/src/avoidance/include/avoidance/avoidance_node.h similarity index 100% rename from avoidance/include/avoidance/avoidance_node.h rename to src/avoidance/include/avoidance/avoidance_node.h diff --git a/avoidance/include/avoidance/common.h b/src/avoidance/include/avoidance/common.h similarity index 98% rename from avoidance/include/avoidance/common.h rename to src/avoidance/include/avoidance/common.h index 336dc346e..f6435fffa 100644 --- a/avoidance/include/avoidance/common.h +++ b/src/avoidance/include/avoidance/common.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -294,6 +295,13 @@ float getYawFromQuaternion(const Eigen::Quaternionf q); **/ float getPitchFromQuaternion(const Eigen::Quaternionf q); + +/** +* @brief Tranform yaw angle to quaternion msg +* @returns quaternion msg (in geometry_msgs) +**/ +geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw); + /** * @brief wrappes the input angle in to plus minus PI space * @param[in] angle to be wrapped [rad] diff --git a/avoidance/include/avoidance/histogram.h b/src/avoidance/include/avoidance/histogram.h similarity index 100% rename from avoidance/include/avoidance/histogram.h rename to src/avoidance/include/avoidance/histogram.h diff --git a/avoidance/include/avoidance/rviz_world_loader.h b/src/avoidance/include/avoidance/rviz_world_loader.h similarity index 100% rename from avoidance/include/avoidance/rviz_world_loader.h rename to src/avoidance/include/avoidance/rviz_world_loader.h diff --git a/avoidance/include/avoidance/transform_buffer.h b/src/avoidance/include/avoidance/transform_buffer.h similarity index 100% rename from avoidance/include/avoidance/transform_buffer.h rename to src/avoidance/include/avoidance/transform_buffer.h diff --git a/avoidance/include/avoidance/usm.h b/src/avoidance/include/avoidance/usm.h similarity index 100% rename from avoidance/include/avoidance/usm.h rename to src/avoidance/include/avoidance/usm.h diff --git a/avoidance/launch/avoidance_sitl.launch.py b/src/avoidance/launch/avoidance_sitl.launch.py similarity index 100% rename from avoidance/launch/avoidance_sitl.launch.py rename to src/avoidance/launch/avoidance_sitl.launch.py diff --git a/avoidance/launch/avoidance_sitl_stereo.launch.py b/src/avoidance/launch/avoidance_sitl_stereo.launch.py similarity index 100% rename from avoidance/launch/avoidance_sitl_stereo.launch.py rename to src/avoidance/launch/avoidance_sitl_stereo.launch.py diff --git a/avoidance/launch/parameters.yaml b/src/avoidance/launch/parameters.yaml similarity index 100% rename from avoidance/launch/parameters.yaml rename to src/avoidance/launch/parameters.yaml diff --git a/avoidance/package.xml b/src/avoidance/package.xml similarity index 100% rename from avoidance/package.xml rename to src/avoidance/package.xml diff --git a/avoidance/sim/models/distance_sensor/distance_sensor.sdf b/src/avoidance/sim/models/distance_sensor/distance_sensor.sdf similarity index 100% rename from avoidance/sim/models/distance_sensor/distance_sensor.sdf rename to src/avoidance/sim/models/distance_sensor/distance_sensor.sdf diff --git a/avoidance/sim/models/distance_sensor/model.config b/src/avoidance/sim/models/distance_sensor/model.config similarity index 100% rename from avoidance/sim/models/distance_sensor/model.config rename to src/avoidance/sim/models/distance_sensor/model.config diff --git a/avoidance/sim/models/iris_realsense/iris_realsense.sdf b/src/avoidance/sim/models/iris_realsense/iris_realsense.sdf similarity index 100% rename from avoidance/sim/models/iris_realsense/iris_realsense.sdf rename to src/avoidance/sim/models/iris_realsense/iris_realsense.sdf diff --git a/avoidance/sim/models/iris_realsense/model.config b/src/avoidance/sim/models/iris_realsense/model.config similarity index 100% rename from avoidance/sim/models/iris_realsense/model.config rename to src/avoidance/sim/models/iris_realsense/model.config diff --git a/avoidance/sim/models/lamp_with_lines/lamp_with_lines.dae b/src/avoidance/sim/models/lamp_with_lines/lamp_with_lines.dae similarity index 100% rename from avoidance/sim/models/lamp_with_lines/lamp_with_lines.dae rename to src/avoidance/sim/models/lamp_with_lines/lamp_with_lines.dae diff --git a/avoidance/sim/models/lamp_with_lines/model.config b/src/avoidance/sim/models/lamp_with_lines/model.config similarity index 100% rename from avoidance/sim/models/lamp_with_lines/model.config rename to src/avoidance/sim/models/lamp_with_lines/model.config diff --git a/avoidance/sim/models/lamp_with_lines/model.sdf b/src/avoidance/sim/models/lamp_with_lines/model.sdf similarity index 100% rename from avoidance/sim/models/lamp_with_lines/model.sdf rename to src/avoidance/sim/models/lamp_with_lines/model.sdf diff --git a/avoidance/sim/models/line_model/line.dae b/src/avoidance/sim/models/line_model/line.dae similarity index 100% rename from avoidance/sim/models/line_model/line.dae rename to src/avoidance/sim/models/line_model/line.dae diff --git a/avoidance/sim/models/line_model/model.config b/src/avoidance/sim/models/line_model/model.config similarity index 100% rename from avoidance/sim/models/line_model/model.config rename to src/avoidance/sim/models/line_model/model.config diff --git a/avoidance/sim/models/line_model/model.sdf b/src/avoidance/sim/models/line_model/model.sdf similarity index 100% rename from avoidance/sim/models/line_model/model.sdf rename to src/avoidance/sim/models/line_model/model.sdf diff --git a/avoidance/sim/models/powerline/model.config b/src/avoidance/sim/models/powerline/model.config similarity index 100% rename from avoidance/sim/models/powerline/model.config rename to src/avoidance/sim/models/powerline/model.config diff --git a/avoidance/sim/models/powerline/model.sdf b/src/avoidance/sim/models/powerline/model.sdf similarity index 100% rename from avoidance/sim/models/powerline/model.sdf rename to src/avoidance/sim/models/powerline/model.sdf diff --git a/avoidance/sim/models/powerline/powerline.dae b/src/avoidance/sim/models/powerline/powerline.dae similarity index 100% rename from avoidance/sim/models/powerline/powerline.dae rename to src/avoidance/sim/models/powerline/powerline.dae diff --git a/avoidance/sim/models/stereo_camera_vertical/model.config b/src/avoidance/sim/models/stereo_camera_vertical/model.config similarity index 100% rename from avoidance/sim/models/stereo_camera_vertical/model.config rename to src/avoidance/sim/models/stereo_camera_vertical/model.config diff --git a/avoidance/sim/models/stereo_camera_vertical/stereo_camera_vertical.sdf b/src/avoidance/sim/models/stereo_camera_vertical/stereo_camera_vertical.sdf similarity index 100% rename from avoidance/sim/models/stereo_camera_vertical/stereo_camera_vertical.sdf rename to src/avoidance/sim/models/stereo_camera_vertical/stereo_camera_vertical.sdf diff --git a/avoidance/sim/worlds/boxes1.world b/src/avoidance/sim/worlds/boxes1.world similarity index 100% rename from avoidance/sim/worlds/boxes1.world rename to src/avoidance/sim/worlds/boxes1.world diff --git a/avoidance/sim/worlds/boxes1.yaml b/src/avoidance/sim/worlds/boxes1.yaml similarity index 100% rename from avoidance/sim/worlds/boxes1.yaml rename to src/avoidance/sim/worlds/boxes1.yaml diff --git a/avoidance/sim/worlds/boxes2.world b/src/avoidance/sim/worlds/boxes2.world similarity index 100% rename from avoidance/sim/worlds/boxes2.world rename to src/avoidance/sim/worlds/boxes2.world diff --git a/avoidance/sim/worlds/boxes2.yaml b/src/avoidance/sim/worlds/boxes2.yaml similarity index 100% rename from avoidance/sim/worlds/boxes2.yaml rename to src/avoidance/sim/worlds/boxes2.yaml diff --git a/avoidance/sim/worlds/boxes3.world b/src/avoidance/sim/worlds/boxes3.world similarity index 100% rename from avoidance/sim/worlds/boxes3.world rename to src/avoidance/sim/worlds/boxes3.world diff --git a/avoidance/sim/worlds/boxes3.yaml b/src/avoidance/sim/worlds/boxes3.yaml similarity index 100% rename from avoidance/sim/worlds/boxes3.yaml rename to src/avoidance/sim/worlds/boxes3.yaml diff --git a/avoidance/sim/worlds/boxes4.world b/src/avoidance/sim/worlds/boxes4.world similarity index 100% rename from avoidance/sim/worlds/boxes4.world rename to src/avoidance/sim/worlds/boxes4.world diff --git a/avoidance/sim/worlds/boxes4.yaml b/src/avoidance/sim/worlds/boxes4.yaml similarity index 100% rename from avoidance/sim/worlds/boxes4.yaml rename to src/avoidance/sim/worlds/boxes4.yaml diff --git a/avoidance/sim/worlds/boxes5.world b/src/avoidance/sim/worlds/boxes5.world similarity index 100% rename from avoidance/sim/worlds/boxes5.world rename to src/avoidance/sim/worlds/boxes5.world diff --git a/avoidance/sim/worlds/boxes5.yaml b/src/avoidance/sim/worlds/boxes5.yaml similarity index 100% rename from avoidance/sim/worlds/boxes5.yaml rename to src/avoidance/sim/worlds/boxes5.yaml diff --git a/avoidance/sim/worlds/boxes6.world b/src/avoidance/sim/worlds/boxes6.world similarity index 100% rename from avoidance/sim/worlds/boxes6.world rename to src/avoidance/sim/worlds/boxes6.world diff --git a/avoidance/sim/worlds/boxes6.yaml b/src/avoidance/sim/worlds/boxes6.yaml similarity index 100% rename from avoidance/sim/worlds/boxes6.yaml rename to src/avoidance/sim/worlds/boxes6.yaml diff --git a/avoidance/sim/worlds/moving_boxes.world b/src/avoidance/sim/worlds/moving_boxes.world similarity index 100% rename from avoidance/sim/worlds/moving_boxes.world rename to src/avoidance/sim/worlds/moving_boxes.world diff --git a/avoidance/sim/worlds/outdoor_village.world b/src/avoidance/sim/worlds/outdoor_village.world similarity index 100% rename from avoidance/sim/worlds/outdoor_village.world rename to src/avoidance/sim/worlds/outdoor_village.world diff --git a/avoidance/sim/worlds/outdoor_village_3.world b/src/avoidance/sim/worlds/outdoor_village_3.world similarity index 100% rename from avoidance/sim/worlds/outdoor_village_3.world rename to src/avoidance/sim/worlds/outdoor_village_3.world diff --git a/avoidance/sim/worlds/simple_obstacle.world b/src/avoidance/sim/worlds/simple_obstacle.world similarity index 100% rename from avoidance/sim/worlds/simple_obstacle.world rename to src/avoidance/sim/worlds/simple_obstacle.world diff --git a/avoidance/sim/worlds/simple_obstacle.yaml b/src/avoidance/sim/worlds/simple_obstacle.yaml similarity index 100% rename from avoidance/sim/worlds/simple_obstacle.yaml rename to src/avoidance/sim/worlds/simple_obstacle.yaml diff --git a/avoidance/sim/worlds/test_city_2.world b/src/avoidance/sim/worlds/test_city_2.world similarity index 100% rename from avoidance/sim/worlds/test_city_2.world rename to src/avoidance/sim/worlds/test_city_2.world diff --git a/avoidance/sim/worlds/test_city_4.world b/src/avoidance/sim/worlds/test_city_4.world similarity index 100% rename from avoidance/sim/worlds/test_city_4.world rename to src/avoidance/sim/worlds/test_city_4.world diff --git a/avoidance/sim/worlds/window.world b/src/avoidance/sim/worlds/window.world similarity index 100% rename from avoidance/sim/worlds/window.world rename to src/avoidance/sim/worlds/window.world diff --git a/avoidance/sim/worlds/window.yaml b/src/avoidance/sim/worlds/window.yaml similarity index 100% rename from avoidance/sim/worlds/window.yaml rename to src/avoidance/sim/worlds/window.yaml diff --git a/avoidance/src/avoidance_node.cpp b/src/avoidance/src/avoidance_node.cpp similarity index 100% rename from avoidance/src/avoidance_node.cpp rename to src/avoidance/src/avoidance_node.cpp diff --git a/avoidance/src/common.cpp b/src/avoidance/src/common.cpp similarity index 98% rename from avoidance/src/common.cpp rename to src/avoidance/src/common.cpp index f97ce79b8..4b4eaed38 100644 --- a/avoidance/src/common.cpp +++ b/src/avoidance/src/common.cpp @@ -253,6 +253,14 @@ float getPitchFromQuaternion(const Eigen::Quaternionf q) { return pitch * RAD_TO_DEG; } +geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) { + tf2::Quaternion quat_tf; + quat_tf.setRPY( 0, 0, yaw ); + geometry_msgs::msg::Quaternion quat_msg; + quat_msg = tf2::toMsg(quat_tf); + return quat_msg; +} + float wrapAngleToPlusMinusPI(float angle) { return angle - 2.0f * M_PI_F * std::floor(angle / (2.0f * M_PI_F) + 0.5f); } float wrapAngleToPlusMinus180(float angle) { return angle - 360.f * std::floor(angle / 360.f + 0.5f); } @@ -304,7 +312,7 @@ void transformToTrajectory(px4_msgs::msg::VehicleTrajectoryWaypoint& obst_avoid, fillUnusedTrajectoryPoint(obst_avoid.waypoints[3]); fillUnusedTrajectoryPoint(obst_avoid.waypoints[4]); - for (size_t i = 0; i < sizeof(obst_avoid.waypoints); i++) { + for (size_t i = 0; i < obst_avoid.waypoints.size(); i++) { obst_avoid.waypoints[i].timestamp = obst_avoid.timestamp; obst_avoid.waypoints[i].point_valid = false; } diff --git a/avoidance/src/histogram.cpp b/src/avoidance/src/histogram.cpp similarity index 100% rename from avoidance/src/histogram.cpp rename to src/avoidance/src/histogram.cpp diff --git a/avoidance/src/rviz_world_loader.cpp b/src/avoidance/src/rviz_world_loader.cpp similarity index 100% rename from avoidance/src/rviz_world_loader.cpp rename to src/avoidance/src/rviz_world_loader.cpp diff --git a/avoidance/src/transform_buffer.cpp b/src/avoidance/src/transform_buffer.cpp similarity index 94% rename from avoidance/src/transform_buffer.cpp rename to src/avoidance/src/transform_buffer.cpp index 2fdfa7b54..91a5a8566 100644 --- a/avoidance/src/transform_buffer.cpp +++ b/src/avoidance/src/transform_buffer.cpp @@ -26,9 +26,10 @@ bool TransformBuffer::interpolateTransform(const geometry_msgs::msg::TransformSt tf2::Vector3 tf_earlier_translation; tf2::Quaternion tf_earlier_rotation; tf2::Quaternion tf_later_rotation; - tf2::fromMsg(tf_earlier_translation, tf_earlier.transform.translation); - tf2::fromMsg(tf_earlier_rotation, tf_earlier.transform.rotation); - tf2::fromMsg(tf_later_rotation, tf_later.transform.rotation); + tf_earlier_translation.setValue(tf_earlier.transform.translation.x, tf_earlier.transform.translation.y, tf_earlier.transform.translation.z); + // tf2::fromMsg(tf_earlier.transform.translation, tf_earlier_translation); + tf2::fromMsg(tf_earlier.transform.rotation, tf_earlier_rotation); + tf2::fromMsg(tf_later.transform.rotation, tf_later_rotation); const tf2::Vector3 translation = tf_earlier_translation * (1.f - tau) + tf_earlier_translation * tau; const tf2::Quaternion rotation = tf_earlier_rotation.slerp(tf_later_rotation, tau); diff --git a/avoidance/test/main.cpp b/src/avoidance/test/main.cpp similarity index 100% rename from avoidance/test/main.cpp rename to src/avoidance/test/main.cpp diff --git a/avoidance/test/test_common.cpp b/src/avoidance/test/test_common.cpp similarity index 100% rename from avoidance/test/test_common.cpp rename to src/avoidance/test/test_common.cpp diff --git a/avoidance/test/test_transform_buffer.cpp b/src/avoidance/test/test_transform_buffer.cpp similarity index 100% rename from avoidance/test/test_transform_buffer.cpp rename to src/avoidance/test/test_transform_buffer.cpp diff --git a/avoidance/test/test_usm.cpp b/src/avoidance/test/test_usm.cpp similarity index 100% rename from avoidance/test/test_usm.cpp rename to src/avoidance/test/test_usm.cpp diff --git a/avoidance/test/test_usm.cpp.dot b/src/avoidance/test/test_usm.cpp.dot similarity index 100% rename from avoidance/test/test_usm.cpp.dot rename to src/avoidance/test/test_usm.cpp.dot diff --git a/src/global_planner/CMakeLists.txt b/src/global_planner/CMakeLists.txt new file mode 100644 index 000000000..5088e687b --- /dev/null +++ b/src/global_planner/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.5) +project(global_planner) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +# Disable Wredundant-decls warnings since rosidl generates redundant function declarations +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++1z") +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(avoidance REQUIRED) +find_package(octomap REQUIRED) +find_package(octomap_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/PathWithRiskMsg.msg" + DEPENDENCIES + std_msgs + geometry_msgs +) + +# find_package PCL interferes with message generation(https://github.com/ros2/rosidl/issues/402). It should be below of rosidl_generate_interfaces + +find_package(PCL 1.8 REQUIRED) + +if(DISABLE_SIMULATION) + message(STATUS "Building avoidance without Gazebo Simulation") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDISABLE_SIMULATION") +endif() + +include_directories( + include + ${avoidance_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + ${octomap_msgs_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_sensor_msgs_INCLUDE_DIRS} + src/library + src/nodes) + +link_directories(${Boost_LIBRARY_DIRS}) + +set(GLOBAL_PLANNER_CPP_FILES + "src/library/cell.cpp" + "src/library/global_planner.cpp" + "src/library/node.cpp" + "src/nodes/global_planner_node.cpp" + "src/nodes/global_planner_node_main.cpp" +) + +add_executable(global_planner_node + ${GLOBAL_PLANNER_CPP_FILES}) + +target_link_libraries(global_planner_node + ${Boost_LIBRARIES} + ${tf2_LIBRARIES} + ${PCL_LIBRARIES} + ${OCTOMAP_LIBRARIES} +) + +ament_target_dependencies(global_planner_node + rclcpp + avoidance + px4_msgs + geometry_msgs + std_msgs + sensor_msgs + nav_msgs + octomap + octomap_msgs + visualization_msgs + tf2 + tf2_ros + tf2_sensor_msgs + pcl) + +rosidl_target_interfaces(global_planner_node ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Install header files +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" +) + +# Install artifacts +install(TARGETS global_planner_node + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +install(TARGETS global_planner_node + RUNTIME DESTINATION bin +) + +add_library(avoidance SHARED ${GLOBAL_PLANNER_CPP_FILES}) + +ament_export_libraries(avoidance) + +ament_package() diff --git a/global_planner/cfg/GlobalPlannerNode.cfg b/src/global_planner/cfg/GlobalPlannerNode.cfg old mode 100755 new mode 100644 similarity index 100% rename from global_planner/cfg/GlobalPlannerNode.cfg rename to src/global_planner/cfg/GlobalPlannerNode.cfg diff --git a/global_planner/include/global_planner/analysis.h b/src/global_planner/include/global_planner/analysis.h similarity index 79% rename from global_planner/include/global_planner/analysis.h rename to src/global_planner/include/global_planner/analysis.h index d4c903eb6..2292a30c6 100644 --- a/global_planner/include/global_planner/analysis.h +++ b/src/global_planner/include/global_planner/analysis.h @@ -92,7 +92,7 @@ void printPathStats(GlobalPlanner* global_planner, const std::vector& path double num_45_deg_turns = std::ceil(ang_diff3 / (M_PI / 4)); // Minimum number of 45-turns to goal printf("\t|| \t%3.2f \t%3.2f \t%3.2f \t%3.2f \t%3.2f \t%3.2f \n", u_ang, goal_ang, ang_diff, ang_diff2, ang_diff3, num_45_deg_turns); - ROS_INFO("WTF? \n %f %f \n\n\n\n\n\n\n\n\n\n\n\n", angleToRange(5.5), angleToRange(-5.5)); + // ROS_INFO("WTF? \n %f %f \n\n\n\n\n\n\n\n\n\n\n\n", angleToRange(5.5), angleToRange(-5.5)); } } printf("\n\n"); @@ -102,35 +102,35 @@ void printPathStats(GlobalPlanner* global_planner, const std::vector& path template void printPointStats(GlobalPlanner* global_planner, double x, double y, double z) { Cell cell(x, y, z); - ROS_INFO("\n\nDEBUG INFO FOR %s", cell.asString().c_str()); - ROS_INFO("Rist cost: %2.2f", global_planner->risk_factor_ * global_planner->getRisk(cell)); - ROS_INFO("getRisk: %2.2f", global_planner->getRisk(cell)); - ROS_INFO("singleCellRisk: %2.2f", global_planner->getSingleCellRisk(cell)); - ROS_INFO( - "Neighbors:\n \t %2.2f \t \t \t %2.2f \n %2.2f \t \t %2.2f \n \t %2.2f " - "\t \t \t %2.2f", - global_planner->getSingleCellRisk(Cell(x, y + 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z + 1)), - global_planner->getSingleCellRisk(Cell(x - 1, y, z)), global_planner->getSingleCellRisk(Cell(x + 1, y, z)), - global_planner->getSingleCellRisk(Cell(x, y - 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z - 1))); + // ROS_INFO("\n\nDEBUG INFO FOR %s", cell.asString().c_str()); + // ROS_INFO("Rist cost: %2.2f", global_planner->risk_factor_ * global_planner->getRisk(cell)); + // ROS_INFO("getRisk: %2.2f", global_planner->getRisk(cell)); + // ROS_INFO("singleCellRisk: %2.2f", global_planner->getSingleCellRisk(cell)); + // ROS_INFO( + // "Neighbors:\n \t %2.2f \t \t \t %2.2f \n %2.2f \t \t %2.2f \n \t %2.2f " + // "\t \t \t %2.2f", + // global_planner->getSingleCellRisk(Cell(x, y + 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z + 1)), + // global_planner->getSingleCellRisk(Cell(x - 1, y, z)), global_planner->getSingleCellRisk(Cell(x + 1, y, z)), + // global_planner->getSingleCellRisk(Cell(x, y - 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z - 1))); double heuristics = global_planner->getHeuristic(Node(cell, cell), global_planner->goal_pos_); - ROS_INFO("Heuristics: %2.2f", heuristics); + // ROS_INFO("Heuristics: %2.2f", heuristics); octomap::OcTreeNode* node = global_planner->octree_->search(x, y, z); if (node) { double prob = octomap::probability(node->getValue()); double post_prob = posterior(global_planner->getAltPrior(cell), prob); - ROS_INFO("prob: %2.2f \t post_prob: %2.2f", prob, post_prob); + // ROS_INFO("prob: %2.2f \t post_prob: %2.2f", prob, post_prob); if (global_planner->occupied_.find(cell) != global_planner->occupied_.end()) { - ROS_INFO("Cell in occupied, posterior: %2.2f", post_prob); + // ROS_INFO("Cell in occupied, posterior: %2.2f", post_prob); } else { - ROS_INFO("Cell NOT in occupied, posterior: %2.2f", global_planner->expore_penalty_ * post_prob); + // ROS_INFO("Cell NOT in occupied, posterior: %2.2f", global_planner->expore_penalty_ * post_prob); } } else { - ROS_INFO("Cell not in tree, prob: %2.2f", global_planner->expore_penalty_ * global_planner->getAltPrior(cell)); + // ROS_INFO("Cell not in tree, prob: %2.2f", global_planner->expore_penalty_ * global_planner->getAltPrior(cell)); } } } // namespace global_planner -#endif /* GLOBAL_PLANNER_ANALYSIS_H_ */ +#endif /* GLOBAL_PLANNER_ANALYSIS_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/bezier.h b/src/global_planner/include/global_planner/bezier.h similarity index 90% rename from global_planner/include/global_planner/bezier.h rename to src/global_planner/include/global_planner/bezier.h index 1333530ac..265179e74 100644 --- a/global_planner/include/global_planner/bezier.h +++ b/src/global_planner/include/global_planner/bezier.h @@ -36,7 +36,7 @@ std::vector

threePointBezier(const P& p0, const P& p1, const P& p2, int num_s // Returns a quadratic Bezier-curve starting in p0 and and ending in p2 template -nav_msgs::Path threePointBezier(const Path& path, int num_steps = 10) { +nav_msgs::msg::Path threePointBezier(const Path& path, int num_steps = 10) { if (path.poses.size() != 3) { printf("Path size error, %d != 3 \n", static_cast(path.poses.size())); return path; @@ -119,14 +119,15 @@ double getAccelerationMagnitude(const P& p0, const P& p1, const P& p2, double du } template -nav_msgs::Path pathToTriplets(const nav_msgs::Path& path, std::vector triplets, std::vector speed) { +nav_msgs::msg::Path pathToTriplets(const nav_msgs::msg::Path& path, std::vector triplets, + std::vector speed) { if (path.poses.size() < 3) { return path; } // Extract the points from path, duplicate the first and last point to // indicate acceleration at the beginning and deceleration at the end - std::vector points; + std::vector points; points.push_back(path.poses.front().pose.position); for (auto pose : path.poses) { points.push_back(pose.pose.position); @@ -134,9 +135,9 @@ nav_msgs::Path pathToTriplets(const nav_msgs::Path& path, std::vector points.push_back(path.poses.back().pose.position); for (int i = 1; i < path.poses.size(); i++) { - geometry_msgs::Point prev = middlePoint(points[i - 1], points[i]); - geometry_msgs::Point ctrl = points[i]; - geometry_msgs::Point next = middlePoint(points[i], points[i + 1]); + geometry_msgs::msg::Point prev = middlePoint(points[i - 1], points[i]); + geometry_msgs::msg::Point ctrl = points[i]; + geometry_msgs::msg::Point next = middlePoint(points[i], points[i + 1]); ; BezierMsg msg; fillBezierMsg(msg, prev, ctrl, next, 1.0); @@ -144,4 +145,4 @@ nav_msgs::Path pathToTriplets(const nav_msgs::Path& path, std::vector } } // namespace global_planner -#endif /* GLOBAL_PLANNER_BEZIER_H_ */ +#endif /* GLOBAL_PLANNER_BEZIER_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/cell.h b/src/global_planner/include/global_planner/cell.h similarity index 95% rename from global_planner/include/global_planner/cell.h rename to src/global_planner/include/global_planner/cell.h index 5eb30d38a..40557a930 100644 --- a/global_planner/include/global_planner/cell.h +++ b/src/global_planner/include/global_planner/cell.h @@ -5,7 +5,7 @@ #include #include -#include +#include #include "global_planner/common.h" @@ -19,7 +19,7 @@ class Cell { Cell(std::tuple new_tuple); Cell(double x, double y, double z); Cell(double x, double y); - Cell(geometry_msgs::Point point); + Cell(geometry_msgs::msg::Point point); // Cell(Eigen::Vector3d point); // Get the indices of the Cell @@ -32,7 +32,7 @@ class Cell { double yPos() const; double zPos() const; - geometry_msgs::Point toPoint() const; + geometry_msgs::msg::Point toPoint() const; double manhattanDist(double _x, double _y, double _z) const; double distance2D(const Cell& b) const; @@ -110,4 +110,4 @@ struct hash { } // namespace std -#endif // GLOBAL_PLANNER_CELL +#endif // GLOBAL_PLANNER_CELL \ No newline at end of file diff --git a/global_planner/include/global_planner/common.h b/src/global_planner/include/global_planner/common.h similarity index 95% rename from global_planner/include/global_planner/common.h rename to src/global_planner/include/global_planner/common.h index 04ebe423c..8f63b694f 100644 --- a/global_planner/include/global_planner/common.h +++ b/src/global_planner/include/global_planner/common.h @@ -3,6 +3,7 @@ #include // sqrt #include +#include "rclcpp/rclcpp.hpp" namespace global_planner { @@ -92,7 +93,7 @@ double distance(const P& p1, const P& p2) { return norm((p2.x - p1.x), (p2.y - p1.y), (p2.z - p1.z)); } -inline double clocksToMicroSec(std::clock_t start, std::clock_t end) { +inline double clocksToMicroSec(clock_t start, clock_t end) { return (end - start) / (double)(CLOCKS_PER_SEC / 1000000); } @@ -113,4 +114,4 @@ inline double posterior(double p, double prior) { } // namespace global_planner -#endif /* GLOBAL_PLANNER_COMMON_H_ */ +#endif /* GLOBAL_PLANNER_COMMON_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/common_ros.h b/src/global_planner/include/global_planner/common_ros.h similarity index 57% rename from global_planner/include/global_planner/common_ros.h rename to src/global_planner/include/global_planner/common_ros.h index b72d1aee2..58c497a12 100644 --- a/global_planner/include/global_planner/common_ros.h +++ b/src/global_planner/include/global_planner/common_ros.h @@ -3,12 +3,15 @@ #include -#include #include // sqrt -#include -#include -#include // getYaw createQuaternionMsgFromYaw -#include +#include +#include +#include // getYaw createQuaternionMsgFromYaw +#include +#include +#include +#include +#include #include "global_planner/common.h" // hasSameYawAndAltitude @@ -19,30 +22,33 @@ namespace global_planner { // GLOBAL PLANNER template -tf::Vector3 toTfVector3(const P& point) { - return tf::Vector3(point.x, point.y, point.z); +tf2::Vector3 toTfVector3(const P& point) { + return tf2::Vector3(point.x, point.y, point.z); } -inline double distance(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b) { +inline double distance(const geometry_msgs::msg::PoseStamped& a, const geometry_msgs::msg::PoseStamped& b) { return distance(a.pose.position, b.pose.position); } -inline geometry_msgs::TwistStamped transformTwistMsg(const tf::TransformListener& listener, - const std::string& target_frame, const std::string& fixed_frame, - const geometry_msgs::TwistStamped& msg) { +// https://github.com/trainman419/fiducials_ros/blob/master/src/fiducials_localization.cpp +inline geometry_msgs::msg::TwistStamped transformTwistMsg(const tf2_ros::TransformListener& listener, + const std::string& target_frame, + const std::string& fixed_frame, + const geometry_msgs::msg::TwistStamped& msg) { auto transformed_msg = msg; - geometry_msgs::Vector3Stamped before; + geometry_msgs::msg::Vector3Stamped before; before.vector = msg.twist.linear; before.header = msg.header; - geometry_msgs::Vector3Stamped after; - listener.transformVector(target_frame, ros::Time(0), before, fixed_frame, after); + geometry_msgs::msg::Vector3Stamped after; + // TODO :: apply tf2 transformVector + // listener.transformVector(target_frame, rclcpp::Time(0), before, fixed_frame, after); transformed_msg.twist.linear = after.vector; return transformed_msg; } // Returns a spectral color between red (0.0) and blue (1.0) -inline std_msgs::ColorRGBA spectralColor(double hue, double alpha = 1.0) { - std_msgs::ColorRGBA color; +inline std_msgs::msg::ColorRGBA spectralColor(double hue, double alpha = 1.0) { + std_msgs::msg::ColorRGBA color; color.r = std::max(0.0, 2 * hue - 1); color.g = 1.0 - 2.0 * std::abs(hue - 0.5); color.b = std::max(0.0, 1.0 - 2 * hue); @@ -51,15 +57,15 @@ inline std_msgs::ColorRGBA spectralColor(double hue, double alpha = 1.0) { } template -visualization_msgs::Marker createMarker(int id, Point position, Color color, double scale = 0.1, - std::string frame_id = "/world") { - visualization_msgs::Marker marker; +visualization_msgs::msg::Marker createMarker(int id, Point position, Color color, double scale = 0.1, + std::string frame_id = "/world") { + visualization_msgs::msg::Marker marker; marker.id = id; marker.header.frame_id = frame_id; - marker.header.stamp = ros::Time(); + marker.header.stamp = rclcpp::Time(); marker.pose.position = position; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; marker.scale.x = marker.scale.y = marker.scale.z = scale; marker.color = color; return marker; @@ -73,12 +79,12 @@ visualization_msgs::Marker createMarker(int id, Point position, Color color, dou // } // Returns true if msg1 and msg2 have both the same altitude and orientation -inline bool hasSameYawAndAltitude(const geometry_msgs::Pose& msg1, const geometry_msgs::Pose& msg2) { +inline bool hasSameYawAndAltitude(const geometry_msgs::msg::Pose& msg1, const geometry_msgs::msg::Pose& msg2) { return msg1.orientation.z == msg2.orientation.z && msg1.orientation.w == msg2.orientation.w && msg1.position.z == msg2.position.z; } -inline double pathLength(const nav_msgs::Path& path) { +inline double pathLength(const nav_msgs::msg::Path& path) { double total_dist = 0.0; for (int i = 1; i < path.poses.size(); ++i) { total_dist += distance(path.poses[i - 1], path.poses[i]); @@ -87,8 +93,9 @@ inline double pathLength(const nav_msgs::Path& path) { } // Returns a path with only the corner points of msg -inline std::vector filterPathCorners(const std::vector& msg) { - std::vector corners = msg; +inline std::vector filterPathCorners( + const std::vector& msg) { + std::vector corners = msg; corners.clear(); if (msg.size() < 1) { return corners; @@ -97,9 +104,9 @@ inline std::vector filterPathCorners(const std::vect int n = msg.size(); corners.push_back(msg.front()); for (int i = 1; i < n - 1; ++i) { - geometry_msgs::Point last = msg[i - 1].pose.position; - geometry_msgs::Point curr = msg[i].pose.position; - geometry_msgs::Point next = msg[i + 1].pose.position; + geometry_msgs::msg::Point last = msg[i - 1].pose.position; + geometry_msgs::msg::Point curr = msg[i].pose.position; + geometry_msgs::msg::Point next = msg[i + 1].pose.position; bool same_x = (next.x - curr.x) == (curr.x - last.x); bool same_y = (next.y - curr.y) == (curr.y - last.y); bool same_z = (next.z - curr.z) == (curr.z - last.z); @@ -111,7 +118,7 @@ inline std::vector filterPathCorners(const std::vect return corners; } -inline double pathKineticEnergy(const nav_msgs::Path& path) { +inline double pathKineticEnergy(const nav_msgs::msg::Path& path) { if (path.poses.size() < 3) { return 0.0; } @@ -133,7 +140,7 @@ inline double pathKineticEnergy(const nav_msgs::Path& path) { return total_energy; } -inline double pathEnergy(const nav_msgs::Path& path, double up_penalty) { +inline double pathEnergy(const nav_msgs::msg::Path& path, double up_penalty) { double total_energy = 0.0; for (int i = 1; i < path.poses.size(); ++i) { total_energy += distance(path.poses[i - 1], path.poses[i]); @@ -145,4 +152,4 @@ inline double pathEnergy(const nav_msgs::Path& path, double up_penalty) { } // namespace global_planner -#endif /* GLOBAL_PLANNER_COMMON_ROS_H_ */ +#endif /* GLOBAL_PLANNER_COMMON_ROS_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/global_planner.h b/src/global_planner/include/global_planner/global_planner.h similarity index 86% rename from global_planner/include/global_planner/global_planner.h rename to src/global_planner/include/global_planner/global_planner.h index 0059335c8..844a5bb77 100644 --- a/global_planner/include/global_planner/global_planner.h +++ b/src/global_planner/include/global_planner/global_planner.h @@ -10,15 +10,16 @@ #include #include -#include -#include -#include // getYaw createQuaternionMsgFromYaw +#include +#include +#include +#include #include #include -#include -#include +// #include +// #include #include "global_planner/analysis.h" #include "global_planner/cell.h" #include "global_planner/common.h" @@ -58,9 +59,9 @@ class GlobalPlanner { // TODO: rename and remove not needed std::vector path_back_; - geometry_msgs::Point curr_pos_; + geometry_msgs::msg::Point curr_pos_; double curr_yaw_; - geometry_msgs::Vector3 curr_vel_; + geometry_msgs::msg::Vector3 curr_vel_; GoalCell goal_pos_ = GoalCell(0.5, 0.5, 3.5); bool going_back_ = true; // we start by just finding the start position @@ -99,7 +100,8 @@ class GlobalPlanner { void calculateAccumulatedHeightPrior(); - void setPose(const geometry_msgs::PoseStamped& new_pose); + void setPose(const geometry_msgs::msg::PoseStamped::SharedPtr new_pose, const double yaw); + // void setPose(const px4_msgs::msg::VehicleLocalPosition::SharedPtr new_pose); void setGoal(const GoalCell& goal); void setPath(const std::vector& path); void setFrame(std::string frame_id); @@ -116,7 +118,7 @@ class GlobalPlanner { bool isLegal(const Node& node); double getRisk(const Cell& cell); double getRisk(const Node& node); - double getRiskOfCurve(const std::vector& msg); + double getRiskOfCurve(const std::vector& msg); double getTurnSmoothness(const Node& u, const Node& v); double getEdgeCost(const Node& u, const Node& v); @@ -126,10 +128,10 @@ class GlobalPlanner { double altitudeHeuristic(const Cell& u, const Cell& goal); double getHeuristic(const Node& u, const Cell& goal); - geometry_msgs::PoseStamped createPoseMsg(const Cell& cell, double yaw); - nav_msgs::Path getPathMsg(); - nav_msgs::Path getPathMsg(const std::vector& path); - PathWithRiskMsg getPathWithRiskMsg(); + geometry_msgs::msg::PoseStamped createPoseMsg(const Cell& cell, double yaw); + nav_msgs::msg::Path getPathMsg(); + nav_msgs::msg::Path getPathMsg(const std::vector& path); + // avoidance_msgs::msg::PathWithRiskMsg getPathWithRiskMsg(); PathInfo getPathInfo(const std::vector& path); NodePtr getStartNode(const Cell& start, const Cell& parent, const std::string& type); diff --git a/src/global_planner/include/global_planner/global_planner_node.h b/src/global_planner/include/global_planner/global_planner_node.h new file mode 100644 index 000000000..9a156bd38 --- /dev/null +++ b/src/global_planner/include/global_planner/global_planner_node.h @@ -0,0 +1,146 @@ +#ifndef GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H +#define GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H + +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "avoidance/avoidance_node.h" +#include "global_planner/global_planner.h" + +#ifndef DISABLE_SIMULATION +#include +#endif + +namespace global_planner { + +using std::placeholders::_1; +using namespace std::chrono_literals; + +struct cameraData { + rclcpp::Subscription::SharedPtr pointcloud_sub_; +}; + +class GlobalPlannerNode : public rclcpp::Node { + public: + // TODO: Deque instead of vector + GlobalPlanner global_planner_; + std::vector waypoints_; // Intermediate goals, from file, mavros + // mission or intermediate goals + GlobalPlannerNode(); + ~GlobalPlannerNode(); + + private: + std::mutex mutex_; + + // Subscribers + // ros::Subscriber octomap_sub_; + rclcpp::Subscription::SharedPtr octomap_full_sub_; + rclcpp::Subscription::SharedPtr position_sub_; + rclcpp::Subscription::SharedPtr clicked_point_sub_; + // ros::Subscriber move_base_simple_sub_; + // ros::Subscriber laser_sensor_sub_; + // ros::Subscriber fcu_input_sub_; + + // Publishers + rclcpp::Publisher::SharedPtr global_temp_path_pub_; + rclcpp::Publisher::SharedPtr smooth_path_pub_; + rclcpp::Publisher::SharedPtr actual_path_pub_; + rclcpp::Publisher::SharedPtr explored_cells_pub_; + rclcpp::Publisher::SharedPtr global_goal_pub_; + rclcpp::Publisher::SharedPtr global_temp_goal_pub_; + rclcpp::Publisher::SharedPtr mavros_obstacle_free_path_pub_; + rclcpp::Publisher::SharedPtr mavros_waypoint_publisher_; + rclcpp::Publisher::SharedPtr current_waypoint_publisher_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + + rclcpp::Time start_time_; + rclcpp::Time last_wp_time_; + + rclcpp::TimerBase::SharedPtr gp_cmdloop_timer_; + rclcpp::TimerBase::SharedPtr gp_plannerloop_timer_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + nav_msgs::msg::Path actual_path_; + geometry_msgs::msg::Point start_pos_; + geometry_msgs::msg::PoseStamped current_goal_; + geometry_msgs::msg::PoseStamped last_goal_; + geometry_msgs::msg::PoseStamped last_pos_; + + std::vector last_clicked_points; + std::vector path_; + std::vector cameras_; + + int num_octomap_msg_ = 0; + int num_pos_msg_ = 0; + std::chrono::milliseconds gp_cmdloop_dt_; + std::chrono::milliseconds gp_plannerloop_dt_; + double mapupdate_dt_; + double min_speed_; + double speed_ = min_speed_; + double start_yaw_; + bool position_received_; + std::string frame_id_; + + double clicked_goal_alt_; + double clicked_goal_radius_; + bool hover_; + int simplify_iterations_; + double simplify_margin_; + + avoidance::AvoidanceNode avoidance_node_; +#ifndef DISABLE_SIMULATION + std::unique_ptr world_visualizer_; +#endif + void readParams(); + void initializeCameraSubscribers(std::vector& camera_topics); + void setNewGoal(const GoalCell& goal); + void popNextGoal(); + void planPath(); + void setIntermediateGoal(); + bool isCloseToGoal(); + void setCurrentPath(const std::vector& poses); + void positionCallback(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + void clickedPointCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg); + // void moveBaseSimpleCallback(const geometry_msgs::msg::PoseStamped& msg); + void octomapFullCallback(const octomap_msgs::msg::Octomap::SharedPtr msg); + void depthCameraCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + // void fcuInputGoalCallback(const mavros_msgs::Trajectory& msg); + void cmdLoopCallback(); + void plannerLoopCallback(); + void publishGoal(const GoalCell& goal); + void publishPath(); + void publishSetpoint(); + void printPointInfo(double x, double y, double z); +}; + +} // namespace global_planner + +#endif // GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H diff --git a/global_planner/include/global_planner/node.h b/src/global_planner/include/global_planner/node.h similarity index 99% rename from global_planner/include/global_planner/node.h rename to src/global_planner/include/global_planner/node.h index 49c134fdd..e0e0c9871 100644 --- a/global_planner/include/global_planner/node.h +++ b/src/global_planner/include/global_planner/node.h @@ -118,4 +118,4 @@ struct hash { } // namespace std -#endif // GLOBAL_PLANNER_NODE +#endif // GLOBAL_PLANNER_NODE \ No newline at end of file diff --git a/global_planner/include/global_planner/search_tools.h b/src/global_planner/include/global_planner/search_tools.h similarity index 93% rename from global_planner/include/global_planner/search_tools.h rename to src/global_planner/include/global_planner/search_tools.h index 65ac372e9..693667172 100644 --- a/global_planner/include/global_planner/search_tools.h +++ b/src/global_planner/include/global_planner/search_tools.h @@ -31,33 +31,33 @@ struct SearchInfo { inline void printSearchInfo(SearchInfo info, std::string node_type = "Node", double overestimate_factor = 1.0) { double avg_time = info.search_time / info.num_iter; - std::cout << std::setw(20) << std::left << node_type << std::setw(10) << std::setprecision(3) << avg_time - << std::setw(10) << std::setprecision(3) << overestimate_factor << std::setw(10) << info.num_iter - << std::setw(10) << 0.0; + // std::cout << std::setw(20) << std::left << node_type << std::setw(10) << std::setprecision(3) << avg_time + // << std::setw(10) << std::setprecision(3) << overestimate_factor << std::setw(10) << info.num_iter + // << std::setw(10) << 0.0; } // Returns a path where corners are smoothed with quadratic Bezier-curves -inline nav_msgs::Path smoothPath(const nav_msgs::Path& path) { +inline nav_msgs::msg::Path smoothPath(const nav_msgs::msg::Path& path) { if (path.poses.size() < 3) { return path; } - nav_msgs::Path smooth_path; + nav_msgs::msg::Path smooth_path; smooth_path.header = path.header; // Repeat the first and last points to get the first half of the first edge // and the second half of the last edge smooth_path.poses.push_back((path.poses.front())); for (int i = 2; i < path.poses.size(); i++) { - geometry_msgs::Point p0 = path.poses[i - 2].pose.position; - geometry_msgs::Point p1 = path.poses[i - 1].pose.position; - geometry_msgs::Point p2 = path.poses[i].pose.position; + geometry_msgs::msg::Point p0 = path.poses[i - 2].pose.position; + geometry_msgs::msg::Point p1 = path.poses[i - 1].pose.position; + geometry_msgs::msg::Point p2 = path.poses[i].pose.position; p0 = middlePoint(p0, p1); p2 = middlePoint(p1, p2); - std::vector smooth_turn = threePointBezier(p0, p1, p2); + std::vector smooth_turn = threePointBezier(p0, p1, p2); for (const auto& point : smooth_turn) { - geometry_msgs::PoseStamped pose_msg = path.poses.front(); // Copy the original header info + geometry_msgs::msg::PoseStamped pose_msg = path.poses.front(); // Copy the original header info pose_msg.pose.position = point; smooth_path.poses.push_back(pose_msg); } @@ -353,4 +353,4 @@ bool findPathOld(GlobalPlanner* global_planner, std::vector& path, const C } } // namespace global_planner -#endif /* GLOBAL_PLANNER_SEARCH_TOOLS_H_ */ +#endif /* GLOBAL_PLANNER_SEARCH_TOOLS_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/visitor.h b/src/global_planner/include/global_planner/visitor.h similarity index 95% rename from global_planner/include/global_planner/visitor.h rename to src/global_planner/include/global_planner/visitor.h index dc386d65b..1c9a1ec0f 100644 --- a/global_planner/include/global_planner/visitor.h +++ b/src/global_planner/include/global_planner/visitor.h @@ -37,4 +37,4 @@ class NullVisitor { } // namespace global_planner -#endif // GLOBAL_PLANNER_VISITOR +#endif // GLOBAL_PLANNER_VISITOR \ No newline at end of file diff --git a/global_planner/launch/global_planner_depth-camera.launch b/src/global_planner/launch/global_planner_depth-camera.launch similarity index 100% rename from global_planner/launch/global_planner_depth-camera.launch rename to src/global_planner/launch/global_planner_depth-camera.launch diff --git a/global_planner/launch/global_planner_octomap.launch b/src/global_planner/launch/global_planner_octomap.launch similarity index 100% rename from global_planner/launch/global_planner_octomap.launch rename to src/global_planner/launch/global_planner_octomap.launch diff --git a/global_planner/launch/global_planner_sitl_3cam.launch b/src/global_planner/launch/global_planner_sitl_3cam.launch similarity index 100% rename from global_planner/launch/global_planner_sitl_3cam.launch rename to src/global_planner/launch/global_planner_sitl_3cam.launch diff --git a/global_planner/launch/global_planner_stereo.launch b/src/global_planner/launch/global_planner_stereo.launch similarity index 100% rename from global_planner/launch/global_planner_stereo.launch rename to src/global_planner/launch/global_planner_stereo.launch diff --git a/global_planner/msg/PathWithRiskMsg.msg b/src/global_planner/msg/PathWithRiskMsg.msg similarity index 68% rename from global_planner/msg/PathWithRiskMsg.msg rename to src/global_planner/msg/PathWithRiskMsg.msg index 837aa4140..bd4c1296d 100644 --- a/global_planner/msg/PathWithRiskMsg.msg +++ b/src/global_planner/msg/PathWithRiskMsg.msg @@ -1,3 +1,3 @@ -Header header +std_msgs/Header header geometry_msgs/PoseStamped[] poses float64[] risks \ No newline at end of file diff --git a/src/global_planner/package.xml b/src/global_planner/package.xml new file mode 100644 index 000000000..8428aacb7 --- /dev/null +++ b/src/global_planner/package.xml @@ -0,0 +1,38 @@ + + + + global_planner + 1.0.0 + PX4 Avoidance global planner ROS2 version + pixhawk + Dongoo Lee + + TODO + + ament_cmake + rosidl_default_generators + + ros_environment + rclcpp + px4_msgs + std_msgs + geometry_msgs + sensor_msgs + avoidance + tf2 + tf2_ros + pcl + octomap + octomap_msgs + avoidance_msgs + visualization_msgs + + builtin_interfaces + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/global_planner/resource/global_planner.rviz b/src/global_planner/resource/global_planner.rviz similarity index 100% rename from global_planner/resource/global_planner.rviz rename to src/global_planner/resource/global_planner.rviz diff --git a/global_planner/resource/parameter_file b/src/global_planner/resource/parameter_file similarity index 100% rename from global_planner/resource/parameter_file rename to src/global_planner/resource/parameter_file diff --git a/global_planner/resource/px4_config.yaml b/src/global_planner/resource/px4_config.yaml similarity index 100% rename from global_planner/resource/px4_config.yaml rename to src/global_planner/resource/px4_config.yaml diff --git a/global_planner/resource/random_goals b/src/global_planner/resource/random_goals similarity index 100% rename from global_planner/resource/random_goals rename to src/global_planner/resource/random_goals diff --git a/global_planner/resource/sample_output_from_mock_data b/src/global_planner/resource/sample_output_from_mock_data similarity index 100% rename from global_planner/resource/sample_output_from_mock_data rename to src/global_planner/resource/sample_output_from_mock_data diff --git a/global_planner/src/library/cell.cpp b/src/global_planner/src/library/cell.cpp similarity index 96% rename from global_planner/src/library/cell.cpp rename to src/global_planner/src/library/cell.cpp index 80aeacb0b..021fbf155 100644 --- a/global_planner/src/library/cell.cpp +++ b/src/global_planner/src/library/cell.cpp @@ -6,7 +6,7 @@ Cell::Cell() = default; Cell::Cell(std::tuple new_tuple) : tpl_(new_tuple) {} Cell::Cell(double x, double y, double z) : tpl_(floor(x / CELL_SCALE), floor(y / CELL_SCALE), floor(z / CELL_SCALE)) {} Cell::Cell(double x, double y) : Cell(x, y, 0.0) {} -Cell::Cell(geometry_msgs::Point point) : Cell(point.x, point.y, point.z) {} +Cell::Cell(geometry_msgs::msg::Point point) : Cell(point.x, point.y, point.z) {} int Cell::xIndex() const { return std::get<0>(tpl_); } int Cell::yIndex() const { return std::get<1>(tpl_); } @@ -16,8 +16,8 @@ double Cell::xPos() const { return CELL_SCALE * (xIndex() + 0.5); } double Cell::yPos() const { return CELL_SCALE * (yIndex() + 0.5); } double Cell::zPos() const { return CELL_SCALE * (zIndex() + 0.5); } -geometry_msgs::Point Cell::toPoint() const { - geometry_msgs::Point point; +geometry_msgs::msg::Point Cell::toPoint() const { + geometry_msgs::msg::Point point; point.x = xPos(); point.y = yPos(); point.z = zPos(); @@ -109,4 +109,4 @@ std::string Cell::asString() const { return s; } -} // namespace global_planner +} // namespace global_planner \ No newline at end of file diff --git a/global_planner/src/library/global_planner.cpp b/src/global_planner/src/library/global_planner.cpp similarity index 90% rename from global_planner/src/library/global_planner.cpp rename to src/global_planner/src/library/global_planner.cpp index 5f65e4fd8..bcab273ba 100644 --- a/global_planner/src/library/global_planner.cpp +++ b/src/global_planner/src/library/global_planner.cpp @@ -27,9 +27,11 @@ void GlobalPlanner::calculateAccumulatedHeightPrior() { } // Updates the current pose and keeps track of the path back -void GlobalPlanner::setPose(const geometry_msgs::PoseStamped& new_pose) { - curr_pos_ = new_pose.pose.position; - curr_yaw_ = tf::getYaw(new_pose.pose.orientation); +void GlobalPlanner::setPose(const geometry_msgs::msg::PoseStamped::SharedPtr new_pose, const double yaw) { + curr_pos_ = new_pose->pose.position; + // curr_yaw_ = tf2::getYaw(new_pose->pose.orientation); + curr_yaw_ = yaw; // get Yaw directly from px4_msgs::msg::VehicleLocalPosition. + Cell curr_cell = Cell(curr_pos_); if (!going_back_ && (path_back_.empty() || curr_cell != path_back_.back())) { // Keep track of where we have been, add current position to path_back_ if @@ -204,9 +206,9 @@ double GlobalPlanner::getRisk(const Node& node) { // Returns the risk of the quadratic Bezier curve defined by poses // TODO: think about this -double GlobalPlanner::getRiskOfCurve(const std::vector& msg) { +double GlobalPlanner::getRiskOfCurve(const std::vector& msg) { if (msg.size() != 3) { - ROS_INFO("Bezier msg must have 3 points"); + // ROS_INFO("Bezier msg must have 3 points"); return -1; } @@ -332,18 +334,29 @@ double GlobalPlanner::getHeuristic(const Node& u, const Cell& goal) { return heuristic; } -geometry_msgs::PoseStamped GlobalPlanner::createPoseMsg(const Cell& cell, double yaw) { - geometry_msgs::PoseStamped pose_msg; +geometry_msgs::msg::PoseStamped GlobalPlanner::createPoseMsg(const Cell& cell, double yaw) { + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = frame_id_; pose_msg.pose.position = cell.toPoint(); - pose_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); + // pose_msg.pose.orientation = tf2::createQuaternionMsgFromYaw(yaw); // tf version + + // tf2 version + tf2::Quaternion quaternion; + quaternion.setRPY(0, 0, yaw); + geometry_msgs::msg::Quaternion qOri; + qOri.x = quaternion.x(); + qOri.y = quaternion.y(); + qOri.z = quaternion.z(); + qOri.w = quaternion.w(); + pose_msg.pose.orientation = qOri; + return pose_msg; } -nav_msgs::Path GlobalPlanner::getPathMsg() { return getPathMsg(curr_path_); } +nav_msgs::msg::Path GlobalPlanner::getPathMsg() { return getPathMsg(curr_path_); } -nav_msgs::Path GlobalPlanner::getPathMsg(const std::vector& path) { - nav_msgs::Path path_msg; +nav_msgs::msg::Path GlobalPlanner::getPathMsg(const std::vector& path) { + nav_msgs::msg::Path path_msg; path_msg.header.frame_id = frame_id_; if (path.size() == 0) { @@ -366,18 +379,18 @@ nav_msgs::Path GlobalPlanner::getPathMsg(const std::vector& path) { return path_msg; } -PathWithRiskMsg GlobalPlanner::getPathWithRiskMsg() { - nav_msgs::Path path_msg = getPathMsg(); - PathWithRiskMsg risk_msg; - risk_msg.header = path_msg.header; - risk_msg.poses = path_msg.poses; +// avoidance_msgs::msg::PathWithRiskMsg GlobalPlanner::getPathWithRiskMsg() { +// nav_msgs::msg::Path path_msg = getPathMsg(); +// avoidance_msgs::msg::PathWithRiskMsg risk_msg; +// risk_msg.header = path_msg.header; +// risk_msg.poses = path_msg.poses; - for (const auto& pose : path_msg.poses) { - double risk = getRisk(Cell(pose.pose.position)); - risk_msg.risks.push_back(risk); - } - return risk_msg; -} +// for (const auto& pose : path_msg.poses) { +// double risk = getRisk(Cell(pose.pose.position)); +// risk_msg.risks.push_back(risk); +// } +// return risk_msg; +// } // Returns details of the cost of the path PathInfo GlobalPlanner::getPathInfo(const std::vector& path) { @@ -421,9 +434,9 @@ bool GlobalPlanner::findPath(std::vector& path) { Cell parent_of_s = s; // Ignore the current yaw } - ROS_INFO("Planning a path from %s to %s", s.asString().c_str(), t.asString().c_str()); - ROS_INFO("curr_pos_: %2.2f,%2.2f,%2.2f\t s: %2.2f,%2.2f,%2.2f", curr_pos_.x, curr_pos_.y, curr_pos_.z, s.xPos(), - s.yPos(), s.zPos()); + // ROS_INFO("Planning a path from %s to %s", s.asString().c_str(), t.asString().c_str()); + // ROS_INFO("curr_pos_: %2.2f,%2.2f,%2.2f\t s: %2.2f,%2.2f,%2.2f", curr_pos_.x, curr_pos_.y, curr_pos_.z, s.xPos(), + // s.yPos(), s.zPos()); bool found_path = false; double best_path_cost = INFINITY; @@ -486,12 +499,12 @@ bool GlobalPlanner::getGlobalPath() { if (goal_must_be_free_ && getRisk(t) > max_cell_risk_) { // If goal is occupied, no path is published - ROS_INFO("Goal position is occupied"); + // ROS_INFO("Goal position is occupied"); goal_is_blocked_ = true; return false; } else if (current_cell_blocked_) { // If current position is occupied the way back is published - ROS_INFO("Current position is occupied, going back."); + // ROS_INFO("Current position is occupied, going back."); // goBack(); // return true; return false; @@ -500,7 +513,7 @@ bool GlobalPlanner::getGlobalPath() { std::vector path; if (!findPath(path)) { double goal_risk = getRisk(t); - ROS_INFO(" Failed to find a path, risk of t: %3.2f", goal_risk); + // ROS_INFO(" Failed to find a path, risk of t: %3.2f", goal_risk); goal_is_blocked_ = true; return false; } @@ -512,7 +525,7 @@ bool GlobalPlanner::getGlobalPath() { // Sets the current path to be the path back until a safe cell is reached // Then the mission can be tried again or a new mission can be set void GlobalPlanner::goBack() { - ROS_INFO(" GO BACK "); + // ROS_INFO(" GO BACK "); going_back_ = true; std::vector new_path = path_back_; std::reverse(new_path.begin(), new_path.end()); @@ -536,4 +549,4 @@ void GlobalPlanner::stop() { void GlobalPlanner::setRobotRadius(double radius) { robot_radius_ = radius; } -} // namespace global_planner +} // namespace global_planner \ No newline at end of file diff --git a/global_planner/src/library/node.cpp b/src/global_planner/src/library/node.cpp similarity index 99% rename from global_planner/src/library/node.cpp rename to src/global_planner/src/library/node.cpp index 6f631f038..f37bb6ad3 100644 --- a/global_planner/src/library/node.cpp +++ b/src/global_planner/src/library/node.cpp @@ -81,4 +81,4 @@ std::string Node::asString() const { return s; } -} // namespace global_planner +} // namespace global_planner \ No newline at end of file diff --git a/src/global_planner/src/nodes/global_planner_node.cpp b/src/global_planner/src/nodes/global_planner_node.cpp new file mode 100644 index 000000000..682b9f058 --- /dev/null +++ b/src/global_planner/src/nodes/global_planner_node.cpp @@ -0,0 +1,425 @@ +#include "global_planner/global_planner_node.h" + +namespace global_planner { + +GlobalPlannerNode::GlobalPlannerNode() + : Node("global_planner_node"), gp_cmdloop_dt_(100ms), gp_plannerloop_dt_(1000ms), start_yaw_(0.0) { + RCLCPP_INFO_ONCE(this->get_logger(), "GlobalPlannerNode STARTED!"); + +// GlobalPlannerNode::GlobalPlannerNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) +// : nh_(nh), +// nh_private_(nh_private), +// avoidance_node_(nh, nh_private), +// cmdloop_dt_(0.1), +// plannerloop_dt_(1.0), +// mapupdate_dt_(0.2), +// start_yaw_(0.0) { + +#ifndef DISABLE_SIMULATION + world_visualizer_.reset(new avoidance::WorldVisualizer()); +#endif + + // Read Ros parameters + readParams(); + + // Subscribers + rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(); + octomap_full_sub_ = this->create_subscription( + "/octomap_full", qos, std::bind(&GlobalPlannerNode::octomapFullCallback, this, _1)); + position_sub_ = this->create_subscription( + "VehicleLocalPosition_PubSubTopic", qos, std::bind(&GlobalPlannerNode::positionCallback, this, _1)); + clicked_point_sub_ = this->create_subscription( + "/clicked_point", qos, std::bind(&GlobalPlannerNode::clickedPointCallback, this, _1)); + // move_base_simple_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &GlobalPlannerNode::moveBaseSimpleCallback, + // this); + // fcu_input_sub_ = nh_.subscribe("/mavros/trajectory/desired", 1, &GlobalPlannerNode::fcuInputGoalCallback, this); + + // Publishers + global_temp_path_pub_ = this->create_publisher("/global_temp_path", 10); + actual_path_pub_ = this->create_publisher("/actual_path", 10); + smooth_path_pub_ = this->create_publisher("/smooth_path", 10); + global_goal_pub_ = this->create_publisher("/global_goal", 10); + global_temp_goal_pub_ = this->create_publisher("/global_temp_goal", 10); + explored_cells_pub_ = this->create_publisher("/explored_cells", 10); + mavros_waypoint_publisher_ = this->create_publisher("/setpoint_position/local", 10); + mavros_obstacle_free_path_pub_ = + this->create_publisher("/trajectory/generated", 10); + current_waypoint_publisher_ = this->create_publisher("/current_setpoint", 10); + pointcloud_pub_ = this->create_publisher("/cloud_in", 10); + + actual_path_.header.frame_id = frame_id_; + + gp_cmdloop_timer_ = this->create_wall_timer(gp_cmdloop_dt_, [&]() { cmdLoopCallback(); }); + gp_plannerloop_timer_ = this->create_wall_timer(gp_plannerloop_dt_, [&]() { plannerLoopCallback(); }); + + current_goal_.header.frame_id = frame_id_; + current_goal_.pose.position = start_pos_; + current_goal_.pose.orientation = avoidance::createQuaternionMsgFromYaw(start_yaw_); + last_goal_ = current_goal_; + + speed_ = 1.0; + + start_time_ = rclcpp::Clock().now(); +} + +GlobalPlannerNode::~GlobalPlannerNode() {} + +void GlobalPlannerNode::readParams() { + std::vector camera_topics; + + this->declare_parameter("frame_id", "/local_origin"); + + this->get_parameter("frame_id", frame_id_); + this->get_parameter_or("start_pos_x", start_pos_.x, 0.5); + this->get_parameter_or("start_pos_y", start_pos_.y, 0.5); + this->get_parameter_or("start_pos_z", start_pos_.z, 3.5); + + this->get_parameter("pointcloud_topics", camera_topics); + camera_topics.push_back("/camera/points"); + + // initializeCameraSubscribers(camera_topics); + global_planner_.goal_pos_ = GoalCell(start_pos_.x, start_pos_.y, start_pos_.z); + double robot_radius; + this->get_parameter_or("robot_radius", robot_radius, 0.5); + global_planner_.setFrame(frame_id_); + global_planner_.setRobotRadius(robot_radius); +} + +void GlobalPlannerNode::initializeCameraSubscribers(std::vector& camera_topics) { + cameras_.resize(camera_topics.size()); + + for (size_t i = 0; i < camera_topics.size(); i++) { + cameras_[i].pointcloud_sub_ = this->create_subscription( + camera_topics[i], 1, std::bind(&GlobalPlannerNode::depthCameraCallback, this, _1)); + } +} + +// Sets a new goal, plans a path to it and publishes some info +void GlobalPlannerNode::setNewGoal(const GoalCell& goal) { + RCLCPP_INFO(this->get_logger(), "========== Set goal : %s ==========", goal.asString().c_str()); + global_planner_.setGoal(goal); + publishGoal(goal); +} + +// Sets the next waypoint to be the current goal +void GlobalPlannerNode::popNextGoal() { + if (!waypoints_.empty()) { + // Set the first goal in waypoints_ as the new goal + GoalCell new_goal = waypoints_.front(); + waypoints_.erase(waypoints_.begin()); + setNewGoal(new_goal); + } else if (global_planner_.goal_is_blocked_) { + // Goal is blocked but there is no other goal in waypoints_, just stop + RCLCPP_INFO(this->get_logger(), " STOP "); + global_planner_.stop(); + } +} + +// Plans a new path and publishes it +void GlobalPlannerNode::planPath() { + std::clock_t start_time = std::clock(); + if (global_planner_.octree_) { + RCLCPP_INFO(this->get_logger(), "OctoMap memory usage: %2.3f MB", + global_planner_.octree_->memoryUsage() / 1000000.0); + } + + bool found_path = global_planner_.getGlobalPath(); + + if (!found_path) { + // TODO: popNextGoal(), instead of checking if goal_is_blocked in + // positionCallback? + RCLCPP_INFO(this->get_logger(), "Failed to find a path"); + } else if (global_planner_.overestimate_factor_ > 1.05) { + // The path is not good enough, set an intermediate goal on the path + setIntermediateGoal(); + } + printf("Total time: %2.2f ms \n", (std::clock() - start_time) / (double)(CLOCKS_PER_SEC / 1000)); +} + +// Sets a temporary goal on the path to the current goal +void GlobalPlannerNode::setIntermediateGoal() { + int curr_path_length = global_planner_.curr_path_.size(); + if (curr_path_length > 10) { + printf("\n ===== Half-way path ====== \n"); + waypoints_.insert(waypoints_.begin(), global_planner_.goal_pos_); + Cell middle_cell = global_planner_.curr_path_[curr_path_length / 2]; + setNewGoal(GoalCell(middle_cell, curr_path_length / 4, true)); + } +} + +// void GlobalPlannerNode::dynamicReconfigureCallback(global_planner::GlobalPlannerNodeConfig& config, uint32_t level) { +// // global_planner_ +// global_planner_.min_altitude_ = config.min_altitude_; +// global_planner_.max_altitude_ = config.max_altitude_; +// global_planner_.max_cell_risk_ = config.max_cell_risk_; +// global_planner_.smooth_factor_ = config.smooth_factor_; +// global_planner_.vert_to_hor_cost_ = config.vert_to_hor_cost_; +// global_planner_.risk_factor_ = config.risk_factor_; +// global_planner_.neighbor_risk_flow_ = config.neighbor_risk_flow_; +// global_planner_.expore_penalty_ = config.expore_penalty_; +// global_planner_.up_cost_ = config.up_cost_; +// global_planner_.down_cost_ = config.down_cost_; +// global_planner_.search_time_ = config.search_time_; +// global_planner_.min_overestimate_factor_ = config.min_overestimate_factor_; +// global_planner_.max_overestimate_factor_ = config.max_overestimate_factor_; +// global_planner_.max_iterations_ = config.max_iterations_; +// global_planner_.goal_must_be_free_ = config.goal_must_be_free_; +// global_planner_.use_current_yaw_ = config.use_current_yaw_; +// global_planner_.use_risk_heuristics_ = config.use_risk_heuristics_; +// global_planner_.use_speedup_heuristics_ = config.use_speedup_heuristics_; + +// // global_planner_node +// clicked_goal_alt_ = config.clicked_goal_alt_; +// clicked_goal_radius_ = config.clicked_goal_radius_; +// simplify_iterations_ = config.simplify_iterations_; +// simplify_margin_ = config.simplify_margin_; + +// // cell +// if (level == 2) { +// CELL_SCALE = config.CELL_SCALE; +// } + +// // node +// if (level == 4) { +// SPEEDNODE_RADIUS = config.SPEEDNODE_RADIUS; +// global_planner_.default_node_type_ = config.default_node_type_; +// } +// } + +// Sets the current position and checks if the current goal has been reached +void GlobalPlannerNode::positionCallback(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) { + RCLCPP_INFO_ONCE(this->get_logger(), "GlobalPlannerNode::positionCallback"); + + // TODO : convert VehicleLocalPosition to PoseStamped + auto pose = std::make_shared(); + pose->pose.position.x = msg->x; + pose->pose.position.y = msg->y; + pose->pose.position.z = msg->z; + pose->pose.orientation = avoidance::createQuaternionMsgFromYaw(msg->yaw); + + // Update position + last_pos_ = *pose; + global_planner_.setPose(pose, msg->yaw); + + // Update velocity + auto vel = std::make_shared(); + vel->x = msg->vx; + vel->y = msg->vy; + vel->z = msg->vz; + global_planner_.curr_vel_ = *vel; + + // Check if a new goal is needed + if (num_pos_msg_++ % 10 == 0) { + // Keep track of and publish the actual travel trajectory + RCLCPP_INFO(this->get_logger(), "Travelled path extended"); + last_pos_.header.frame_id = frame_id_; + actual_path_.poses.push_back(last_pos_); + actual_path_pub_->publish(actual_path_); + } + + position_received_ = true; + + // Check if we are close enough to current goal to get the next part of the + // path + if (path_.size() > 0 && isCloseToGoal()) { + double yaw1 = tf2::getYaw(current_goal_.pose.orientation); + double yaw2 = tf2::getYaw(last_pos_.pose.orientation); + double yaw_diff = std::abs(yaw2 - yaw1); + // Transform yaw_diff to [0, 2*pi] + yaw_diff -= std::floor(yaw_diff / (2 * M_PI)) * (2 * M_PI); + double max_yaw_diff = M_PI / 1.0; + if (yaw_diff < max_yaw_diff || yaw_diff > 2 * M_PI - max_yaw_diff) { + // If we are facing the right direction, then pop the first point of the + // path + last_goal_ = current_goal_; + current_goal_ = path_[0]; + path_.erase(path_.begin()); + } + } +} + +void GlobalPlannerNode::clickedPointCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg) { + printPointInfo(msg->point.x, msg->point.y, msg->point.z); + + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose.position = msg->point; + pose.pose.position.z = global_planner_.curr_pos_.z; + last_clicked_points.push_back(pose); +} + +// void GlobalPlannerNode::moveBaseSimpleCallback(const geometry_msgs::msg::PoseStamped& msg) { +// setNewGoal(GoalCell(msg.pose.position.x, msg.pose.position.y, clicked_goal_alt_, clicked_goal_radius_)); +// } + +// void GlobalPlannerNode::fcuInputGoalCallback(const mavros_msgs::msg::Trajectory& msg) { +// const GoalCell new_goal = GoalCell(msg.point_2.position.x, msg.point_2.position.y, msg.point_2.position.z, 1.0); +// if (msg.point_valid[1] == true && ((std::fabs(global_planner_.goal_pos_.xPos() - new_goal.xPos()) > 0.001) || +// (std::fabs(global_planner_.goal_pos_.yPos() - new_goal.yPos()) > 0.001))) { +// setNewGoal(new_goal); +// } +// } + +// // Check if the current path is blocked +void GlobalPlannerNode::octomapFullCallback(const octomap_msgs::msg::Octomap::SharedPtr msg) { + std::lock_guard lock(mutex_); + + rclcpp::Time current = rclcpp::Clock().now(); + // Update map at a fixed rate. This is useful on setting replanning rates for the planner. + if ((current - last_wp_time_).seconds() < mapupdate_dt_) { + return; + } + last_wp_time_ = rclcpp::Clock().now(); + + octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*msg); + + global_planner_.updateFullOctomap(tree); +} + +// Go through obstacle points and store them +void GlobalPlannerNode::depthCameraCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "depthCameraCallback called!"); + try { + // Transform msg from camera frame to world frame + rclcpp::Time now = rclcpp::Clock().now(); + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + sensor_msgs::msg::PointCloud2 transformed_msg; + + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped = tf_buffer_->lookupTransform("base_link", msg->header.frame_id, + tf2_ros::fromMsg(msg->header.stamp), tf2::durationFromSec(5.0)); + tf2::doTransform(*msg, transformed_msg, transformStamped); + + pcl::PointCloud cloud; // Easier to loop through pcl::PointCloud + pcl::fromROSMsg(transformed_msg, cloud); + + // Store the obstacle points + for (const auto& p : cloud) { + if (!std::isnan(p.x)) { + // TODO: Not all points end up here + Cell occupied_cell(p.x, p.y, p.z); + global_planner_.occupied_.insert(occupied_cell); + } + } + pointcloud_pub_->publish(*msg); + } catch (tf2::TransformException const& ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN(this->get_logger(), "Transformation not available local_origin to /camera_link"); + } +} + +void GlobalPlannerNode::setCurrentPath(const std::vector& poses) { + path_.clear(); + + if (poses.size() < 2) { + RCLCPP_INFO(this->get_logger(), " Received empty path\n"); + return; + } + last_goal_ = poses[0]; + current_goal_ = poses[1]; + + for (int i = 2; i < poses.size(); ++i) { + path_.push_back(poses[i]); + } +} + +void GlobalPlannerNode::cmdLoopCallback() { + hover_ = false; + + // Check if all information was received + rclcpp::Time now = rclcpp::Clock().now(); + + rclcpp::Duration since_last_cloud = now - last_wp_time_; + rclcpp::Duration since_start = now - start_time_; + + avoidance_node_.checkFailsafe(since_last_cloud, since_start, hover_); + publishSetpoint(); +} + +void GlobalPlannerNode::plannerLoopCallback() { + std::lock_guard lock(mutex_); + bool is_in_goal = global_planner_.goal_pos_.withinPositionRadius(global_planner_.curr_pos_); + if (is_in_goal || global_planner_.goal_is_blocked_) { + popNextGoal(); + } + + planPath(); + + // Print and publish info + if (is_in_goal && !waypoints_.empty()) { + RCLCPP_INFO(this->get_logger(), "Reached current goal %s, %d goals left\n\n", + global_planner_.goal_pos_.asString().c_str(), (int)waypoints_.size()); + RCLCPP_INFO(this->get_logger(), "Actual travel distance: %2.2f \t Actual energy usage: %2.2f", + pathLength(actual_path_), pathEnergy(actual_path_, global_planner_.up_cost_)); + } + + publishPath(); +} + +// Publish the position of goal +void GlobalPlannerNode::publishGoal(const GoalCell& goal) { + geometry_msgs::msg::PointStamped pointMsg; + pointMsg.header.frame_id = frame_id_; + pointMsg.point = goal.toPoint(); + + // Always publish as temporary to remove any obsolete temporary path + global_temp_goal_pub_->publish(pointMsg); + if (!goal.is_temporary_) { + global_goal_pub_->publish(pointMsg); + } +} + +// Publish the current path +void GlobalPlannerNode::publishPath() { + auto path_msg = global_planner_.getPathMsg(); + // PathWithRiskMsg risk_msg = global_planner_.getPathWithRiskMsg(); + // Always publish as temporary to remove any obsolete temporary path + global_temp_path_pub_->publish(path_msg); + setCurrentPath(path_msg.poses); + smooth_path_pub_->publish(smoothPath(path_msg)); + + auto simple_path = simplifyPath(&global_planner_, global_planner_.curr_path_, simplify_iterations_, simplify_margin_); + auto simple_path_msg = global_planner_.getPathMsg(simple_path); + global_temp_path_pub_->publish(simple_path_msg); + setCurrentPath(simple_path_msg.poses); + smooth_path_pub_->publish(smoothPath(simple_path_msg)); +} + +// Prints information about the point, mostly the risk of the containing cell +void GlobalPlannerNode::printPointInfo(double x, double y, double z) { + // Update explored cells + printPointStats(&global_planner_, x, y, z); +} + +void GlobalPlannerNode::publishSetpoint() { + // Vector pointing from current position to the current goal + tf2::Vector3 vec = toTfVector3(subtractPoints(current_goal_.pose.position, last_pos_.pose.position)); + // If we are less than 1.0 away, then we should stop at the goal + double new_len = vec.length() < 1.0 ? vec.length() : speed_; + vec.normalize(); + vec *= new_len; + + auto setpoint = current_goal_; // The intermediate position sent to Mavros + setpoint.pose.position.x = last_pos_.pose.position.x + vec.getX(); + setpoint.pose.position.y = last_pos_.pose.position.y + vec.getY(); + setpoint.pose.position.z = last_pos_.pose.position.z + vec.getZ(); + + // Publish setpoint for vizualization + current_waypoint_publisher_->publish(setpoint); + + // Publish setpoint to Mavros + mavros_waypoint_publisher_->publish(setpoint); + px4_msgs::msg::VehicleTrajectoryWaypoint obst_free_path; + geometry_msgs::msg::Twist velocity_setpoint{}; + velocity_setpoint.linear.x = NAN; + velocity_setpoint.linear.y = NAN; + velocity_setpoint.linear.z = NAN; + + avoidance::transformToTrajectory(obst_free_path, setpoint, velocity_setpoint); + mavros_obstacle_free_path_pub_->publish(obst_free_path); +} + +bool GlobalPlannerNode::isCloseToGoal() { return distance(current_goal_, last_pos_) < 1.5; } + +} // namespace global_planner \ No newline at end of file diff --git a/src/global_planner/src/nodes/global_planner_node_main.cpp b/src/global_planner/src/nodes/global_planner_node_main.cpp new file mode 100644 index 000000000..e8770da61 --- /dev/null +++ b/src/global_planner/src/nodes/global_planner_node_main.cpp @@ -0,0 +1,10 @@ +#include "global_planner/global_planner_node.h" +#include + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto global_planner_nd = std::make_shared(); + RCLCPP_INFO_ONCE(global_planner_nd->get_logger(), "global_planner_nd spin START"); + rclcpp::spin(global_planner_nd); + return 0; +} diff --git a/global_planner/test/main.cpp b/src/global_planner/test/main.cpp similarity index 100% rename from global_planner/test/main.cpp rename to src/global_planner/test/main.cpp diff --git a/global_planner/test/test_example.cpp b/src/global_planner/test/test_example.cpp similarity index 100% rename from global_planner/test/test_example.cpp rename to src/global_planner/test/test_example.cpp diff --git a/local_planner/CMakeLists.txt b/src/local_planner/CMakeLists.txt similarity index 100% rename from local_planner/CMakeLists.txt rename to src/local_planner/CMakeLists.txt diff --git a/local_planner/cfg/LocalPlannerNode.cfg b/src/local_planner/cfg/LocalPlannerNode.cfg old mode 100755 new mode 100644 similarity index 100% rename from local_planner/cfg/LocalPlannerNode.cfg rename to src/local_planner/cfg/LocalPlannerNode.cfg diff --git a/local_planner/cfg/vehicle.yaml b/src/local_planner/cfg/vehicle.yaml similarity index 100% rename from local_planner/cfg/vehicle.yaml rename to src/local_planner/cfg/vehicle.yaml diff --git a/local_planner/include/local_planner/avoidance_output.h b/src/local_planner/include/local_planner/avoidance_output.h similarity index 100% rename from local_planner/include/local_planner/avoidance_output.h rename to src/local_planner/include/local_planner/avoidance_output.h diff --git a/local_planner/include/local_planner/candidate_direction.h b/src/local_planner/include/local_planner/candidate_direction.h similarity index 100% rename from local_planner/include/local_planner/candidate_direction.h rename to src/local_planner/include/local_planner/candidate_direction.h diff --git a/local_planner/include/local_planner/cost_parameters.h b/src/local_planner/include/local_planner/cost_parameters.h similarity index 100% rename from local_planner/include/local_planner/cost_parameters.h rename to src/local_planner/include/local_planner/cost_parameters.h diff --git a/local_planner/include/local_planner/local_planner.h b/src/local_planner/include/local_planner/local_planner.h similarity index 100% rename from local_planner/include/local_planner/local_planner.h rename to src/local_planner/include/local_planner/local_planner.h diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/src/local_planner/include/local_planner/local_planner_visualization.h similarity index 100% rename from local_planner/include/local_planner/local_planner_visualization.h rename to src/local_planner/include/local_planner/local_planner_visualization.h diff --git a/local_planner/include/local_planner/planner_functions.h b/src/local_planner/include/local_planner/planner_functions.h similarity index 100% rename from local_planner/include/local_planner/planner_functions.h rename to src/local_planner/include/local_planner/planner_functions.h diff --git a/local_planner/include/local_planner/star_planner.h b/src/local_planner/include/local_planner/star_planner.h similarity index 100% rename from local_planner/include/local_planner/star_planner.h rename to src/local_planner/include/local_planner/star_planner.h diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/src/local_planner/include/local_planner/trajectory_simulator.h similarity index 100% rename from local_planner/include/local_planner/trajectory_simulator.h rename to src/local_planner/include/local_planner/trajectory_simulator.h diff --git a/local_planner/include/local_planner/tree_node.h b/src/local_planner/include/local_planner/tree_node.h similarity index 100% rename from local_planner/include/local_planner/tree_node.h rename to src/local_planner/include/local_planner/tree_node.h diff --git a/local_planner/include/local_planner/waypoint_generator.h b/src/local_planner/include/local_planner/waypoint_generator.h similarity index 100% rename from local_planner/include/local_planner/waypoint_generator.h rename to src/local_planner/include/local_planner/waypoint_generator.h diff --git a/local_planner/launch/local_planner_aero.launch b/src/local_planner/launch/local_planner_aero.launch similarity index 100% rename from local_planner/launch/local_planner_aero.launch rename to src/local_planner/launch/local_planner_aero.launch diff --git a/local_planner/launch/local_planner_aeroD415.launch b/src/local_planner/launch/local_planner_aeroD415.launch similarity index 100% rename from local_planner/launch/local_planner_aeroD415.launch rename to src/local_planner/launch/local_planner_aeroD415.launch diff --git a/local_planner/launch/local_planner_depth-camera.launch b/src/local_planner/launch/local_planner_depth-camera.launch similarity index 100% rename from local_planner/launch/local_planner_depth-camera.launch rename to src/local_planner/launch/local_planner_depth-camera.launch diff --git a/local_planner/launch/local_planner_sitl_3cam.launch b/src/local_planner/launch/local_planner_sitl_3cam.launch similarity index 100% rename from local_planner/launch/local_planner_sitl_3cam.launch rename to src/local_planner/launch/local_planner_sitl_3cam.launch diff --git a/local_planner/launch/local_planner_stereo.launch b/src/local_planner/launch/local_planner_stereo.launch similarity index 100% rename from local_planner/launch/local_planner_stereo.launch rename to src/local_planner/launch/local_planner_stereo.launch diff --git a/local_planner/launch/log_replay.launch b/src/local_planner/launch/log_replay.launch similarity index 100% rename from local_planner/launch/log_replay.launch rename to src/local_planner/launch/log_replay.launch diff --git a/local_planner/launch/mavros.launch b/src/local_planner/launch/mavros.launch similarity index 100% rename from local_planner/launch/mavros.launch rename to src/local_planner/launch/mavros.launch diff --git a/local_planner/launch/rs_depthcloud.launch b/src/local_planner/launch/rs_depthcloud.launch similarity index 100% rename from local_planner/launch/rs_depthcloud.launch rename to src/local_planner/launch/rs_depthcloud.launch diff --git a/local_planner/nodelets.xml b/src/local_planner/nodelets.xml similarity index 100% rename from local_planner/nodelets.xml rename to src/local_planner/nodelets.xml diff --git a/local_planner/package.xml b/src/local_planner/package.xml similarity index 100% rename from local_planner/package.xml rename to src/local_planner/package.xml diff --git a/local_planner/resource/custom_rosconsole.conf b/src/local_planner/resource/custom_rosconsole.conf similarity index 100% rename from local_planner/resource/custom_rosconsole.conf rename to src/local_planner/resource/custom_rosconsole.conf diff --git a/local_planner/resource/local_planner.rviz b/src/local_planner/resource/local_planner.rviz similarity index 100% rename from local_planner/resource/local_planner.rviz rename to src/local_planner/resource/local_planner.rviz diff --git a/local_planner/resource/rqt_param_toggle.sh b/src/local_planner/resource/rqt_param_toggle.sh old mode 100755 new mode 100644 similarity index 100% rename from local_planner/resource/rqt_param_toggle.sh rename to src/local_planner/resource/rqt_param_toggle.sh diff --git a/local_planner/resource/stereo_calib.json b/src/local_planner/resource/stereo_calib.json similarity index 100% rename from local_planner/resource/stereo_calib.json rename to src/local_planner/resource/stereo_calib.json diff --git a/local_planner/src/nodes/local_planner.cpp b/src/local_planner/src/nodes/local_planner.cpp similarity index 100% rename from local_planner/src/nodes/local_planner.cpp rename to src/local_planner/src/nodes/local_planner.cpp diff --git a/local_planner/src/nodes/local_planner_node_main.cpp b/src/local_planner/src/nodes/local_planner_node_main.cpp similarity index 100% rename from local_planner/src/nodes/local_planner_node_main.cpp rename to src/local_planner/src/nodes/local_planner_node_main.cpp diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/src/local_planner/src/nodes/local_planner_visualization.cpp similarity index 100% rename from local_planner/src/nodes/local_planner_visualization.cpp rename to src/local_planner/src/nodes/local_planner_visualization.cpp diff --git a/local_planner/src/nodes/planner_functions.cpp b/src/local_planner/src/nodes/planner_functions.cpp similarity index 100% rename from local_planner/src/nodes/planner_functions.cpp rename to src/local_planner/src/nodes/planner_functions.cpp diff --git a/local_planner/src/nodes/star_planner.cpp b/src/local_planner/src/nodes/star_planner.cpp similarity index 100% rename from local_planner/src/nodes/star_planner.cpp rename to src/local_planner/src/nodes/star_planner.cpp diff --git a/local_planner/src/nodes/tree_node.cpp b/src/local_planner/src/nodes/tree_node.cpp similarity index 100% rename from local_planner/src/nodes/tree_node.cpp rename to src/local_planner/src/nodes/tree_node.cpp diff --git a/local_planner/src/nodes/waypoint_generator.cpp b/src/local_planner/src/nodes/waypoint_generator.cpp similarity index 100% rename from local_planner/src/nodes/waypoint_generator.cpp rename to src/local_planner/src/nodes/waypoint_generator.cpp diff --git a/local_planner/src/nodes/waypoint_generator.cpp.dot b/src/local_planner/src/nodes/waypoint_generator.cpp.dot similarity index 100% rename from local_planner/src/nodes/waypoint_generator.cpp.dot rename to src/local_planner/src/nodes/waypoint_generator.cpp.dot diff --git a/local_planner/src/utils/trajectory_simulator.cpp b/src/local_planner/src/utils/trajectory_simulator.cpp similarity index 100% rename from local_planner/src/utils/trajectory_simulator.cpp rename to src/local_planner/src/utils/trajectory_simulator.cpp diff --git a/local_planner/test/main.cpp b/src/local_planner/test/main.cpp similarity index 100% rename from local_planner/test/main.cpp rename to src/local_planner/test/main.cpp diff --git a/local_planner/test/test_example.cpp b/src/local_planner/test/test_example.cpp similarity index 100% rename from local_planner/test/test_example.cpp rename to src/local_planner/test/test_example.cpp diff --git a/local_planner/test/test_local_planner.cpp b/src/local_planner/test/test_local_planner.cpp similarity index 100% rename from local_planner/test/test_local_planner.cpp rename to src/local_planner/test/test_local_planner.cpp diff --git a/local_planner/test/test_local_planner_nodelet.cpp b/src/local_planner/test/test_local_planner_nodelet.cpp similarity index 100% rename from local_planner/test/test_local_planner_nodelet.cpp rename to src/local_planner/test/test_local_planner_nodelet.cpp diff --git a/local_planner/test/test_planner_functions.cpp b/src/local_planner/test/test_planner_functions.cpp similarity index 100% rename from local_planner/test/test_planner_functions.cpp rename to src/local_planner/test/test_planner_functions.cpp diff --git a/local_planner/test/test_star_planner.cpp b/src/local_planner/test/test_star_planner.cpp similarity index 100% rename from local_planner/test/test_star_planner.cpp rename to src/local_planner/test/test_star_planner.cpp diff --git a/local_planner/test/test_trajectory_simulator.cpp b/src/local_planner/test/test_trajectory_simulator.cpp similarity index 100% rename from local_planner/test/test_trajectory_simulator.cpp rename to src/local_planner/test/test_trajectory_simulator.cpp diff --git a/local_planner/test/test_waypoint_generator.cpp b/src/local_planner/test/test_waypoint_generator.cpp similarity index 100% rename from local_planner/test/test_waypoint_generator.cpp rename to src/local_planner/test/test_waypoint_generator.cpp diff --git a/local_planner/test/valgrind_suppressions.sup b/src/local_planner/test/valgrind_suppressions.sup similarity index 100% rename from local_planner/test/valgrind_suppressions.sup rename to src/local_planner/test/valgrind_suppressions.sup diff --git a/src/octomap_msgs b/src/octomap_msgs new file mode 160000 index 000000000..65d0b3b81 --- /dev/null +++ b/src/octomap_msgs @@ -0,0 +1 @@ +Subproject commit 65d0b3b81f890b5dc3bf531abe0e8c12ef3858e2 diff --git a/safe_landing_planner/CMakeLists.txt b/src/safe_landing_planner/CMakeLists.txt similarity index 100% rename from safe_landing_planner/CMakeLists.txt rename to src/safe_landing_planner/CMakeLists.txt diff --git a/global_planner/COLCON_IGNORE b/src/safe_landing_planner/COLCON_IGNORE similarity index 100% rename from global_planner/COLCON_IGNORE rename to src/safe_landing_planner/COLCON_IGNORE diff --git a/safe_landing_planner/cfg/SafeLandingPlannerNode.cfg b/src/safe_landing_planner/cfg/SafeLandingPlannerNode.cfg old mode 100755 new mode 100644 similarity index 100% rename from safe_landing_planner/cfg/SafeLandingPlannerNode.cfg rename to src/safe_landing_planner/cfg/SafeLandingPlannerNode.cfg diff --git a/safe_landing_planner/cfg/WaypointGeneratorNode.cfg b/src/safe_landing_planner/cfg/WaypointGeneratorNode.cfg old mode 100755 new mode 100644 similarity index 100% rename from safe_landing_planner/cfg/WaypointGeneratorNode.cfg rename to src/safe_landing_planner/cfg/WaypointGeneratorNode.cfg diff --git a/safe_landing_planner/cfg/slpn.yaml b/src/safe_landing_planner/cfg/slpn.yaml similarity index 100% rename from safe_landing_planner/cfg/slpn.yaml rename to src/safe_landing_planner/cfg/slpn.yaml diff --git a/safe_landing_planner/cfg/wpgn.yaml b/src/safe_landing_planner/cfg/wpgn.yaml similarity index 100% rename from safe_landing_planner/cfg/wpgn.yaml rename to src/safe_landing_planner/cfg/wpgn.yaml diff --git a/safe_landing_planner/include/safe_landing_planner/grid.hpp b/src/safe_landing_planner/include/safe_landing_planner/grid.hpp similarity index 100% rename from safe_landing_planner/include/safe_landing_planner/grid.hpp rename to src/safe_landing_planner/include/safe_landing_planner/grid.hpp diff --git a/safe_landing_planner/include/safe_landing_planner/safe_landing_planner.hpp b/src/safe_landing_planner/include/safe_landing_planner/safe_landing_planner.hpp similarity index 100% rename from safe_landing_planner/include/safe_landing_planner/safe_landing_planner.hpp rename to src/safe_landing_planner/include/safe_landing_planner/safe_landing_planner.hpp diff --git a/safe_landing_planner/include/safe_landing_planner/safe_landing_planner_node.hpp b/src/safe_landing_planner/include/safe_landing_planner/safe_landing_planner_node.hpp similarity index 100% rename from safe_landing_planner/include/safe_landing_planner/safe_landing_planner_node.hpp rename to src/safe_landing_planner/include/safe_landing_planner/safe_landing_planner_node.hpp diff --git a/safe_landing_planner/include/safe_landing_planner/safe_landing_planner_visualization.hpp b/src/safe_landing_planner/include/safe_landing_planner/safe_landing_planner_visualization.hpp similarity index 100% rename from safe_landing_planner/include/safe_landing_planner/safe_landing_planner_visualization.hpp rename to src/safe_landing_planner/include/safe_landing_planner/safe_landing_planner_visualization.hpp diff --git a/safe_landing_planner/include/safe_landing_planner/waypoint_generator.hpp b/src/safe_landing_planner/include/safe_landing_planner/waypoint_generator.hpp similarity index 100% rename from safe_landing_planner/include/safe_landing_planner/waypoint_generator.hpp rename to src/safe_landing_planner/include/safe_landing_planner/waypoint_generator.hpp diff --git a/safe_landing_planner/include/safe_landing_planner/waypoint_generator_node.hpp b/src/safe_landing_planner/include/safe_landing_planner/waypoint_generator_node.hpp similarity index 100% rename from safe_landing_planner/include/safe_landing_planner/waypoint_generator_node.hpp rename to src/safe_landing_planner/include/safe_landing_planner/waypoint_generator_node.hpp diff --git a/safe_landing_planner/launch/safe_landing_planner.launch b/src/safe_landing_planner/launch/safe_landing_planner.launch similarity index 100% rename from safe_landing_planner/launch/safe_landing_planner.launch rename to src/safe_landing_planner/launch/safe_landing_planner.launch diff --git a/safe_landing_planner/launch/safe_landing_planner_launch.launch b/src/safe_landing_planner/launch/safe_landing_planner_launch.launch similarity index 100% rename from safe_landing_planner/launch/safe_landing_planner_launch.launch rename to src/safe_landing_planner/launch/safe_landing_planner_launch.launch diff --git a/safe_landing_planner/launch/safe_landing_planner_rosbag.launch b/src/safe_landing_planner/launch/safe_landing_planner_rosbag.launch similarity index 100% rename from safe_landing_planner/launch/safe_landing_planner_rosbag.launch rename to src/safe_landing_planner/launch/safe_landing_planner_rosbag.launch diff --git a/safe_landing_planner/launch/safe_landing_planner_vtol.launch b/src/safe_landing_planner/launch/safe_landing_planner_vtol.launch similarity index 100% rename from safe_landing_planner/launch/safe_landing_planner_vtol.launch rename to src/safe_landing_planner/launch/safe_landing_planner_vtol.launch diff --git a/safe_landing_planner/msg/SLPGridMsg.msg b/src/safe_landing_planner/msg/SLPGridMsg.msg similarity index 100% rename from safe_landing_planner/msg/SLPGridMsg.msg rename to src/safe_landing_planner/msg/SLPGridMsg.msg diff --git a/safe_landing_planner/package.xml b/src/safe_landing_planner/package.xml similarity index 100% rename from safe_landing_planner/package.xml rename to src/safe_landing_planner/package.xml diff --git a/safe_landing_planner/resource/lsd_realsense.rviz b/src/safe_landing_planner/resource/lsd_realsense.rviz similarity index 100% rename from safe_landing_planner/resource/lsd_realsense.rviz rename to src/safe_landing_planner/resource/lsd_realsense.rviz diff --git a/safe_landing_planner/resource/realsense_params.sh b/src/safe_landing_planner/resource/realsense_params.sh old mode 100755 new mode 100644 similarity index 100% rename from safe_landing_planner/resource/realsense_params.sh rename to src/safe_landing_planner/resource/realsense_params.sh diff --git a/safe_landing_planner/resource/rqt_param_toggle.sh b/src/safe_landing_planner/resource/rqt_param_toggle.sh old mode 100755 new mode 100644 similarity index 100% rename from safe_landing_planner/resource/rqt_param_toggle.sh rename to src/safe_landing_planner/resource/rqt_param_toggle.sh diff --git a/safe_landing_planner/resource/safe_landing_planner.rviz b/src/safe_landing_planner/resource/safe_landing_planner.rviz similarity index 100% rename from safe_landing_planner/resource/safe_landing_planner.rviz rename to src/safe_landing_planner/resource/safe_landing_planner.rviz diff --git a/safe_landing_planner/resource/safe_landing_planner_rosbag.rviz b/src/safe_landing_planner/resource/safe_landing_planner_rosbag.rviz similarity index 100% rename from safe_landing_planner/resource/safe_landing_planner_rosbag.rviz rename to src/safe_landing_planner/resource/safe_landing_planner_rosbag.rviz diff --git a/safe_landing_planner/resource/stereo_calib.json b/src/safe_landing_planner/resource/stereo_calib.json similarity index 100% rename from safe_landing_planner/resource/stereo_calib.json rename to src/safe_landing_planner/resource/stereo_calib.json diff --git a/safe_landing_planner/src/nodes/safe_landing_planner.cpp b/src/safe_landing_planner/src/nodes/safe_landing_planner.cpp similarity index 100% rename from safe_landing_planner/src/nodes/safe_landing_planner.cpp rename to src/safe_landing_planner/src/nodes/safe_landing_planner.cpp diff --git a/safe_landing_planner/src/nodes/safe_landing_planner_node.cpp b/src/safe_landing_planner/src/nodes/safe_landing_planner_node.cpp similarity index 100% rename from safe_landing_planner/src/nodes/safe_landing_planner_node.cpp rename to src/safe_landing_planner/src/nodes/safe_landing_planner_node.cpp diff --git a/safe_landing_planner/src/nodes/safe_landing_planner_node_main.cpp b/src/safe_landing_planner/src/nodes/safe_landing_planner_node_main.cpp similarity index 100% rename from safe_landing_planner/src/nodes/safe_landing_planner_node_main.cpp rename to src/safe_landing_planner/src/nodes/safe_landing_planner_node_main.cpp diff --git a/safe_landing_planner/src/nodes/safe_landing_planner_visualization.cpp b/src/safe_landing_planner/src/nodes/safe_landing_planner_visualization.cpp similarity index 100% rename from safe_landing_planner/src/nodes/safe_landing_planner_visualization.cpp rename to src/safe_landing_planner/src/nodes/safe_landing_planner_visualization.cpp diff --git a/safe_landing_planner/src/nodes/waypoint_generator.cpp b/src/safe_landing_planner/src/nodes/waypoint_generator.cpp similarity index 100% rename from safe_landing_planner/src/nodes/waypoint_generator.cpp rename to src/safe_landing_planner/src/nodes/waypoint_generator.cpp diff --git a/safe_landing_planner/src/nodes/waypoint_generator.cpp.dot b/src/safe_landing_planner/src/nodes/waypoint_generator.cpp.dot similarity index 100% rename from safe_landing_planner/src/nodes/waypoint_generator.cpp.dot rename to src/safe_landing_planner/src/nodes/waypoint_generator.cpp.dot diff --git a/safe_landing_planner/src/nodes/waypoint_generator_node.cpp b/src/safe_landing_planner/src/nodes/waypoint_generator_node.cpp similarity index 100% rename from safe_landing_planner/src/nodes/waypoint_generator_node.cpp rename to src/safe_landing_planner/src/nodes/waypoint_generator_node.cpp diff --git a/safe_landing_planner/test/main.cpp b/src/safe_landing_planner/test/main.cpp similarity index 100% rename from safe_landing_planner/test/main.cpp rename to src/safe_landing_planner/test/main.cpp diff --git a/safe_landing_planner/test/test_grid.cpp b/src/safe_landing_planner/test/test_grid.cpp similarity index 100% rename from safe_landing_planner/test/test_grid.cpp rename to src/safe_landing_planner/test/test_grid.cpp diff --git a/safe_landing_planner/test/test_safe_landing_planner.cpp b/src/safe_landing_planner/test/test_safe_landing_planner.cpp similarity index 100% rename from safe_landing_planner/test/test_safe_landing_planner.cpp rename to src/safe_landing_planner/test/test_safe_landing_planner.cpp diff --git a/safe_landing_planner/test/test_waypoint_generator.cpp b/src/safe_landing_planner/test/test_waypoint_generator.cpp similarity index 100% rename from safe_landing_planner/test/test_waypoint_generator.cpp rename to src/safe_landing_planner/test/test_waypoint_generator.cpp