diff --git a/mav_planning_rviz/CMakeLists.txt b/mav_planning_rviz/CMakeLists.txt index 219a64bc..209f504d 100644 --- a/mav_planning_rviz/CMakeLists.txt +++ b/mav_planning_rviz/CMakeLists.txt @@ -107,13 +107,13 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDIN pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) -# add_executable(standalone_test -# src/standalone_test.cpp -# ) +add_executable(standalone_test + src/standalone_test.cpp +) -# target_link_libraries(standalone_test PUBLIC -# ${PROJECT_NAME} -# ) +target_link_libraries(standalone_test PUBLIC + ${PROJECT_NAME} +) # Install install( @@ -139,11 +139,17 @@ ament_export_dependencies( rclcpp ) -# install( -# TARGETS -# standalone_test -# DESTINATION lib/${PROJECT_NAME} -# ) +install( + TARGETS + standalone_test + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + rviz + DESTINATION share/${PROJECT_NAME}/ +) if(BUILD_TESTING) # find_package(ament_lint_auto REQUIRED) diff --git a/mav_planning_rviz/include/mav_planning_rviz/edit_button.h b/mav_planning_rviz/include/mav_planning_rviz/edit_button.h index 2f7f4ca5..11890562 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/edit_button.h +++ b/mav_planning_rviz/include/mav_planning_rviz/edit_button.h @@ -2,9 +2,9 @@ #define MAV_PLANNING_RVIZ_EDIT_BUTTON_H_ #ifndef Q_MOC_RUN -#include #include #include +#include #endif namespace mav_planning_rviz { diff --git a/mav_planning_rviz/include/mav_planning_rviz/goal_marker.h b/mav_planning_rviz/include/mav_planning_rviz/goal_marker.h index 2a50e23d..dfe5f095 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/goal_marker.h +++ b/mav_planning_rviz/include/mav_planning_rviz/goal_marker.h @@ -2,28 +2,24 @@ #ifndef MAV_PLANNING_RVIZ_GOAL_MARKER_H_ #define MAV_PLANNING_RVIZ_GOAL_MARKER_H_ -#include +#include #include -#include - -#include -#include -#include #include +#include #include +#include +#include +#include class GoalMarker { public: GoalMarker(rclcpp::Node::SharedPtr node); virtual ~GoalMarker(); - Eigen::Vector3d getGoalPosition() { return goal_pos_; }; + Eigen::Vector3d getGoalPosition(); private: - Eigen::Vector3d toEigen(const geometry_msgs::msg::Pose &p) { - Eigen::Vector3d position(p.position.x, p.position.y, p.position.z); - return position; - } + Eigen::Vector3d toEigen(const geometry_msgs::msg::Pose &p); void processSetPoseFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback); void GridmapCallback(const grid_map_msgs::msg::GridMap &msg); diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h b/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h index f2f18948..f5dc77d2 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h @@ -2,12 +2,10 @@ #define MAV_PLANNING_RVIZ_PLANNING_INTERACTIVE_MARKERS_H_ #include - #include -#include - #include #include +#include namespace mav_planning_rviz { diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h index 25595bd0..159a8a12 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h @@ -2,13 +2,14 @@ #define MAV_PLANNING_RVIZ_PLANNING_PANEL_H_ #ifndef Q_MOC_RUN +//! @todo(srmainwaring) prevent race condition with async service calls +#include +#include #include #include #include - #include -#include #include "mav_planning_rviz/edit_button.h" #include "mav_planning_rviz/goal_marker.h" #include "mav_planning_rviz/planning_interactive_markers.h" @@ -100,6 +101,9 @@ class PlanningPanel : public rviz_common::Panel { std::shared_ptr goal_marker_; + //! @todo(srmainwaring) prevent race condition with async service calls + std::mutex node_mutex_; // protects node_ + // QT stuff: QLineEdit* namespace_editor_; QLineEdit* planner_name_editor_; diff --git a/mav_planning_rviz/include/mav_planning_rviz/pose_widget.h b/mav_planning_rviz/include/mav_planning_rviz/pose_widget.h index c188be62..24728209 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/pose_widget.h +++ b/mav_planning_rviz/include/mav_planning_rviz/pose_widget.h @@ -2,11 +2,11 @@ #define MAV_PLANNING_RVIZ_POSE_WIDGET_H_ #ifndef Q_MOC_RUN -#include #include #include #include #include +#include #endif class QLineEdit; diff --git a/mav_planning_rviz/launch/run_rviz.launch b/mav_planning_rviz/launch/run_rviz.launch index 1041303c..722452ef 100644 --- a/mav_planning_rviz/launch/run_rviz.launch +++ b/mav_planning_rviz/launch/run_rviz.launch @@ -1,7 +1,16 @@ - + + - - - + + + + + + + + + + + diff --git a/mav_planning_rviz/rviz/config.rviz b/mav_planning_rviz/rviz/config.rviz new file mode 100644 index 00000000..7f808f4d --- /dev/null +++ b/mav_planning_rviz/rviz/config.rviz @@ -0,0 +1,190 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 290 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: mav_planning_rviz/PlanningPanel + Name: PlanningPanel + namespace: "" + odometry_topic: "" + planner_name: davosdorf + planning_budget: 4 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1000 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.8999999761581421 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: ColorLayer + Enabled: true + Height Layer: elevation + Height Transformer: Layer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map + Use Rainbow: true + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: visualization_marker_array + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8401.0107421875 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -515.1697998046875 + Y: 526.9537353515625 + Z: 855.5363159179688 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5897997617721558 + Target Frame: map + Value: Orbit (rviz_default_plugins) + Yaw: 5.818671703338623 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 950 + Hide Left Dock: false + Hide Right Dock: true + PlanningPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002ad00000335fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006600000171000000e300fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c01000001d8000001c30000016d00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000000000000000000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000005dc00000039fc0100000002fb0000000800540069006d00650100000000000005dc0000023d00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d006501000000000000045000000000000000000000032e0000033500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1500 + X: 0 + Y: 25 diff --git a/mav_planning_rviz/src/edit_button.cpp b/mav_planning_rviz/src/edit_button.cpp index 28204ee1..cac7d6d6 100644 --- a/mav_planning_rviz/src/edit_button.cpp +++ b/mav_planning_rviz/src/edit_button.cpp @@ -1,11 +1,11 @@ +#include "mav_planning_rviz/edit_button.h" + #include #include #include #include #include -#include "mav_planning_rviz/edit_button.h" - namespace mav_planning_rviz { EditButton::EditButton(const std::string& id, QWidget* parent) : QWidget(parent), id_(id), editing_(false) { diff --git a/mav_planning_rviz/src/goal_marker.cpp b/mav_planning_rviz/src/goal_marker.cpp index cd4485fd..6975ea1d 100644 --- a/mav_planning_rviz/src/goal_marker.cpp +++ b/mav_planning_rviz/src/goal_marker.cpp @@ -1,10 +1,10 @@ #include "mav_planning_rviz/goal_marker.h" + #include using std::placeholders::_1; -GoalMarker::GoalMarker(rclcpp::Node::SharedPtr node) - : node_(node), marker_server_("goal", node) { +GoalMarker::GoalMarker(rclcpp::Node::SharedPtr node) : node_(node), marker_server_("goal", node) { set_goal_marker_.header.frame_id = "map"; set_goal_marker_.name = "set_pose"; set_goal_marker_.scale = 100.0; @@ -24,17 +24,23 @@ GoalMarker::GoalMarker(rclcpp::Node::SharedPtr node) set_goal_marker_.controls.push_back(control); marker_server_.insert(set_goal_marker_); - marker_server_.setCallback(set_goal_marker_.name, - std::bind(&GoalMarker::processSetPoseFeedback, this, _1)); + marker_server_.setCallback(set_goal_marker_.name, std::bind(&GoalMarker::processSetPoseFeedback, this, _1)); marker_server_.applyChanges(); grid_map_sub_ = node_->create_subscription( - "/grid_map", 1, - std::bind(&GoalMarker::GridmapCallback, this, _1)); + "/grid_map", 1, std::bind(&GoalMarker::GridmapCallback, this, _1)); } GoalMarker::~GoalMarker() = default; -void GoalMarker::processSetPoseFeedback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback) { +Eigen::Vector3d GoalMarker::getGoalPosition() { return goal_pos_; }; + +Eigen::Vector3d GoalMarker::toEigen(const geometry_msgs::msg::Pose &p) { + Eigen::Vector3d position(p.position.x, p.position.y, p.position.z); + return position; +} + +void GoalMarker::processSetPoseFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback) { const std::lock_guard lock(goal_mutex_); // TODO: Set goal position from menu if (feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE) { diff --git a/mav_planning_rviz/src/planning_interactive_markers.cpp b/mav_planning_rviz/src/planning_interactive_markers.cpp index c20a034c..336dd7e7 100644 --- a/mav_planning_rviz/src/planning_interactive_markers.cpp +++ b/mav_planning_rviz/src/planning_interactive_markers.cpp @@ -14,11 +14,7 @@ using std::placeholders::_1; namespace mav_planning_rviz { PlanningInteractiveMarkers::PlanningInteractiveMarkers(rclcpp::Node::SharedPtr node) - : node_(node), - marker_server_("planning_markers", node), - frame_id_("odom"), - initialized_(false) { -} + : node_(node), marker_server_("planning_markers", node), frame_id_("odom"), initialized_(false) {} PlanningInteractiveMarkers::~PlanningInteractiveMarkers() = default; @@ -34,6 +30,8 @@ void PlanningInteractiveMarkers::initialize() { } void PlanningInteractiveMarkers::createMarkers() { + RCLCPP_INFO_STREAM(node_->get_logger(), "Create markers"); + const double kSqrt2Over2 = sqrt(2.0) / 2.0; // Set up controls: x, y, z, and yaw. @@ -71,19 +69,22 @@ void PlanningInteractiveMarkers::createMarkers() { marker_prototype_.controls.push_back(control); } -void PlanningInteractiveMarkers::enableSetPoseMarker( - const mav_msgs::EigenTrajectoryPoint& pose) { +void PlanningInteractiveMarkers::enableSetPoseMarker(const mav_msgs::EigenTrajectoryPoint& pose) { + RCLCPP_INFO_STREAM(node_->get_logger(), "Enable set pose marker"); + geometry_msgs::msg::PoseStamped pose_stamped; mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(pose, &pose_stamped); set_pose_marker_.pose = pose_stamped.pose; marker_server_.insert(set_pose_marker_); marker_server_.setCallback(set_pose_marker_.name, - std::bind(&PlanningInteractiveMarkers::processSetPoseFeedback, this, _1)); + std::bind(&PlanningInteractiveMarkers::processSetPoseFeedback, this, _1)); marker_server_.applyChanges(); } void PlanningInteractiveMarkers::disableSetPoseMarker() { + RCLCPP_INFO_STREAM(node_->get_logger(), "Disable set pose marker"); + marker_server_.erase(set_pose_marker_.name); marker_server_.applyChanges(); } @@ -94,6 +95,8 @@ void PlanningInteractiveMarkers::setPose(const mav_msgs::EigenTrajectoryPoint& p set_pose_marker_.pose = pose_stamped.pose; marker_server_.setPose(set_pose_marker_.name, set_pose_marker_.pose); marker_server_.applyChanges(); + + RCLCPP_INFO_STREAM(node_->get_logger(), "Set pose: " << to_yaml(pose_stamped)); } void PlanningInteractiveMarkers::processSetPoseFeedback( @@ -103,14 +106,15 @@ void PlanningInteractiveMarkers::processSetPoseFeedback( mav_msgs::EigenTrajectoryPoint pose; mav_msgs::eigenTrajectoryPointFromPoseMsg(feedback->pose, &pose); pose_updated_function_(pose); + + RCLCPP_INFO_STREAM(node_->get_logger(), "Set pose feedback: " << to_yaml(feedback->pose)); } } marker_server_.applyChanges(); } -void PlanningInteractiveMarkers::enableMarker(const std::string& id, - const mav_msgs::EigenTrajectoryPoint& pose) { +void PlanningInteractiveMarkers::enableMarker(const std::string& id, const mav_msgs::EigenTrajectoryPoint& pose) { geometry_msgs::msg::PoseStamped pose_stamped; mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(pose, &pose_stamped); @@ -132,8 +136,7 @@ void PlanningInteractiveMarkers::enableMarker(const std::string& id, marker_server_.applyChanges(); } -void PlanningInteractiveMarkers::updateMarkerPose(const std::string& id, - const mav_msgs::EigenTrajectoryPoint& pose) { +void PlanningInteractiveMarkers::updateMarkerPose(const std::string& id, const mav_msgs::EigenTrajectoryPoint& pose) { auto search = marker_map_.find(id); if (search == marker_map_.end()) { return; @@ -144,6 +147,8 @@ void PlanningInteractiveMarkers::updateMarkerPose(const std::string& id, search->second.pose = pose_stamped.pose; marker_server_.setPose(id, pose_stamped.pose); marker_server_.applyChanges(); + + RCLCPP_INFO_STREAM(node_->get_logger(), "Update marker pose: " << to_yaml(pose_stamped)); } void PlanningInteractiveMarkers::disableMarker(const std::string& id) { diff --git a/mav_planning_rviz/src/planning_panel.cpp b/mav_planning_rviz/src/planning_panel.cpp index 8d55dc31..e3429165 100644 --- a/mav_planning_rviz/src/planning_panel.cpp +++ b/mav_planning_rviz/src/planning_panel.cpp @@ -1,8 +1,6 @@ #include "mav_planning_rviz/planning_panel.h" #include -#include -#include #include #include @@ -12,19 +10,19 @@ #include #include #include - +#include #include +#include // #include // #include -#include -#include #include - #include #include #include #include #include +#include +#include #include "mav_planning_rviz/edit_button.h" #include "mav_planning_rviz/goal_marker.h" @@ -36,23 +34,23 @@ using namespace std::chrono_literals; namespace mav_planning_rviz { PlanningPanel::PlanningPanel(QWidget* parent) - : rviz_common::Panel(parent), - node_(std::make_shared("mav_planning_rviz")), - interactive_markers_(node_) { + : rviz_common::Panel(parent), + node_(std::make_shared("mav_planning_rviz")), + interactive_markers_(node_) { createLayout(); - goal_marker_ = std::make_shared(node_); - planner_state_sub_ = node_->create_subscription( - "/planner_status", 1, - std::bind(&PlanningPanel::plannerstateCallback, this, _1)); } void PlanningPanel::onInitialize() { + auto rviz_ros_node = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + goal_marker_ = std::make_shared(rviz_ros_node); + + planner_state_sub_ = rviz_ros_node->create_subscription( + "/planner_status", 1, std::bind(&PlanningPanel::plannerstateCallback, this, _1)); + interactive_markers_.initialize(); - interactive_markers_.setPoseUpdatedCallback( - std::bind(&PlanningPanel::updateInteractiveMarkerPose, this, _1)); + interactive_markers_.setPoseUpdatedCallback(std::bind(&PlanningPanel::updateInteractiveMarkerPose, this, _1)); - //! @todo(srmainwaring) port to ROS 2 - // interactive_markers_.setFrameId(vis_manager_->getFixedFrame().toStdString()); + interactive_markers_.setFrameId(getDisplayContext()->getFixedFrame().toStdString()); // Initialize all the markers. for (const auto& kv : pose_widget_map_) { mav_msgs::EigenTrajectoryPoint pose; @@ -181,7 +179,8 @@ void PlanningPanel::terrainAlignmentStateChanged(int state) { // Set the topic name we are publishing to. void PlanningPanel::setNamespace(const QString& new_namespace) { - RCLCPP_DEBUG_STREAM(node_->get_logger(), "Setting namespace from: " << namespace_.toStdString() << " to " << new_namespace.toStdString()); + RCLCPP_DEBUG_STREAM(node_->get_logger(), + "Setting namespace from: " << namespace_.toStdString() << " to " << new_namespace.toStdString()); // Only take action if the name has changed. if (new_namespace != namespace_) { namespace_ = new_namespace; @@ -191,10 +190,10 @@ void PlanningPanel::setNamespace(const QString& new_namespace) { //! @todo(srmainwaring) port to ROS 2 // if (ros::names::validate(namespace_.toStdString(), error)) { - waypoint_pub_ = node_->create_publisher( - namespace_.toStdString() + "/waypoint", 1); - controller_pub_ = node_->create_publisher( - namespace_.toStdString() + "/command/pose", 1); + waypoint_pub_ = + node_->create_publisher(namespace_.toStdString() + "/waypoint", 1); + controller_pub_ = + node_->create_publisher(namespace_.toStdString() + "/command/pose", 1); odometry_sub_ = node_->create_subscription( namespace_.toStdString() + "/" + odometry_topic_.toStdString(), 1, std::bind(&PlanningPanel::odometryCallback, this, _1)); @@ -221,9 +220,8 @@ void PlanningPanel::setPlannerName() { std::thread t([this, service_name, new_planner_name, align_terrain] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -232,11 +230,10 @@ void PlanningPanel::setPlannerName() { auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -294,9 +291,8 @@ void PlanningPanel::startEditing(const std::string& id) { if (search == pose_widget_map_.end()) { return; } - // Update fixed frame (may have changed since last time): - //! @todo(srmainwaring) port to ROS 2 - // interactive_markers_.setFrameId(vis_manager_->getFixedFrame().toStdString()); + // Update fixed frame (may have changed since last time): + interactive_markers_.setFrameId(getDisplayContext()->getFixedFrame().toStdString()); mav_msgs::EigenTrajectoryPoint pose; search->second->getPose(&pose); interactive_markers_.enableSetPoseMarker(pose); @@ -378,21 +374,21 @@ void PlanningPanel::callPlannerService() { std::thread t([this, service_name] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); req->custom_mode = "OFFBOARD"; + //! @todo(srmainwaring) for AP custom mode is "GUIDED". + // req->custom_mode = "GUIDED"; auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -412,20 +408,18 @@ void PlanningPanel::callPublishPath() { std::string service_name = namespace_.toStdString() + "/" + planner_name_.toStdString() + "/publish_path"; auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -444,9 +438,8 @@ void PlanningPanel::publishWaypoint() { std::thread t([this, service_name] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -454,11 +447,10 @@ void PlanningPanel::publishWaypoint() { auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -487,9 +479,8 @@ void PlanningPanel::setMaxAltitudeConstrant(bool set_constraint) { std::thread t([this, service_name, new_planner_name, align_terrain] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -498,11 +489,10 @@ void PlanningPanel::setMaxAltitudeConstrant(bool set_constraint) { auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -528,9 +518,8 @@ void PlanningPanel::setGoalService() { std::thread t([this, service_name, goal_pos, goal_altitude] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -540,11 +529,10 @@ void PlanningPanel::setGoalService() { auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -566,20 +554,18 @@ void PlanningPanel::setPathService() { std::thread t([this, service_name] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -611,9 +597,8 @@ void PlanningPanel::setPlanningBudgetService() { std::thread t([this, service_name, planning_budget] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -622,11 +607,10 @@ void PlanningPanel::setPlanningBudgetService() { auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -662,9 +646,8 @@ void PlanningPanel::callSetPlannerStateService(std::string service_name, const i std::thread t([this, service_name, mode] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -672,11 +655,10 @@ void PlanningPanel::callSetPlannerStateService(std::string service_name, const i auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -702,9 +684,8 @@ void PlanningPanel::setStartService() { std::thread t([this, service_name, goal_pos, goal_altitude] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); @@ -714,11 +695,10 @@ void PlanningPanel::setStartService() { auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -740,20 +720,18 @@ void PlanningPanel::setStartLoiterService() { std::thread t([this, service_name] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -775,20 +753,18 @@ void PlanningPanel::setCurrentSegmentService() { std::thread t([this, service_name] { auto client = node_->create_client(service_name); if (!client->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" - << service_name << "] not available."); - return; + RCLCPP_WARN_STREAM(node_->get_logger(), "Service [" << service_name << "] not available."); + return; } auto req = std::make_shared(); auto result = client->async_send_request(req); - if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" - << client->get_service_name() - << "] failed."); + //! @todo(srmainwaring) prevent race condition with async service calls + const std::lock_guard lock(node_mutex_); + if (rclcpp::spin_until_future_complete(node_, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Call to service [" << client->get_service_name() << "] failed."); return; } @@ -809,13 +785,12 @@ void PlanningPanel::publishToController() { goal_pose_widget_->getPose(&goal_point); geometry_msgs::msg::PoseStamped pose; - //! @todo(srmainwaring) port to ROS 2 - // pose.header.frame_id = vis_manager_->getFixedFrame().toStdString(); + pose.header.frame_id = getDisplayContext()->getFixedFrame().toStdString(); mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(goal_point, &pose); - RCLCPP_DEBUG_STREAM(node_->get_logger(), - "Publishing controller goal on " << controller_pub_->get_topic_name() - << " subscribers: " << controller_pub_->get_subscription_count()); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Publishing controller goal on " + << controller_pub_->get_topic_name() + << " subscribers: " << controller_pub_->get_subscription_count()); controller_pub_->publish(pose); } diff --git a/mav_planning_rviz/src/pose_widget.cpp b/mav_planning_rviz/src/pose_widget.cpp index 837d1c57..7f5c553f 100644 --- a/mav_planning_rviz/src/pose_widget.cpp +++ b/mav_planning_rviz/src/pose_widget.cpp @@ -1,11 +1,11 @@ +#include "mav_planning_rviz/pose_widget.h" + #include #include #include #include #include -#include "mav_planning_rviz/pose_widget.h" - namespace mav_planning_rviz { constexpr double kDegToRad = M_PI / 180.0; @@ -58,14 +58,14 @@ void PoseWidget::setPose(const mav_msgs::EigenTrajectoryPoint& point) { table_widget_->blockSignals(false); } -void PoseWidget::itemChanged(QTableWidgetItem* item) { +void PoseWidget::itemChanged(QTableWidgetItem* /*item*/) { mav_msgs::EigenTrajectoryPoint point; getPose(&point); Q_EMIT poseUpdated(id_, point); } -QWidget* DoubleTableDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& option, - const QModelIndex& index) const { +QWidget* DoubleTableDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& /*option*/, + const QModelIndex& /*index*/) const { // From: // https://stackoverflow.com/questions/22708623/qtablewidget-only-numbers-permitted QLineEdit* line_edit = new QLineEdit(parent); diff --git a/terrain_navigation/include/terrain_navigation/path.h b/terrain_navigation/include/terrain_navigation/path.h index cfdf9039..41f660d3 100644 --- a/terrain_navigation/include/terrain_navigation/path.h +++ b/terrain_navigation/include/terrain_navigation/path.h @@ -70,7 +70,7 @@ class Path { void prependSegment(const PathSegment &trajectory) { segments.insert(segments.begin(), trajectory); }; void appendSegment(const PathSegment &trajectory) { segments.push_back(trajectory); }; void appendSegment(const Path &trajectory_segments) { - for (const auto& trajectory : trajectory_segments.segments) { + for (const auto &trajectory : trajectory_segments.segments) { appendSegment(trajectory); } }; diff --git a/terrain_navigation/include/terrain_navigation/path_segment.h b/terrain_navigation/include/terrain_navigation/path_segment.h index 0a2dd9a7..cc777fdb 100644 --- a/terrain_navigation/include/terrain_navigation/path_segment.h +++ b/terrain_navigation/include/terrain_navigation/path_segment.h @@ -36,12 +36,11 @@ #ifndef PATH_SEGMENT_H #define PATH_SEGMENT_H +#include #include #include #include -#include - struct State { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d position; diff --git a/terrain_navigation/include/terrain_navigation/terrain_map.h b/terrain_navigation/include/terrain_navigation/terrain_map.h index da134245..8584b73b 100644 --- a/terrain_navigation/include/terrain_navigation/terrain_map.h +++ b/terrain_navigation/include/terrain_navigation/terrain_map.h @@ -36,12 +36,11 @@ #ifndef TERRAIN_MAP_H #define TERRAIN_MAP_H -#include -#include - -#include #include #include +#include +#include +#include #if __APPLE__ #include diff --git a/terrain_navigation/include/terrain_navigation/viewpoint.h b/terrain_navigation/include/terrain_navigation/viewpoint.h index e623d668..b4561050 100644 --- a/terrain_navigation/include/terrain_navigation/viewpoint.h +++ b/terrain_navigation/include/terrain_navigation/viewpoint.h @@ -41,16 +41,14 @@ #ifndef TERRAIN_PLANNER_VIEWPOINT_H #define TERRAIN_PLANNER_VIEWPOINT_H -#include -#include - #include - +#include #include #include #include #include #include +#include class ViewPoint { public: diff --git a/terrain_navigation/include/terrain_navigation/visualization.h b/terrain_navigation/include/terrain_navigation/visualization.h index 3c3f52e7..f8f0ce80 100644 --- a/terrain_navigation/include/terrain_navigation/visualization.h +++ b/terrain_navigation/include/terrain_navigation/visualization.h @@ -42,9 +42,8 @@ #ifndef VISUALIZATION_H #define VISUALIZATION_H -#include - #include +#include #include geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { @@ -56,7 +55,7 @@ geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { } visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { + Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { double scale{15}; // Size of the viewpoint markers visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; diff --git a/terrain_navigation/test/test_terrain_map.cpp b/terrain_navigation/test/test_terrain_map.cpp index 15cc64af..f40c7590 100644 --- a/terrain_navigation/test/test_terrain_map.cpp +++ b/terrain_navigation/test/test_terrain_map.cpp @@ -1,8 +1,9 @@ -#include "terrain_navigation/terrain_map.h" - #include + #include +#include "terrain_navigation/terrain_map.h" + TEST(TerrainMapTest, geoTransform) { // Depending on Gdal versions, lon lat order are reversed #if GDAL_VERSION_MAJOR > 2 diff --git a/terrain_navigation/test/test_trajectory.cpp b/terrain_navigation/test/test_trajectory.cpp index 4848dc70..028a0410 100644 --- a/terrain_navigation/test/test_trajectory.cpp +++ b/terrain_navigation/test/test_trajectory.cpp @@ -1,8 +1,9 @@ -#include "terrain_navigation/path.h" - #include + #include +#include "terrain_navigation/path.h" + //! @todo(srmainwaring)check includes // #include "terrain_navigation/data_logger.h" // #include "terrain_navigation/path_segment.h" diff --git a/terrain_navigation_ros/CMakeLists.txt b/terrain_navigation_ros/CMakeLists.txt index fbacc081..3daf7bbd 100644 --- a/terrain_navigation_ros/CMakeLists.txt +++ b/terrain_navigation_ros/CMakeLists.txt @@ -64,7 +64,9 @@ find_package(PkgConfig REQUIRED) pkg_check_modules(ODE REQUIRED ode) add_library(${PROJECT_NAME} + src/geo_conversions.cpp src/terrain_planner.cpp + src/visualization.cpp ) target_include_directories(${PROJECT_NAME} @@ -136,7 +138,9 @@ install( ) install(DIRECTORY + config launch + resources DESTINATION share/${PROJECT_NAME}/ ) diff --git a/terrain_navigation_ros/cfg/HeightRateTuning.cfg b/terrain_navigation_ros/config/HeightRateTuning.cfg similarity index 100% rename from terrain_navigation_ros/cfg/HeightRateTuning.cfg rename to terrain_navigation_ros/config/HeightRateTuning.cfg diff --git a/terrain_navigation_ros/config/ap_config.yaml b/terrain_navigation_ros/config/ap_config.yaml new file mode 100644 index 00000000..0f826e4f --- /dev/null +++ b/terrain_navigation_ros/config/ap_config.yaml @@ -0,0 +1,273 @@ +# Common configuration for ArduPilot +# +# node: +/mavros/**: + ros__parameters: + startup_px4_usb_quirk: false + +# --- system plugins --- + +# sys_status & sys_time connection options +/mavros/**/conn: + ros__parameters: + heartbeat_rate: 1.0 # send heartbeat rate in Hertz + heartbeat_mav_type: "ONBOARD_CONTROLLER" + timeout: 10.0 # heartbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) + +# sys_status +/mavros/**/sys: + ros__parameters: + min_voltage: [10.0] # diagnostics min voltage, use a vector i.e. [16.2, 16.0] for multiple batteries, up-to 10 are supported + # to achieve the same on a ROS launch file do: [16.2, 16.0] + disable_diag: false # disable all sys_status diagnostics, except heartbeat + +# sys_time +/mavros/**/time: + ros__parameters: + time_ref_source: "fcu" # time_reference source + timesync_mode: MAVLINK + timesync_avg_alpha: 0.6 # timesync averaging factor + +# --- mavros plugins (alphabetical order) --- + +# 3dr_radio +/mavros/**/tdr_radio: + ros__parameters: + low_rssi: 40 # raw rssi lower level for diagnostics + +# actuator_control +# None + +# command +/mavros/**/cmd: + ros__parameters: + use_comp_id_system_control: false # quirk for some old FCUs + +# dummy +# None + +# ftp +# None + +# global_position +/mavros/**/global_position: + ros__parameters: + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + rot_covariance: 99999.0 # covariance for attitude? + gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) + use_relative_alt: true # use relative altitude for local coordinates + tf.send: false # send TF? + tf.frame_id: "map" # TF frame_id + tf.global_frame_id: "earth" # TF earth frame_id + tf.child_frame_id: "base_link" # TF child_frame_id + +# imu_pub +/mavros/**/imu: + ros__parameters: + frame_id: "base_link" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 # 0.02 degrees + orientation_stdev: 1.0 + magnetic_stdev: 0.0 + +# local_position +/mavros/**/local_position: + ros__parameters: + frame_id: "map" + tf.send: false + tf.frame_id: "map" + tf.child_frame_id: "base_link" + tf.send_fcu: false + +# param +# None, used for FCU params + +# rc_io +# None + +# setpoint_accel +/mavros/**/setpoint_accel: + ros__parameters: + send_force: false + +# setpoint_attitude +/mavros/**/setpoint_attitude: + ros__parameters: + reverse_thrust: false # allow reversed thrust + use_quaternion: false # enable PoseStamped topic subscriber + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_attitude" + tf.rate_limit: 50.0 + +# setpoint_raw +/mavros/**/setpoint_raw: + ros__parameters: + thrust_scaling: 1.0 # specify thrust scaling (normalized, 0 to 1) for thrust (like PX4) + +# setpoint_position +/mavros/**/setpoint_position: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_position" + tf.rate_limit: 50.0 + mav_frame: LOCAL_NED + +# guided_target +/mavros/**/guided_target: + ros__parameters: + tf.listen: false # enable tf listener (disable topic subscribers) + tf.frame_id: "map" + tf.child_frame_id: "target_position" + tf.rate_limit: 50.0 + +# setpoint_velocity +/mavros/**/setpoint_velocity: + ros__parameters: + mav_frame: LOCAL_NED + +# vfr_hud +# None + +# waypoint +/mavros/**/mission: + ros__parameters: + pull_after_gcs: true # update mission if gcs updates + use_mission_item_int: true # use the MISSION_ITEM_INT message instead of MISSION_ITEM + # for uploading waypoints to FCU + + +# --- mavros extras plugins (same order) --- + +# /mavros: + # distance_sensor node + # distance_sensor: + # ros__parameters: + # base_frame_id: base_footprint + # # This config will suppress the warnings in the mavros node about + # # missing sensor definitions for distance_sensor. + # # + # # The distance_sensor config requires a workaround for the parameter definition. + # # See comments in: ROS2: port mavros launch #1564 + # # https://github.com/mavlink/mavros/issues/1564#issuecomment-1055155609 + # config: |- + # sensor_10: + # id: 10 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_11: + # id: 11 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_12: + # id: 12 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_13: + # id: 13 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_14: + # id: 14 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_15: + # id: 15 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_16: + # id: 16 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + # sensor_17: + # id: 17 + # subscriber: false + # frame_id: lidar_link + # field_of_view: 0.0 + # send_tf: false + # sensor_position: {x: 0.0, y: 0.0, z: 0.0} + # covariance: 0.0 + # horizontal_fov_ratio: 0.0 + # vertical_fov_ratio: 0.0 + + # # Example: all parameters defined in code + # # config: |- + # # sensor_10: + # # id: 10 + # # subscriber: false + # # orientation: PITCH_270 + # # # if subscriber != true + # # frame_id: lidar_link + # # field_of_view: 0.0 + # # send_tf: false + # # sensor_position: + # # x: 0.0 + # # y: 0.0 + # # z: 0.0 + # # # else subscriber == true + # # custom_orientation: + # # roll: 0.0 + # # pitch: 0.0 + # # yaw: 0.0 + # # covariance: 0.0 + # # horizontal_fov_ratio: 0.0 + # # vertical_fov_ratio: 0.0 + + # obstacle node + # + # The mav_frame does not appear to be used to transform the data + # on either the mavros side or in ArduPilot SITL. The correct frame + # for data received from Gazebo is BODY_FLU, but this is not available + # as an option. + # obstacle: + # ros__parameters: + # mav_frame: BODY_FRD + + diff --git a/terrain_navigation_ros/config/ap_pluginlists.yaml b/terrain_navigation_ros/config/ap_pluginlists.yaml new file mode 100644 index 00000000..896791c7 --- /dev/null +++ b/terrain_navigation_ros/config/ap_pluginlists.yaml @@ -0,0 +1,21 @@ +/mavros/**: + ros__parameters: + plugin_denylist: + # common + - actuator_control + - ftp + - hil + # extras + - altitude + - cellular_status + - debug_value + - fake_gps + - guided_target + - image_pub + - px4flow + - vibration + - vision_speed_estimate + - wheel_odometry + + # plugin_allowlist: + # - 'sys_*' diff --git a/terrain_navigation_ros/config/davosdorf_mission.plan b/terrain_navigation_ros/config/davosdorf_mission.plan new file mode 100644 index 00000000..9304284f --- /dev/null +++ b/terrain_navigation_ros/config/davosdorf_mission.plan @@ -0,0 +1,89 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 12, + "globalPlanAltitudeMode": 1, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 84, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 46.814186199999995, + 9.8481572, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 50, + "AltitudeMode": 1, + "autoContinue": true, + "command": 17, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 80, + null, + 46.8148827, + 9.8493624, + 50 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 85, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 46.8132054, + 9.8479537, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 46.813209, + 9.8479582, + 1562 + ], + "vehicleType": 20, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +} diff --git a/terrain_navigation_ros/config/davosdorf_mission.txt b/terrain_navigation_ros/config/davosdorf_mission.txt new file mode 100644 index 00000000..2fe7e393 --- /dev/null +++ b/terrain_navigation_ros/config/davosdorf_mission.txt @@ -0,0 +1,5 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 46.813205 9.847954 1513.099976 1 +1 0 3 84 0.000000 0.000000 0.000000 0.000000 46.813205 9.847954 50.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 46.814614 9.849095 50.000000 1 +3 0 3 17 0.000000 0.000000 80.000000 0.000000 46.814614 9.849095 50.000000 1 diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/geo_conversions.h b/terrain_navigation_ros/include/terrain_navigation_ros/geo_conversions.h index 332d82a3..b2befdf0 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/geo_conversions.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/geo_conversions.h @@ -35,8 +35,6 @@ #ifndef GEOCONVERSIONS_H #define GEOCONVERSIONS_H -#include - class GeoConversions { public: GeoConversions(); @@ -52,31 +50,7 @@ class GeoConversions { * @param y * @param h */ - static void forward(const double lat, const double lon, const double alt, double &y, double &x, double &h) { - // 1. Convert the ellipsoidal latitudes φ and longitudes λ into arcseconds ["] - const double lat_arc = lat * 3600.0; - const double lon_arc = lon * 3600.0; - - // 2. Calculate the auxiliary values (differences of latitude and longitude relative to Bern in the unit [10000"]): - // φ' = (φ – 169028.66 ")/10000 - // λ' = (λ – 26782.5 ")/10000 - const double lat_aux = (lat_arc - 169028.66) / 10000.0; - const double lon_aux = (lon_arc - 26782.5) / 10000.0; - - // 3. Calculate projection coordinates in LV95 (E, N, h) or in LV03 (y, x, h) - // E [m] = 2600072.37 + 211455.93 * λ' - 10938.51 * λ' * φ' - 0.36 * λ' * φ'2 - 44.54 * λ'3 - // y [m] = E – 2000000.00 N [m] = 1200147.07 + 308807.95 * φ' + 3745.25 * λ'2 + 76.63 * φ'2 - 194.56 * λ'2 * φ' + - // 119.79 * φ'3 x [m] = N – 1000000.00 - // hCH [m] =hWGS – 49.55 + 2.73 * λ' + 6.94 * φ' - const double E = 2600072.37 + 211455.93 * lon_aux - 10938.51 * lon_aux * lat_aux - - 0.36 * lon_aux * std::pow(lat_aux, 2) - 44.54 * std::pow(lon_aux, 3); - y = E - 2000000.00; - const double N = 1200147.07 + 308807.95 * lat_aux + 3745.25 * std::pow(lon_aux, 2) + 76.63 * std::pow(lat_aux, 2) - - 194.56 * std::pow(lon_aux, 2) * lat_aux + 119.79 * std::pow(lat_aux, 3); - x = N - 1000000.00; - - h = alt - 49.55 + 2.73 * lon_aux + 6.84 * lat_aux; - }; + static void forward(const double lat, const double lon, const double alt, double &y, double &x, double &h); /** * @brief LV03/CH1903 to Convert WGS84 (LLA) @@ -88,29 +62,7 @@ class GeoConversions { * @param lon longitude * @param alt altitude */ - static void reverse(const double y, const double x, const double h, double &lat, double &lon, double &alt) { - // 1. Convert the projection coordinates E (easting) and N (northing) in LV95 (or y / x in LV03) into the civilian - // system (Bern = 0 / 0) and express in the unit [1000 km]: E' = (E – 2600000 m)/1000000 = (y – 600000 m)/1000000 - // N' = (N – 1200000 m)/1000000 = (x – 200000 m)/1000000 - const double y_aux = (y - 600000.0) / 1000000.0; - const double x_aux = (x - 200000.0) / 1000000.0; - - // 2. Calculate longitude λ and latitude φ in the unit [10000"]: - // λ' = 2.6779094 + 4.728982 * y' + 0.791484* y' * x' + 0.1306 * y' * x'2 - 0.0436 * y'3 - // φ' = 16.9023892 + 3.238272 * x' - 0.270978 * y'2 - 0.002528 * x'2 - 0.0447 * y'2 * x' - 0.0140 * x'3 - // hWGS [m] = hCH + 49.55 - 12.60 * y' - 22.64 * x' - const double lon_aux = 2.6779094 + 4.728982 * y_aux + 0.791484 * y_aux * x_aux + - 0.1306 * y_aux * std::pow(x_aux, 2) - 0.0436 * std::pow(y_aux, 3); - const double lat_aux = 16.9023892 + 3.238272 * x_aux - 0.270978 * std::pow(y_aux, 2) - - 0.002528 * std::pow(x_aux, 2) - 0.0447 * std::pow(y_aux, 2) * x_aux - - 0.0140 * std::pow(x_aux, 3); - alt = h + 49.55 - 12.60 * y_aux - 22.64 * x_aux; - - lon = lon_aux * 100.0 / 36.0; - lat = lat_aux * 100.0 / 36.0; - }; - - private: + static void reverse(const double y, const double x, const double h, double &lat, double &lon, double &alt); }; #endif diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h index 4eb0533e..926a511e 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/terrain_planner.h @@ -42,8 +42,15 @@ #ifndef TERRAIN_PLANNER_H #define TERRAIN_PLANNER_H -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -55,47 +62,20 @@ #include #include #include +#include #include #include -#include -#include - -#include - -#include -#include - #include #include #include #include - #include +#include +#include -#include -#include - -#include -#include -#include - -enum class PLANNER_MODE { - ACTIVE_MAPPING, - EMERGENCY_ABORT, - EXHAUSTIVE, - GLOBAL, - GLOBAL_REPLANNING, - RANDOM, - RETURN -}; +enum class PLANNER_MODE { ACTIVE_MAPPING, EMERGENCY_ABORT, EXHAUSTIVE, GLOBAL, GLOBAL_REPLANNING, RANDOM, RETURN }; -enum class PLANNER_STATE { - HOLD = 1, - NAVIGATE = 2, - ROLLOUT = 3, - ABORT = 4, - RETURN = 5 -}; +enum class PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 }; class TerrainPlanner : public rclcpp::Node { public: @@ -122,54 +102,49 @@ class TerrainPlanner : public rclcpp::Node { void mavMissionCallback(const mavros_msgs::msg::WaypointList &msg); void mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured &msg); - bool setLocationCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setMaxAltitudeCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setGoalCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setStartCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setCurrentSegmentCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setStartLoiterCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setPlanningCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setPlannerStateCallback( - const std::shared_ptr req, - std::shared_ptr res); - bool setPathCallback( - const std::shared_ptr req, - std::shared_ptr res); + bool setLocationCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setMaxAltitudeCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setGoalCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setStartCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setCurrentSegmentCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setStartLoiterCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setPlanningCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setPlannerStateCallback(const std::shared_ptr req, + std::shared_ptr res); + bool setPathCallback(const std::shared_ptr req, + std::shared_ptr res); void MapPublishOnce(rclcpp::Publisher::SharedPtr pub, const grid_map::GridMap &map); void publishPositionHistory(rclcpp::Publisher::SharedPtr, const Eigen::Vector3d &position, std::vector &history_vector); - void publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity, const double curvature); - void publishGlobalPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const double latitude, const double longitude, - const double altitude, const Eigen::Vector3d &velocity, const double curvature); - void publishReferenceMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity, const double curvature); - void publishVelocityMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity); - void publishPathSetpoints(const Eigen::Vector3d &position, const Eigen::Vector3d &velocity); + void publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, + const double curvature); + void publishGlobalPositionSetpoints(rclcpp::Publisher::SharedPtr pub, + const double latitude, const double longitude, const double altitude, + const Eigen::Vector3d &velocity, const double curvature); + void publishReferenceMarker(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, const double curvature); + void publishVelocityMarker(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector3d &velocity); void publishPathSegments(rclcpp::Publisher::SharedPtr, Path &trajectory); - void publishGoal(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const double radius, - Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "goal"); - void publishRallyPoints(rclcpp::Publisher::SharedPtr pub, const std::vector &positions, const double radius, + void publishGoal(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, + const double radius, Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), + std::string name_space = "goal"); + void publishRallyPoints(rclcpp::Publisher::SharedPtr pub, + const std::vector &positions, const double radius, Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "rallypoints"); + // Create a goal marker, a circle located at position with given radius and color. visualization_msgs::msg::Marker getGoalMarker(const int id, const Eigen::Vector3d &position, const double radius, - const Eigen::Vector3d color); + const Eigen::Vector3d color); void generateCircle(const Eigen::Vector3d end_position, const Eigen::Vector3d end_velocity, const Eigen::Vector3d center_pos, PathSegment &trajectory); PathSegment generateArcTrajectory(Eigen::Vector3d rates, const double horizon, Eigen::Vector3d current_pos, @@ -205,10 +180,8 @@ class TerrainPlanner : public rclcpp::Node { rclcpp::Publisher::SharedPtr camera_pose_pub_; rclcpp::Publisher::SharedPtr posehistory_pub_; rclcpp::Publisher::SharedPtr referencehistory_pub_; - rclcpp::Publisher::SharedPtr position_setpoint_pub_; rclcpp::Publisher::SharedPtr global_position_setpoint_pub_; rclcpp::Publisher::SharedPtr position_target_pub_; - rclcpp::Publisher::SharedPtr path_target_pub_; rclcpp::Publisher::SharedPtr planner_status_pub_; rclcpp::Publisher::SharedPtr goal_pub_; rclcpp::Publisher::SharedPtr rallypoint_pub_; @@ -258,6 +231,9 @@ class TerrainPlanner : public rclcpp::Node { // std::unique_ptr plannerloop_spinner_; // std::unique_ptr statusloop_spinner_; // std::unique_ptr cmdloop_spinner_; + // std::shared_ptr plannerloop_node_; + // std::shared_ptr statusloop_node_; + // std::shared_ptr cmdloop_node_; rclcpp::executors::SingleThreadedExecutor cmdloop_executor_; rclcpp::executors::SingleThreadedExecutor statusloop_executor_; rclcpp::executors::SingleThreadedExecutor plannerloop_executor_; @@ -298,9 +274,13 @@ class TerrainPlanner : public rclcpp::Node { // Altitude tracking loop parameters /// TODO: This needs to be handed over to TECS - double K_z_{2.0}; - double cruise_speed_{20.0}; - double max_climb_rate_control_{5.0}; + + // Altitude controller P gain. + double K_z_{0.5}; + // Trim (cruise) airpseed. Set to PX4 FW_AIRSPD_TRIM. + double cruise_speed_{15.0}; + // Altitude controller max climb rate. Set to PX4 FW_T_CLMB_R_SP. + double max_climb_rate_control_{3.0}; std::string map_path_{}; std::string map_color_path_{}; diff --git a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h index 9f27a2a2..ca3ae70d 100644 --- a/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h +++ b/terrain_navigation_ros/include/terrain_navigation_ros/visualization.h @@ -34,121 +34,33 @@ #ifndef TERRAIN_NAVIGATION_ROS_VISUALIZATION_H #define TERRAIN_NAVIGATION_ROS_VISUALIZATION_H -#include - -#include -#include -#include +#include -#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p); -inline geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { - geometry_msgs::msg::Point position; - position.x = p(0); - position.y = p(1); - position.z = p(2); - return position; -} - -inline void publishVehiclePose( - rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d &position, - const Eigen::Vector4d &attitude, - std::string mesh_resource_path) { - Eigen::Vector4d mesh_attitude = - quatMultiplication(attitude, Eigen::Vector4d(std::cos(M_PI / 2), 0.0, 0.0, std::sin(M_PI / 2))); - geometry_msgs::msg::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude); - visualization_msgs::msg::Marker marker; - marker.header.stamp = rclcpp::Clock().now(); - marker.header.frame_id = "map"; - marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; - marker.ns = "my_namespace"; - marker.mesh_resource = "package://terrain_planner/" + mesh_resource_path; - marker.scale.x = 10.0; - marker.scale.y = 10.0; - marker.scale.z = 10.0; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 0.5; - marker.color.g = 0.5; - marker.color.b = 0.5; - marker.pose = vehicle_pose; - pub->publish(marker); -} - -inline visualization_msgs::msg::Marker Viewpoint2MarkerMsg( - int id, ViewPoint &viewpoint, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { - double scale{15}; // Size of the viewpoint markers - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Clock().now(); - marker.ns = "my_namespace"; - marker.id = id; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - const Eigen::Vector3d position = viewpoint.getCenterLocal(); - std::vector points; - std::vector corner_ray_vectors = viewpoint.getCornerRayVectors(); - std::vector vertex; - for (auto &corner_ray : corner_ray_vectors) { - vertex.push_back(position + scale * corner_ray); - } - - for (size_t i = 0; i < vertex.size(); i++) { - points.push_back(toPoint(position)); // Viewpoint center - points.push_back(toPoint(vertex[i])); - points.push_back(toPoint(vertex[i])); - points.push_back(toPoint(vertex[(i + 1) % vertex.size()])); - } - - marker.points = points; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 1.0; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = color(0); - marker.color.g = color(1); - marker.color.b = color(2); - return marker; -} - -inline void publishCameraView( - rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d &position, - const Eigen::Vector4d &attitude) { - visualization_msgs::msg::Marker marker; - ViewPoint viewpoint(-1, position, attitude); - marker = Viewpoint2MarkerMsg(viewpoint.getIndex(), viewpoint); - pub->publish(marker); -} +void publishVehiclePose(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector4d &attitude, + std::string mesh_resource_path); -inline void publishViewpoints( - rclcpp::Publisher::SharedPtr pub, - std::vector &viewpoint_vector, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)) { - visualization_msgs::msg::MarkerArray msg; +visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)); - std::vector marker; - visualization_msgs::msg::Marker mark; - mark.action = visualization_msgs::msg::Marker::DELETEALL; - marker.push_back(mark); - msg.markers = marker; - pub->publish(msg); +void publishCameraView(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector4d &attitude); - std::vector viewpoint_marker_vector; - int i = 0; - for (auto viewpoint : viewpoint_vector) { - viewpoint_marker_vector.insert(viewpoint_marker_vector.begin(), Viewpoint2MarkerMsg(i, viewpoint, color)); - i++; - } - msg.markers = viewpoint_marker_vector; - pub->publish(msg); -} +void publishViewpoints(rclcpp::Publisher::SharedPtr pub, + std::vector &viewpoint_vector, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0)); #endif diff --git a/terrain_navigation_ros/launch/ardupilot.launch.py b/terrain_navigation_ros/launch/ardupilot.launch.py new file mode 100644 index 00000000..2407702a --- /dev/null +++ b/terrain_navigation_ros/launch/ardupilot.launch.py @@ -0,0 +1,77 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + """Generate a launch description.""" + + pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl") + + # TODO: hardcoded home location (lat, lon, alt, yaw) for davosdorf example. + latitude_deg = 46.8132056 + longitude_deg = 9.8479538 + elevation = 1562.0 + heading_deg = 0 + home = f"'{latitude_deg}, {longitude_deg}, {elevation}, {heading_deg}'" + + # ardupilot sitl node with DDS and mavproxy + sitl_dds = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("ardupilot_sitl"), + "launch", + "sitl_dds_udp.launch.py", + ] + ), + ] + ), + launch_arguments={ + "transport": "udp4", + "refs": PathJoinSubstitution( + [ + FindPackageShare("ardupilot_sitl"), + "config", + "dds_xrce_profile.xml", + ] + ), + "port": "2019", + "command": "arduplane", + "synthetic_clock": "True", + "wipe": "False", + "model": "quadplane", + "speedup": "1", + "slave": "0", + "instance": "0", + "defaults": os.path.join( + pkg_ardupilot_sitl, + "config", + "default_params", + "quadplane.parm", + ) + + "," + + os.path.join( + pkg_ardupilot_sitl, + "config", + "default_params", + "dds_udp.parm", + ), + "home": home, + "sim_address": "127.0.0.1", + "master": "tcp:127.0.0.1:5760", + "sitl": "127.0.0.1:5501", + }.items(), + ) + + return LaunchDescription( + [ + sitl_dds, + ] + ) diff --git a/terrain_navigation_ros/launch/config_visualize.rviz b/terrain_navigation_ros/launch/config_visualize.rviz deleted file mode 100644 index b2ba62c2..00000000 --- a/terrain_navigation_ros/launch/config_visualize.rviz +++ /dev/null @@ -1,208 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /GridMap1 - - /MarkerArray1 - - /MarkerArray2 - - /Path1 - - /Path2 - Splitter Ratio: 0.5 - Tree Height: 916 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.5 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 200; 200; 200 - Color Layer: geometric_prior - Color Transformer: IntensityLayer - Enabled: true - Height Layer: elevation - Height Transformer: Layer - History Length: 1 - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 1 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: GridMap - Show Grid Lines: false - Topic: /grid_map - Unreliable: false - Use Rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /planner/viewpoints - Name: MarkerArray - Namespaces: - my_namespace: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /coverage/viewpoints - Name: MarkerArray - Namespaces: - my_namespace: true - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /coverage/camera_path - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /planner/camera_path - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1709.5120849609375 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -576.2509765625 - Y: 202.5871124267578 - Z: -84.47080993652344 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.604800820350647 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.47487032413482666 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1145 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001df0000041ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000041f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000208000001aa0000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006500000000000000073d000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000004430000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1853 - X: 67 - Y: 27 diff --git a/terrain_navigation_ros/launch/mavros.launch.py b/terrain_navigation_ros/launch/mavros.launch.py new file mode 100644 index 00000000..eb42f1fd --- /dev/null +++ b/terrain_navigation_ros/launch/mavros.launch.py @@ -0,0 +1,60 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + # package directories + pkg_mavros = get_package_share_directory("mavros") + pkg_terrain_navigation_ros = get_package_share_directory("terrain_navigation_ros") + + # TODO: remove hardcoding + + # mavros config + # ardupilot + fcu_url = "udp://127.0.0.1:14551@14555" + # px4 + fcu_url = "udp://127.0.0.1:14540@14557" + + gcs_url = "" + tgt_system = 1 + tgt_component = 1 + fcu_protocol = "v2.0" + respawn_mavros = False + namespace = "mavros" + + # ardupilot + config_yaml = os.path.join(pkg_terrain_navigation_ros, "config", "ap_config.yaml") + pluginlists_yaml = os.path.join( + pkg_terrain_navigation_ros, "config", "ap_pluginlists.yaml" + ) + # px4 + config_yaml = os.path.join(pkg_mavros, "launch", "px4_config.yaml") + pluginlists_yaml = os.path.join(pkg_mavros, "launch", "px4_pluginlists.yaml") + + # mavros node + mavros = Node( + package="mavros", + executable="mavros_node", + namespace=namespace, + parameters=[ + {"fcu_url": fcu_url}, + {"gcs_url": gcs_url}, + {"tgt_system": tgt_system}, + {"tgt_component": tgt_component}, + {"fcu_protocol": fcu_protocol}, + pluginlists_yaml, + config_yaml, + ], + remappings=[], + respawn=respawn_mavros, + output="screen", + ) + + return LaunchDescription( + [ + mavros, + ] + ) diff --git a/terrain_navigation_ros/launch/rviz.launch.py b/terrain_navigation_ros/launch/rviz.launch.py new file mode 100644 index 00000000..e3580851 --- /dev/null +++ b/terrain_navigation_ros/launch/rviz.launch.py @@ -0,0 +1,24 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Generate a launch description.""" + + pkg_terrain_planner = get_package_share_directory("terrain_planner") + + # rviz node + rviz = Node( + package="rviz2", + executable="rviz2", + arguments=["-d", os.path.join(pkg_terrain_planner, "launch", "config.rviz")], + ) + + return LaunchDescription( + [ + rviz, + ] + ) diff --git a/terrain_navigation_ros/launch/test_terrain_planner.launch.py b/terrain_navigation_ros/launch/terrain_planner.launch.py similarity index 52% rename from terrain_navigation_ros/launch/test_terrain_planner.launch.py rename to terrain_navigation_ros/launch/terrain_planner.launch.py index 12aa29e8..48af87b8 100644 --- a/terrain_navigation_ros/launch/test_terrain_planner.launch.py +++ b/terrain_navigation_ros/launch/terrain_planner.launch.py @@ -1,7 +1,6 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchContext from launch import LaunchDescription from launch.actions import DeclareLaunchArgument @@ -9,7 +8,6 @@ from launch.actions import OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration - from launch_ros.actions import Node @@ -22,24 +20,29 @@ def generate_terrain_navigation_action( # arguments. location = LaunchConfiguration("location").perform(context) - - # terrain navigation node - terrain_path = os.path.join( - pkg_terrain_navigation_ros, - "../resources", - location + ".tf", + minimum_turn_radius = float( + LaunchConfiguration("minimum_turn_radius").perform(context) ) - terrain_color_path = os.path.join( - pkg_terrain_navigation_ros, - "../resources", - location + "_color.tf", + alt_control_p = float(LaunchConfiguration("alt_control_p").perform(context)) + alt_control_max_climb_rate = float( + LaunchConfiguration("alt_control_max_climb_rate").perform(context) ) - resource_path = os.path.join(pkg_terrain_navigation_ros, "../resources") + cruise_speed = float(LaunchConfiguration("cruise_speed").perform(context)) + + # terrain navigation node + resource_path = os.path.join(pkg_terrain_navigation_ros, "resources") + terrain_path = os.path.join(resource_path, location + ".tif") + terrain_color_path = os.path.join(resource_path, location + "_color.tif") + meshresource_path = os.path.join(resource_path, "believer.dae") # debug - check the context is resolved correctly. - # print(f"terrain_path: {terrain_path}") - # print(f"terrain_color_path: {terrain_color_path}") - # print(f"resource_path: {resource_path}") + # print(f"resource_path: {resource_path}") + # print(f"terrain_path: {terrain_path}") + # print(f"terrain_color_path: {terrain_color_path}") + # print(f"minimum_turn_radius: {minimum_turn_radius}") + # print(f"alt_control_p: {alt_control_p}") + # print(f"alt_control_max_climb_rate: {alt_control_max_climb_rate}") + # print(f"cruise_speed: {cruise_speed}") # Create action. node = Node( @@ -47,10 +50,14 @@ def generate_terrain_navigation_action( executable="terrain_planner_node", name="terrain_planner", parameters=[ + {"meshresource_path": meshresource_path}, + {"minimum_turn_radius": minimum_turn_radius}, + {"resource_path": resource_path}, {"terrain_path": terrain_path}, {"terrain_color_path": terrain_color_path}, - {"resource_path": resource_path}, - {"minimum_turn_radius": "80.0"}, + {"alt_control_p": alt_control_p}, + {"alt_control_max_climb_rate": alt_control_max_climb_rate}, + {"cruise_speed": cruise_speed}, ], output="screen", ) @@ -63,10 +70,13 @@ def generate_launch_description(): pkg_terrain_planner = get_package_share_directory("terrain_planner") # mavros node - # TODO include launch description + # TODO include launch description # px4 posix_sitl node - # TODO include launch description + # TODO include launch description + + # ardupilot_sitl node + # TODO include launch description # terrain navigation node terrain_navigation = OpaqueFunction(function=generate_terrain_navigation_action) @@ -81,47 +91,34 @@ def generate_launch_description(): return LaunchDescription( [ + # terrain_planner arguments DeclareLaunchArgument( - "mav_name", default_value="tiltrotor", description="Vehicle name." - ), - DeclareLaunchArgument( - "fcu_url", - default_value="udp://:14540@127.0.0.1:14557", - description="Flight controller address.", - ), - DeclareLaunchArgument( - "gcs_url", - default_value="", - description="Ground control station address.", + "location", default_value="davosdorf", description="Location." ), - DeclareLaunchArgument("tgt_system", default_value="1", description=""), - DeclareLaunchArgument("tgt_component", default_value="1", description=""), - DeclareLaunchArgument("command_input", default_value="2", description=""), DeclareLaunchArgument( - "gazebo_simulation", default_value="true", description="Run Gazebo." + "minimum_turn_radius", + default_value="80.0", + description="Minimum turn radius.", ), DeclareLaunchArgument( - "log_output", - default_value="screen", - description="Location to log output.", + "alt_control_p", + default_value="0.5", + description="Altitude controller proportional gain.", ), DeclareLaunchArgument( - "fcu_protocol", - default_value="v2.0", - description="Flight controller protocol version.", + "alt_control_max_climb_rate", + default_value="3.0", + description="Altitude controller maximim climb rate (m/s).", ), DeclareLaunchArgument( - "respawn_mavros", default_value="false", description="Respawn mavros." + "cruise_speed", + default_value="15.0", + description="Vehicle cruise speed (m/s).", ), + # rviz arguments DeclareLaunchArgument( "rviz", default_value="true", description="Open RViz." ), - DeclareLaunchArgument( - "location", default_value="davosdorf", description="Location." - ), - DeclareLaunchArgument( - "gui", default_value="false", description="Open SITL GUI." - ), terrain_navigation, rviz, ] diff --git a/terrain_navigation_ros/launch/test_terrain_planner.launch b/terrain_navigation_ros/launch/test_terrain_planner.launch deleted file mode 100644 index c223e899..00000000 --- a/terrain_navigation_ros/launch/test_terrain_planner.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/terrain_navigation_ros/launch/visualize_path.launch b/terrain_navigation_ros/launch/visualize_path.launch deleted file mode 100644 index 69b0c498..00000000 --- a/terrain_navigation_ros/launch/visualize_path.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/resources/believer.dae b/terrain_navigation_ros/resources/believer.dae similarity index 100% rename from resources/believer.dae rename to terrain_navigation_ros/resources/believer.dae diff --git a/resources/davosdorf.tif b/terrain_navigation_ros/resources/davosdorf.tif similarity index 100% rename from resources/davosdorf.tif rename to terrain_navigation_ros/resources/davosdorf.tif diff --git a/resources/davosdorf.world b/terrain_navigation_ros/resources/davosdorf.world similarity index 100% rename from resources/davosdorf.world rename to terrain_navigation_ros/resources/davosdorf.world diff --git a/resources/davosdorf_color.tif b/terrain_navigation_ros/resources/davosdorf_color.tif similarity index 100% rename from resources/davosdorf_color.tif rename to terrain_navigation_ros/resources/davosdorf_color.tif diff --git a/terrain_navigation_ros/scripts/global_postion_relay.py b/terrain_navigation_ros/scripts/global_postion_relay.py new file mode 100755 index 00000000..80cbd044 --- /dev/null +++ b/terrain_navigation_ros/scripts/global_postion_relay.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 + +""" +ROS 2 node to convert from mavros_msgs/GlobalPositionTarget Message +to ardupilot_msgs/GlobalPosition +""" + +import sys + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from mavros_msgs.msg import GlobalPositionTarget +from ardupilot_msgs.msg import GlobalPosition + + +class GlobalPositionRelay(Node): + def __init__(self): + super().__init__("global_position_relay") + + self.pub = self.create_publisher( + GlobalPosition, + "/ap/cmd_gps_pose", + 10, + ) + + self.sub = self.create_subscription( + GlobalPositionTarget, + "/mavros/setpoint_raw/global", + self.global_position_cb, + 10, + ) + + def global_position_cb(self, msg): + + pos_msg = GlobalPosition() + # header + pos_msg.header = msg.header + pos_msg.header.frame_id = "map" + # coordinate frame and type mask + pos_msg.coordinate_frame = msg.coordinate_frame + pos_msg.type_mask = msg.type_mask + # geodetic position (datum AMSL) + pos_msg.latitude = msg.latitude + pos_msg.longitude = msg.longitude + pos_msg.altitude = msg.altitude + # velocity + pos_msg.velocity.linear.x = msg.velocity.x + pos_msg.velocity.linear.y = msg.velocity.y + pos_msg.velocity.linear.z = msg.velocity.z + # acceleration or force + pos_msg.acceleration_or_force.linear.x = msg.acceleration_or_force.x + pos_msg.acceleration_or_force.linear.y = msg.acceleration_or_force.y + pos_msg.acceleration_or_force.linear.z = msg.acceleration_or_force.z + # yaw rate + pos_msg.velocity.angular.z = msg.yaw_rate + # TODO: yaw (no corresponding member in GlobalPosition) + + self.pub.publish(pos_msg) + + self.get_logger().info("recv: {}\n".format(msg)) + self.get_logger().info("send: {}\n".format(pos_msg)) + + +def main(args=None): + rclpy.init(args=args) + + node = GlobalPositionRelay() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/terrain_navigation_ros/src/geo_conversions.cpp b/terrain_navigation_ros/src/geo_conversions.cpp new file mode 100644 index 00000000..c2095a9b --- /dev/null +++ b/terrain_navigation_ros/src/geo_conversions.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Jaeyoung Lim, Autonomous Systems Lab, + * ETH Zürich. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "terrain_navigation_ros/geo_conversions.h" + +#include + +GeoConversions::GeoConversions() = default; + +GeoConversions::~GeoConversions() = default; + +void GeoConversions::forward(const double lat, const double lon, const double alt, double &y, double &x, double &h) { + // 1. Convert the ellipsoidal latitudes φ and longitudes λ into arcseconds ["] + const double lat_arc = lat * 3600.0; + const double lon_arc = lon * 3600.0; + + // 2. Calculate the auxiliary values (differences of latitude and longitude relative to Bern in the unit [10000"]): + // φ' = (φ – 169028.66 ")/10000 + // λ' = (λ – 26782.5 ")/10000 + const double lat_aux = (lat_arc - 169028.66) / 10000.0; + const double lon_aux = (lon_arc - 26782.5) / 10000.0; + + // 3. Calculate projection coordinates in LV95 (E, N, h) or in LV03 (y, x, h) + // E [m] = 2600072.37 + 211455.93 * λ' - 10938.51 * λ' * φ' - 0.36 * λ' * φ'2 - 44.54 * λ'3 + // y [m] = E – 2000000.00 N [m] = 1200147.07 + 308807.95 * φ' + 3745.25 * λ'2 + 76.63 * φ'2 - 194.56 * λ'2 * φ' + + // 119.79 * φ'3 x [m] = N – 1000000.00 + // hCH [m] =hWGS – 49.55 + 2.73 * λ' + 6.94 * φ' + const double E = 2600072.37 + 211455.93 * lon_aux - 10938.51 * lon_aux * lat_aux - + 0.36 * lon_aux * std::pow(lat_aux, 2) - 44.54 * std::pow(lon_aux, 3); + y = E - 2000000.00; + const double N = 1200147.07 + 308807.95 * lat_aux + 3745.25 * std::pow(lon_aux, 2) + 76.63 * std::pow(lat_aux, 2) - + 194.56 * std::pow(lon_aux, 2) * lat_aux + 119.79 * std::pow(lat_aux, 3); + x = N - 1000000.00; + + h = alt - 49.55 + 2.73 * lon_aux + 6.84 * lat_aux; +}; + +void GeoConversions::reverse(const double y, const double x, const double h, double &lat, double &lon, double &alt) { + // 1. Convert the projection coordinates E (easting) and N (northing) in LV95 (or y / x in LV03) into the civilian + // system (Bern = 0 / 0) and express in the unit [1000 km]: E' = (E – 2600000 m)/1000000 = (y – 600000 m)/1000000 + // N' = (N – 1200000 m)/1000000 = (x – 200000 m)/1000000 + const double y_aux = (y - 600000.0) / 1000000.0; + const double x_aux = (x - 200000.0) / 1000000.0; + + // 2. Calculate longitude λ and latitude φ in the unit [10000"]: + // λ' = 2.6779094 + 4.728982 * y' + 0.791484* y' * x' + 0.1306 * y' * x'2 - 0.0436 * y'3 + // φ' = 16.9023892 + 3.238272 * x' - 0.270978 * y'2 - 0.002528 * x'2 - 0.0447 * y'2 * x' - 0.0140 * x'3 + // hWGS [m] = hCH + 49.55 - 12.60 * y' - 22.64 * x' + const double lon_aux = 2.6779094 + 4.728982 * y_aux + 0.791484 * y_aux * x_aux + 0.1306 * y_aux * std::pow(x_aux, 2) - + 0.0436 * std::pow(y_aux, 3); + const double lat_aux = 16.9023892 + 3.238272 * x_aux - 0.270978 * std::pow(y_aux, 2) - 0.002528 * std::pow(x_aux, 2) - + 0.0447 * std::pow(y_aux, 2) * x_aux - 0.0140 * std::pow(x_aux, 3); + alt = h + 49.55 - 12.60 * y_aux - 22.64 * x_aux; + + lon = lon_aux * 100.0 / 36.0; + lat = lat_aux * 100.0 / 36.0; +}; diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp index 0bea9a1a..80413f98 100644 --- a/terrain_navigation_ros/src/terrain_planner.cpp +++ b/terrain_navigation_ros/src/terrain_planner.cpp @@ -41,39 +41,68 @@ #include "terrain_navigation_ros/terrain_planner.h" +#include + #include #include -#include - -#include "terrain_navigation_ros/geo_conversions.h" -#include "terrain_navigation_ros/visualization.h" - #include +#include #include -#include #include #include #include +#include +#include #include +#include #include #include -#include -#include -#include +#include "terrain_navigation_ros/geo_conversions.h" +#include "terrain_navigation_ros/visualization.h" using std::placeholders::_1; using std::placeholders::_2; using namespace std::chrono_literals; +TerrainPlanner::TerrainPlanner() : Node("terrain_planner") { + std::string avalanche_map_path; + + // original parameters: resource locations and goal radius + resource_path_ = this->declare_parameter("resource_path", "resources"); + map_path_ = this->declare_parameter("terrain_path", resource_path_ + "/davosdorf.tif"); + map_color_path_ = this->declare_parameter("terrain_color_path", resource_path_ + "/davosdorf_color"); + mesh_resource_path_ = this->declare_parameter("meshresource_path", resource_path_ + "/believer.dae"); + avalanche_map_path = this->declare_parameter("avalanche_map_path", resource_path_ + "/avalanche.tif"); + goal_radius_ = this->declare_parameter("minimum_turn_radius", 66.67); -TerrainPlanner::TerrainPlanner() - : Node("terrain_planner") { + RCLCPP_INFO_STREAM(this->get_logger(), "resource_path: " << resource_path_); + RCLCPP_INFO_STREAM(this->get_logger(), "map_path_: " << map_path_); + RCLCPP_INFO_STREAM(this->get_logger(), "map_color_path_: " << map_color_path_); + RCLCPP_INFO_STREAM(this->get_logger(), "mesh_resource_path_: " << mesh_resource_path_); + RCLCPP_INFO_STREAM(this->get_logger(), "avalanche_map_path: " << avalanche_map_path); + RCLCPP_INFO_STREAM(this->get_logger(), "goal_radius_: " << goal_radius_); + + // additional parameters: vehicle guidance + K_z_ = this->declare_parameter("alt_control_p", 0.5); + max_climb_rate_control_ = this->declare_parameter("alt_control_max_climb_rate", 3.0); + cruise_speed_ = this->declare_parameter("cruise_speed", 15.0); + + RCLCPP_INFO_STREAM(this->get_logger(), "alt_control_p: " << K_z_); + RCLCPP_INFO_STREAM(this->get_logger(), "alt_control_max_climb_rate: " << max_climb_rate_control_); + RCLCPP_INFO_STREAM(this->get_logger(), "cruise_speed: " << cruise_speed_); + + // quality of service settings + rclcpp::QoS latching_qos(1); + latching_qos.reliable().transient_local(); + + auto mavros_position_qos = rclcpp::SensorDataQoS(); + // mavros_position_qos.best_effort(); vehicle_path_pub_ = this->create_publisher("vehicle_path", 1); //! @todo(srmainwaring) add latching - grid_map_pub_ = this->create_publisher("grid_map", 1); + grid_map_pub_ = this->create_publisher("grid_map", latching_qos); posehistory_pub_ = this->create_publisher("geometric_controller/path", 10); referencehistory_pub_ = this->create_publisher("reference/path", 10); @@ -85,15 +114,12 @@ TerrainPlanner::TerrainPlanner() candidate_start_pub_ = this->create_publisher("candidate_start_marker", 1); mavstate_sub_ = this->create_subscription( - "mavros/state", 1, - std::bind(&TerrainPlanner::mavstateCallback, this, _1)); + "mavros/state", 1, std::bind(&TerrainPlanner::mavstateCallback, this, _1)); mavmission_sub_ = this->create_subscription( - "mavros/mission/waypoints", 1, - std::bind(&TerrainPlanner::mavMissionCallback, this, _1)); + "mavros/mission/waypoints", 1, std::bind(&TerrainPlanner::mavMissionCallback, this, _1)); - position_setpoint_pub_ = this->create_publisher("mavros/setpoint_raw/local", 1); - global_position_setpoint_pub_ = this->create_publisher("mavros/setpoint_raw/global", 1); - path_target_pub_ = this->create_publisher("mavros/trajectory/generated", 1); + global_position_setpoint_pub_ = + this->create_publisher("mavros/setpoint_raw/global", 1); vehicle_pose_pub_ = this->create_publisher("vehicle_pose_marker", 1); camera_pose_pub_ = this->create_publisher("camera_pose_marker", 1); planner_status_pub_ = this->create_publisher("planner_status", 1); @@ -103,61 +129,41 @@ TerrainPlanner::TerrainPlanner() tree_pub_ = this->create_publisher("tree", 1); mavlocalpose_sub_ = this->create_subscription( - "mavros/local_position/pose", 1, - std::bind(&TerrainPlanner::mavLocalPoseCallback, this, _1)); + "mavros/local_position/pose", mavros_position_qos, std::bind(&TerrainPlanner::mavLocalPoseCallback, this, _1)); mavglobalpose_sub_ = this->create_subscription( - "mavros/global_position/global", 1, + "mavros/global_position/global", mavros_position_qos, std::bind(&TerrainPlanner::mavGlobalPoseCallback, this, _1)); mavtwist_sub_ = this->create_subscription( - "mavros/local_position/velocity_local", 1, + "mavros/local_position/velocity_local", mavros_position_qos, std::bind(&TerrainPlanner::mavtwistCallback, this, _1)); global_origin_sub_ = this->create_subscription( - "mavros/global_position/gp_origin", 1, + "mavros/global_position/gp_origin", mavros_position_qos, std::bind(&TerrainPlanner::mavGlobalOriginCallback, this, _1)); image_captured_sub_ = this->create_subscription( - "mavros/camera/image_captured", 1, - std::bind(&TerrainPlanner::mavImageCapturedCallback, this, _1)); + "mavros/camera/image_captured", 1, std::bind(&TerrainPlanner::mavImageCapturedCallback, this, _1)); setlocation_serviceserver_ = this->create_service( - "/terrain_planner/set_location", - std::bind(&TerrainPlanner::setLocationCallback, this, _1, _2)); + "/terrain_planner/set_location", std::bind(&TerrainPlanner::setLocationCallback, this, _1, _2)); setmaxaltitude_serviceserver_ = this->create_service( - "/terrain_planner/set_max_altitude", - std::bind(&TerrainPlanner::setMaxAltitudeCallback, this, _1, _2)); + "/terrain_planner/set_max_altitude", std::bind(&TerrainPlanner::setMaxAltitudeCallback, this, _1, _2)); setgoal_serviceserver_ = this->create_service( - "/terrain_planner/set_goal", - std::bind(&TerrainPlanner::setGoalCallback, this, _1, _2)); + "/terrain_planner/set_goal", std::bind(&TerrainPlanner::setGoalCallback, this, _1, _2)); setstart_serviceserver_ = this->create_service( - "/terrain_planner/set_start", - std::bind(&TerrainPlanner::setStartCallback, this, _1, _2)); + "/terrain_planner/set_start", std::bind(&TerrainPlanner::setStartCallback, this, _1, _2)); setcurrentsegment_serviceserver_ = this->create_service( - "/terrain_planner/set_current_segment", - std::bind(&TerrainPlanner::setCurrentSegmentCallback, this, _1, _2)); + "/terrain_planner/set_current_segment", std::bind(&TerrainPlanner::setCurrentSegmentCallback, this, _1, _2)); setstartloiter_serviceserver_ = this->create_service( - "/terrain_planner/set_start_loiter", - std::bind(&TerrainPlanner::setStartLoiterCallback, this, _1, _2)); + "/terrain_planner/set_start_loiter", std::bind(&TerrainPlanner::setStartLoiterCallback, this, _1, _2)); setplannerstate_service_server_ = this->create_service( - "/terrain_planner/set_planner_state", - std::bind(&TerrainPlanner::setPlannerStateCallback, this, _1, _2)); - setplanning_serviceserver_ = this->create_service( - "/terrain_planner/trigger_planning", - std::bind(&TerrainPlanner::setPlanningCallback, this, _1, _2)); - updatepath_serviceserver_ = this->create_service( - "/terrain_planner/set_path", - std::bind(&TerrainPlanner::setPathCallback, this, _1, _2)); - - msginterval_serviceclient_ = this->create_client( - "mavros/cmd/command"); - - std::string avalanche_map_path; + "/terrain_planner/set_planner_state", std::bind(&TerrainPlanner::setPlannerStateCallback, this, _1, _2)); + setplanning_serviceserver_ = this->create_service( + "/terrain_planner/trigger_planning", std::bind(&TerrainPlanner::setPlanningCallback, this, _1, _2)); + updatepath_serviceserver_ = this->create_service( + "/terrain_planner/set_path", std::bind(&TerrainPlanner::setPathCallback, this, _1, _2)); - map_path_ = this->declare_parameter("terrain_path", "resources/cadastre.tif"); - map_color_path_ = this->declare_parameter("terrain_color_path", ""); - resource_path_ = this->declare_parameter("resource_path", "resources"); - mesh_resource_path_ = this->declare_parameter("meshresource_path", "../resources/believer.dae"); - avalanche_map_path = this->declare_parameter("avalanche_map_path", "../data/believer.dae"); - goal_radius_ = this->declare_parameter("minimum_turn_radius", 66.67); + msginterval_serviceclient_ = this->create_client("mavros/cmd/command"); + // create terrain map terrain_map_ = std::make_shared(); // Initialize Dubins state space @@ -167,6 +173,10 @@ TerrainPlanner::TerrainPlanner() global_planner_->setAltitudeLimits(max_elevation_, min_elevation_); planner_profiler_ = std::make_shared("planner"); + + // Initialise times - required to ensure all time sources are consistent + plan_time_ = this->get_clock()->now(); + last_triggered_time_ = this->get_clock()->now(); } void TerrainPlanner::init() { @@ -183,8 +193,7 @@ void TerrainPlanner::init() { // plannerloop_spinner_->start(); auto plannerloop_dt_ = 2s; - plannerloop_timer_ = this->create_wall_timer( - plannerloop_dt_, std::bind(&TerrainPlanner::plannerloopCallback, this)); + plannerloop_timer_ = this->create_wall_timer(plannerloop_dt_, std::bind(&TerrainPlanner::plannerloopCallback, this)); // plannerloop_executor_.add_node(plannerloop_node_); // plannerloop_executor_.spin(); @@ -197,8 +206,7 @@ void TerrainPlanner::init() { // statusloop_spinner_->start(); auto statusloop_dt_ = 500ms; - statusloop_timer_ = this->create_wall_timer( - statusloop_dt_, std::bind(&TerrainPlanner::statusloopCallback, this)); + statusloop_timer_ = this->create_wall_timer(statusloop_dt_, std::bind(&TerrainPlanner::statusloopCallback, this)); // statusloop_executor_.add_node(statusloop_node_); // statusloop_executor_.spin(); @@ -211,8 +219,7 @@ void TerrainPlanner::init() { // cmdloop_spinner_->start(); auto cmdloop_dt_ = 100ms; - cmdloop_timer_ = this->create_wall_timer( - cmdloop_dt_, std::bind(&TerrainPlanner::cmdloopCallback, this)); + cmdloop_timer_ = this->create_wall_timer(cmdloop_dt_, std::bind(&TerrainPlanner::cmdloopCallback, this)); // cmdloop_executor_.add_node(cmdloop_node_); // cmdloop_executor_.spin(); } @@ -287,12 +294,21 @@ void TerrainPlanner::cmdloopCallback() { curvature_reference); /// TODO: Switch mode to planner engaged - if (current_state_.mode == "OFFBOARD") { + //! @todo(srmainwaring) current_state_.mode == "GUIDED" for AP + if (current_state_.mode == "OFFBOARD" || current_state_.mode == "GUIDED") { publishPositionHistory(referencehistory_pub_, reference_position, referencehistory_vector_); tracking_error_ = reference_position - vehicle_position_; planner_enabled_ = true; - if ((this->get_clock()->now() - last_triggered_time_).nanoseconds() * 1.0E9 > 2.0 && planner_mode_ == PLANNER_MODE::ACTIVE_MAPPING) { + auto time_now = this->get_clock()->now(); + double time_since_trigger_s = (time_now - last_triggered_time_).seconds(); + + // RCLCPP_INFO_STREAM(this->get_logger(), "time_now: " << time_now.seconds()); + // RCLCPP_INFO_STREAM(this->get_logger(), "last_triggered_time: " << last_triggered_time_.seconds()); + + if (time_since_trigger_s > 2.0 && planner_mode_ == PLANNER_MODE::ACTIVE_MAPPING) { + RCLCPP_INFO_STREAM(this->get_logger(), "time_since_trigger: " << time_since_trigger_s); + bool dummy_camera = false; if (dummy_camera) { const int id = viewpoints_.size() + added_viewpoint_list.size(); @@ -312,21 +328,37 @@ void TerrainPlanner::cmdloopCallback() { image_capture_req->param7 = 0; // sequence number while (!msginterval_serviceclient_->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(this->get_logger(), "Service [" - << msginterval_serviceclient_->get_service_name() - << "] not available."); + RCLCPP_WARN_STREAM(this->get_logger(), + "Service [" << msginterval_serviceclient_->get_service_name() << "] not available."); return; } - auto result = msginterval_serviceclient_->async_send_request(image_capture_req); - - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" - << msginterval_serviceclient_->get_service_name() - << "] failed."); - return; + bool received_response = false; + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [&received_response](ServiceResponseFuture future) { + auto result = future.get(); + received_response = true; + }; + auto future = msginterval_serviceclient_->async_send_request(image_capture_req, response_received_callback); + + //! @todo wait for response_received_callback to set done + //! @todo add timeout + while (!received_response) { + ; } + + // return; + + // auto future = msginterval_serviceclient_->async_send_request(image_capture_req); + + // if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != + // rclcpp::FutureReturnCode::SUCCESS) + // { + // RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" + // << msginterval_serviceclient_->get_service_name() + // << "] failed."); + // return; + // } } /// TODO: Get reference attitude from path reference states const double pitch = std::atan(reference_tangent(2) / reference_tangent.head(2).norm()); @@ -372,8 +404,9 @@ void TerrainPlanner::statusloopCallback() { void TerrainPlanner::plannerloopCallback() { const std::lock_guard lock(goal_mutex_); if (local_origin_received_ && !map_initialized_) { + //! @todo(srmainwaring) consolidate duplicate code from here and TerrainPlanner::setLocationCallback std::cout << "[TerrainPlanner] Local origin received, loading map" << std::endl; - map_initialized_ = terrain_map_->Load(map_path_, true, map_color_path_); + map_initialized_ = terrain_map_->Load(map_path_, map_color_path_); terrain_map_->AddLayerDistanceTransform(min_elevation_, "distance_surface"); terrain_map_->AddLayerDistanceTransform(max_elevation_, "max_elevation"); terrain_map_->AddLayerHorizontalDistanceTransform(goal_radius_, "ics_+", "distance_surface"); @@ -402,22 +435,37 @@ void TerrainPlanner::plannerloopCallback() { request_global_origin_req->param1 = 49; while (!msginterval_serviceclient_->wait_for_service(1s)) { - RCLCPP_WARN_STREAM(this->get_logger(), "Service [" - << msginterval_serviceclient_->get_service_name() - << "] not available."); - return; + RCLCPP_WARN_STREAM(this->get_logger(), + "Service [" << msginterval_serviceclient_->get_service_name() << "] not available."); + return; } - auto result = msginterval_serviceclient_->async_send_request(request_global_origin_req); - - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" - << msginterval_serviceclient_->get_service_name() - << "] failed."); - return; + bool received_response = false; + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [&received_response](ServiceResponseFuture future) { + auto result = future.get(); + received_response = true; + }; + auto future = msginterval_serviceclient_->async_send_request(request_global_origin_req, response_received_callback); + + //! @todo wait for response_received_callback to set done + //! @todo add timeout + while (!received_response) { + ; } + return; + + // auto future = msginterval_serviceclient_->async_send_request(request_global_origin_req); + + // if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != + // rclcpp::FutureReturnCode::SUCCESS) + // { + // RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service [" + // << msginterval_serviceclient_->get_service_name() + // << "] failed."); + // } + // return; } // planner_profiler_->tic(); @@ -425,9 +473,17 @@ void TerrainPlanner::plannerloopCallback() { switch (planner_mode_) { case PLANNER_MODE::GLOBAL: { // Solve planning problem with RRT* - double time_spent_planning = rclcpp::Duration(this->get_clock()->now() - plan_time_).nanoseconds() * 1.0E9; - if (time_spent_planning < planner_time_budget_) { - //bool found_solution = + auto time_now = this->get_clock()->now(); + double time_spent_planning_s = (time_now - plan_time_).seconds(); + + // RCLCPP_INFO_STREAM(this->get_logger(), "plan_time: " << plan_time_.seconds()); + // RCLCPP_INFO_STREAM(this->get_logger(), "time_now: " << time_now.seconds()); + + if (time_spent_planning_s < planner_time_budget_) { + RCLCPP_INFO_STREAM(this->get_logger(), "time_spent_planning: " << time_spent_planning_s); + RCLCPP_INFO_STREAM(this->get_logger(), "planner_time_budget: " << planner_time_budget_); + + // bool found_solution = global_planner_->Solve(1.0, candidate_primitive_); publishTree(tree_pub_, global_planner_->getPlannerData(), global_planner_->getProblemSetup()); } else { @@ -508,10 +564,10 @@ void TerrainPlanner::plannerloopCallback() { } Eigen::Vector3d radial_vector = (end_position - rally_points[min_distance_index]); radial_vector(2) = 0.0; // Only consider horizontal loiters - //Eigen::Vector3d emergency_rates = - // 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); - //double horizon = 2 * M_PI / std::abs(emergency_rates(2)); - // Append a loiter at the end of the planned path + // Eigen::Vector3d emergency_rates = + // 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); + // double horizon = 2 * M_PI / std::abs(emergency_rates(2)); + // Append a loiter at the end of the planned path PathSegment loiter_trajectory; generateCircle(end_position, end_velocity, rally_points[min_distance_index], loiter_trajectory); updated_segment.appendSegment(loiter_trajectory); @@ -568,7 +624,7 @@ void TerrainPlanner::plannerloopCallback() { radial_vector(2) = 0.0; // Only consider horizontal loiters // Eigen::Vector3d emergency_rates = // 20.0 * end_velocity.normalized().cross(radial_vector.normalized()) / radial_vector.norm(); - //double horizon = 2 * M_PI / std::abs(emergency_rates(2)); + // double horizon = 2 * M_PI / std::abs(emergency_rates(2)); // Append a loiter at the end of the planned path PathSegment loiter_trajectory; generateCircle(end_position, end_velocity, home_position_, loiter_trajectory); @@ -581,7 +637,6 @@ void TerrainPlanner::plannerloopCallback() { } publishPathSegments(path_segment_pub_, candidate_primitive_); break; - } default: break; @@ -671,8 +726,8 @@ PLANNER_STATE TerrainPlanner::finiteStateMachine(const PLANNER_STATE current_sta planner_mode_ = PLANNER_MODE::ACTIVE_MAPPING; next_state = query_state; break; - default: - break; + default: + break; } } break; @@ -760,13 +815,17 @@ void TerrainPlanner::mavtwistCallback(const geometry_msgs::msg::TwistStamped &ms // mavRate_ = toEigen(msg.twist.angular); } -void TerrainPlanner::MapPublishOnce(rclcpp::Publisher::SharedPtr pub, const grid_map::GridMap &map) { +void TerrainPlanner::MapPublishOnce(rclcpp::Publisher::SharedPtr pub, + const grid_map::GridMap &map) { auto message = grid_map::GridMapRosConverter::toMessage(map); pub->publish(*message); } -void TerrainPlanner::publishPositionHistory(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, +void TerrainPlanner::publishPositionHistory(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, std::vector &history_vector) { + //! @todo(srmainwaring) provide an editor to allow this to be modified. + //! @todo(srmainwaring) make member variable and set default in header. unsigned int posehistory_window_ = 200; Eigen::Vector4d vehicle_attitude(1.0, 0.0, 0.0, 0.0); history_vector.insert(history_vector.begin(), vector3d2PoseStampedMsg(position, vehicle_attitude)); @@ -782,9 +841,9 @@ void TerrainPlanner::publishPositionHistory(rclcpp::Publisherpublish(msg); } -void TerrainPlanner::publishGlobalPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const double latitude, - const double longitude, const double altitude, - const Eigen::Vector3d &velocity, const double curvature) { +void TerrainPlanner::publishGlobalPositionSetpoints( + rclcpp::Publisher::SharedPtr pub, const double latitude, + const double longitude, const double altitude, const Eigen::Vector3d &velocity, const double curvature) { using namespace mavros_msgs; // Publishes position setpoints sequentially as trajectory setpoints mavros_msgs::msg::GlobalPositionTarget msg; @@ -807,8 +866,9 @@ void TerrainPlanner::publishGlobalPositionSetpoints(rclcpp::Publisherpublish(msg); } -void TerrainPlanner::publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity, const double curvature) { +void TerrainPlanner::publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, + const double curvature) { using namespace mavros_msgs; // Publishes position setpoints sequentially as trajectory setpoints mavros_msgs::msg::PositionTarget msg; @@ -831,14 +891,15 @@ void TerrainPlanner::publishPositionSetpoints(rclcpp::Publisherpublish(msg); } -void TerrainPlanner::publishVelocityMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity) { +void TerrainPlanner::publishVelocityMarker(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector3d &velocity) { visualization_msgs::msg::Marker marker = vector2ArrowsMsg(position, velocity, 0, Eigen::Vector3d(1.0, 0.0, 1.0)); pub->publish(marker); } -void TerrainPlanner::publishReferenceMarker(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, - const Eigen::Vector3d &velocity, const double /*curvature*/) { +void TerrainPlanner::publishReferenceMarker(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector3d &velocity, + const double /*curvature*/) { Eigen::Vector3d scaled_velocity = 20.0 * velocity; visualization_msgs::msg::Marker marker = vector2ArrowsMsg(position, scaled_velocity, 0, Eigen::Vector3d(0.0, 0.0, 1.0), "reference"); @@ -846,7 +907,8 @@ void TerrainPlanner::publishReferenceMarker(rclcpp::Publisherpublish(marker); } -void TerrainPlanner::publishPathSegments(rclcpp::Publisher::SharedPtr pub, Path &trajectory) { +void TerrainPlanner::publishPathSegments(rclcpp::Publisher::SharedPtr pub, + Path &trajectory) { visualization_msgs::msg::MarkerArray msg; std::vector marker; @@ -873,16 +935,18 @@ void TerrainPlanner::publishPathSegments(rclcpp::Publisherpublish(msg); } -void TerrainPlanner::publishGoal(rclcpp::Publisher::SharedPtr pub, const Eigen::Vector3d &position, const double radius, - Eigen::Vector3d color, std::string name_space) { +void TerrainPlanner::publishGoal(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const double radius, Eigen::Vector3d color, + std::string name_space) { visualization_msgs::msg::Marker marker; marker = getGoalMarker(1, position, radius, color); marker.ns = name_space; pub->publish(marker); } -void TerrainPlanner::publishRallyPoints(rclcpp::Publisher::SharedPtr pub, const std::vector &positions, - const double radius, Eigen::Vector3d color, std::string name_space) { +void TerrainPlanner::publishRallyPoints(rclcpp::Publisher::SharedPtr pub, + const std::vector &positions, const double radius, + Eigen::Vector3d color, std::string name_space) { visualization_msgs::msg::MarkerArray marker_array; std::vector markers; int marker_id = 1; @@ -898,7 +962,7 @@ void TerrainPlanner::publishRallyPoints(rclcpp::Publisherget_clock()->now(); @@ -930,61 +994,58 @@ visualization_msgs::msg::Marker TerrainPlanner::getGoalMarker(const int id, cons return marker; } -void TerrainPlanner::mavstateCallback(const mavros_msgs::msg::State &msg) { - current_state_ = msg; -} - -void TerrainPlanner::publishPathSetpoints(const Eigen::Vector3d &position, const Eigen::Vector3d &velocity) { - using namespace mavros_msgs; - // Publishes position setpoints sequentially as trajectory setpoints - mavros_msgs::msg::PositionTarget msg; - msg.header.stamp = this->get_clock()->now(); - msg.coordinate_frame = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED; - msg.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_AFX | mavros_msgs::msg::PositionTarget::IGNORE_AFY | mavros_msgs::msg::PositionTarget::IGNORE_AFZ; - msg.position.x = position(0); - msg.position.y = position(1); - msg.position.z = position(2); - msg.velocity.x = velocity(0); - msg.velocity.y = velocity(1); - msg.velocity.z = velocity(2); - - /// TODO: Package trajectory segments into trajectory waypoints - - mavros_msgs::msg::Trajectory trajectory_msg; - trajectory_msg.header.stamp = this->get_clock()->now(); - trajectory_msg.type = mavros_msgs::msg::Trajectory::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - trajectory_msg.point_valid[0] = true; - trajectory_msg.point_valid[1] = true; - trajectory_msg.point_1 = msg; - trajectory_msg.point_2 = msg; - trajectory_msg.point_3 = msg; - trajectory_msg.point_4 = msg; - trajectory_msg.point_5 = msg; - - path_target_pub_->publish(trajectory_msg); -} - +void TerrainPlanner::mavstateCallback(const mavros_msgs::msg::State &msg) { current_state_ = msg; } + +// Notes on the conversions used in `mavGlobalOriginCallback` +// +// FCU emits mavlink GPS_GLOBAL_ORIGIN (#49 ) which is a geodetic coordinate +// with the altitude defined relative to the geoid (AMSL). +// +// mavros plugin: global_position +// - converts geodetic GPS_GLOBAL_ORIGIN LLA from geoid to ellipsoid datum +// - converts geodetic LLA to geocentric LLA (!?) +// - then publishes geocentic LLA as geographic_msgs/msg/GeoPointStamped - which is +// very clearly documented as geodetic coordinate +// +// # http://docs.ros.org/en/jade/api/geographic_msgs/html/msg/GeoPoint.html +// # Geographic point, using the WGS 84 reference ellipsoid. +// void TerrainPlanner::mavGlobalOriginCallback(const geographic_msgs::msg::GeoPointStamped &msg) { std::cout << "[TerrainPlanner] Received Global Origin from FMU" << std::endl; local_origin_received_ = true; - double X = static_cast(msg.position.latitude); - double Y = static_cast(msg.position.longitude); - double Z = static_cast(msg.position.altitude); + // receive geocentric LLA coordinate from mavros/global_position/gp_origin + double geocentric_lat = static_cast(msg.position.latitude); + double geocentric_lon = static_cast(msg.position.longitude); + double geocentric_alt = static_cast(msg.position.altitude); + + // convert to geodetic coordinates with WGS-84 ellipsoid as datum GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); - double lat, lon, alt; - earth.Reverse(X, Y, Z, lat, lon, alt); - enu_.emplace(lat, lon, alt, GeographicLib::Geocentric::WGS84()); - local_origin_altitude_ = alt; - local_origin_latitude_ = lat; - local_origin_longitude_ = lon; + double geodetic_lat, geodetic_lon, geodetic_alt; + earth.Reverse(geocentric_lat, geocentric_lon, geocentric_alt, geodetic_lat, geodetic_lon, geodetic_alt); + + // create local cartesian coordinates (ENU) + enu_.emplace(geodetic_lat, geodetic_lon, geodetic_alt, GeographicLib::Geocentric::WGS84()); + + // store the geodetic coordinates (why is this called local origin?) + local_origin_altitude_ = geodetic_alt; + local_origin_latitude_ = geodetic_lat; + local_origin_longitude_ = geodetic_lon; + + std::cout << "Global Origin" << std::endl; + std::cout << "lat: " << local_origin_latitude_ << std::endl; + std::cout << "lon: " << local_origin_longitude_ << std::endl; + std::cout << "alt: " << local_origin_altitude_ << std::endl; } void TerrainPlanner::mavMissionCallback(const mavros_msgs::msg::WaypointList &msg) { for (auto &waypoint : msg.waypoints) { if (waypoint.is_current) { - if (waypoint.command == mavros_msgs::msg::CommandCode::NAV_LOITER_UNLIM) { + if (waypoint.command == mavros_msgs::msg::CommandCode::NAV_LOITER_UNLIM || + waypoint.command == mavros_msgs::msg::CommandCode::NAV_LOITER_TURNS || + waypoint.command == mavros_msgs::msg::CommandCode::NAV_LOITER_TIME || + waypoint.command == mavros_msgs::msg::CommandCode::NAV_LOITER_TO_ALT) { // Get Loiter position std::cout << "NAV Loiter Center" << std::endl; std::cout << " - x_lat : " << waypoint.x_lat << std::endl; @@ -1014,7 +1075,7 @@ void TerrainPlanner::mavMissionCallback(const mavros_msgs::msg::WaypointList &ms } } -void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured &/*msg*/) { +void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::msg::CameraImageCaptured & /*msg*/) { // Publish recorded viewpoints /// TODO: Transform image tag into local position int id = viewpoints_.size(); @@ -1023,18 +1084,22 @@ void TerrainPlanner::mavImageCapturedCallback(const mavros_msgs::msg::CameraImag // publishViewpoints(viewpoint_pub_, viewpoints_); } -bool TerrainPlanner::setLocationCallback( - const std::shared_ptr req, - std::shared_ptr res) { +bool TerrainPlanner::setLocationCallback(const std::shared_ptr req, + std::shared_ptr res) { + //! @todo(srmainwaring) consolidate duplicate code from here and TerrainPlanner::plannerloopCallback std::string set_location = req->string; - bool align_location = req->align; + //! @todo(srmainwaring) interface supporting align_location has been removed, + //! decide how to treat (and see below L1112) + // bool align_location = req->align; std::cout << "[TerrainPlanner] Set Location: " << set_location << std::endl; - std::cout << "[TerrainPlanner] Set Alignment: " << align_location << std::endl; + // std::cout << "[TerrainPlanner] Set Alignment: " << align_location << std::endl; /// TODO: Add location from the new set location service map_path_ = resource_path_ + "/" + set_location + ".tif"; map_color_path_ = resource_path_ + "/" + set_location + "_color.tif"; - bool result = terrain_map_->Load(map_path_, align_location, map_color_path_); + bool result = terrain_map_->Load(map_path_, map_color_path_); + + //! @todo(srmainwaring) check result valid before further operations? std::cout << "[TerrainPlanner] Computing distance transforms" << std::endl; terrain_map_->AddLayerDistanceTransform(min_elevation_, "distance_surface"); terrain_map_->AddLayerDistanceTransform(max_elevation_, "max_elevation"); @@ -1044,17 +1109,17 @@ bool TerrainPlanner::setLocationCallback( std::cout << "[TerrainPlanner] Computing safety layers" << std::endl; terrain_map_->addLayerSafety("safety", "ics_+", "ics_-"); - if (!align_location) { - // Depending on Gdal versions, lon lat order are reversed - Eigen::Vector3d lv03_local_origin; - GeoConversions::forward(local_origin_latitude_, local_origin_longitude_, local_origin_altitude_, - lv03_local_origin.x(), lv03_local_origin.y(), lv03_local_origin.z()); - if (terrain_map_->getGridMap().isInside(Eigen::Vector2d(0.0, 0.0))) { - double terrain_altitude = terrain_map_->getGridMap().atPosition("elevation", Eigen::Vector2d(0.0, 0.0)); - lv03_local_origin(2) = lv03_local_origin(2) - terrain_altitude; - } - terrain_map_->setGlobalOrigin(ESPG::CH1903_LV03, lv03_local_origin); - } + // if (!align_location) { + // // Depending on Gdal versions, lon lat order are reversed + // Eigen::Vector3d lv03_local_origin; + // GeoConversions::forward(local_origin_latitude_, local_origin_longitude_, local_origin_altitude_, + // lv03_local_origin.x(), lv03_local_origin.y(), lv03_local_origin.z()); + // if (terrain_map_->getGridMap().isInside(Eigen::Vector2d(0.0, 0.0))) { + // double terrain_altitude = terrain_map_->getGridMap().atPosition("elevation", Eigen::Vector2d(0.0, 0.0)); + // lv03_local_origin(2) = lv03_local_origin(2) - terrain_altitude; + // } + // terrain_map_->setGlobalOrigin(ESPG::CH1903_LV03, lv03_local_origin); + // } if (result) { global_planner_->setBoundsFromMap(terrain_map_->getGridMap()); global_planner_->setupProblem(start_pos_, goal_pos_, start_loiter_radius_); @@ -1066,9 +1131,8 @@ bool TerrainPlanner::setLocationCallback( return true; } -bool TerrainPlanner::setMaxAltitudeCallback( - const std::shared_ptr req, - std::shared_ptr res) { +bool TerrainPlanner::setMaxAltitudeCallback(const std::shared_ptr req, + std::shared_ptr res) { bool check_max_altitude = req->align; std::cout << "[TerrainPlanner] Max altitude constraint configured: " << check_max_altitude << std::endl; global_planner_->setMaxAltitudeCollisionChecks(check_max_altitude); @@ -1076,9 +1140,8 @@ bool TerrainPlanner::setMaxAltitudeCallback( return true; } -bool TerrainPlanner::setGoalCallback( - const std::shared_ptr req, - std::shared_ptr res) { +bool TerrainPlanner::setGoalCallback(const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); Eigen::Vector3d candidate_goal = Eigen::Vector3d(req->vector.x, req->vector.y, req->vector.z); Eigen::Vector3d new_goal; @@ -1098,9 +1161,8 @@ bool TerrainPlanner::setGoalCallback( } } -bool TerrainPlanner::setStartCallback( - const std::shared_ptr req, - std::shared_ptr res) { +bool TerrainPlanner::setStartCallback(const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); Eigen::Vector3d candidate_start = Eigen::Vector3d(req->vector.x, req->vector.y, req->vector.z); Eigen::Vector3d new_start; @@ -1118,9 +1180,8 @@ bool TerrainPlanner::setStartCallback( } } -bool TerrainPlanner::setCurrentSegmentCallback( - const std::shared_ptr /*req*/, - std::shared_ptr res) { +bool TerrainPlanner::setCurrentSegmentCallback(const std::shared_ptr /*req*/, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); /// TODO: Get center of the last segment of the reference path if (!reference_primitive_.segments.empty()) { @@ -1155,9 +1216,8 @@ bool TerrainPlanner::setCurrentSegmentCallback( return true; } -bool TerrainPlanner::setStartLoiterCallback( - const std::shared_ptr /*req*/, - std::shared_ptr res) { +bool TerrainPlanner::setStartLoiterCallback(const std::shared_ptr /*req*/, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); std::cout << "[TerrainPlanner] Current Loiter start: " << mission_loiter_center_.transpose() << std::endl; Eigen::Vector3d new_start; @@ -1179,23 +1239,26 @@ bool TerrainPlanner::setStartLoiterCallback( } } -bool TerrainPlanner::setPlanningCallback( - const std::shared_ptr req, - std::shared_ptr res) { +bool TerrainPlanner::setPlanningCallback(const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); planner_time_budget_ = req->vector.z; - std::cout << "[TerrainPlanner] Planning budget: " << planner_time_budget_ << std::endl; problem_updated_ = true; plan_time_ = this->get_clock()->now(); + + std::cout << "[TerrainPlanner] Planning budget: " << planner_time_budget_ << std::endl; + std::cout << "[TerrainPlanner] Start position: " << start_pos_.transpose() << std::endl; + std::cout << "[TerrainPlanner] Goal position: " << goal_pos_.transpose() << std::endl; + std::cout << "[TerrainPlanner] Loiter radius: " << start_loiter_radius_ << std::endl; + global_planner_->setupProblem(start_pos_, goal_pos_, start_loiter_radius_); planner_mode_ = PLANNER_MODE::GLOBAL; res->success = true; return true; } -bool TerrainPlanner::setPathCallback( - const std::shared_ptr /*req*/, - std::shared_ptr res) { +bool TerrainPlanner::setPathCallback(const std::shared_ptr /*req*/, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); if (!candidate_primitive_.segments.empty()) { @@ -1207,9 +1270,8 @@ bool TerrainPlanner::setPathCallback( return true; } -bool TerrainPlanner::setPlannerStateCallback( - const std::shared_ptr req, - std::shared_ptr res) { +bool TerrainPlanner::setPlannerStateCallback(const std::shared_ptr req, + std::shared_ptr res) { const std::lock_guard lock(goal_mutex_); int planner_state = static_cast(req->state); switch (planner_state) { diff --git a/terrain_navigation_ros/src/terrain_planner_node.cpp b/terrain_navigation_ros/src/terrain_planner_node.cpp index b709a0b1..b4c21a80 100644 --- a/terrain_navigation_ros/src/terrain_planner_node.cpp +++ b/terrain_navigation_ros/src/terrain_planner_node.cpp @@ -58,5 +58,4 @@ int main(int argc, char **argv) { rclcpp::spin(terrain_planner_node); rclcpp::shutdown(); return 0; - } diff --git a/terrain_navigation_ros/src/visualization.cpp b/terrain_navigation_ros/src/visualization.cpp new file mode 100644 index 00000000..0ceddfed --- /dev/null +++ b/terrain_navigation_ros/src/visualization.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Jaeyoung Lim. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "terrain_navigation_ros/visualization.h" + +#include + +geometry_msgs::msg::Point toPoint(const Eigen::Vector3d &p) { + geometry_msgs::msg::Point position; + position.x = p(0); + position.y = p(1); + position.z = p(2); + return position; +} + +void publishVehiclePose(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector4d &attitude, + std::string mesh_resource_path) { + Eigen::Vector4d mesh_attitude = + quatMultiplication(attitude, Eigen::Vector4d(std::cos(M_PI / 2), 0.0, 0.0, std::sin(M_PI / 2))); + geometry_msgs::msg::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude); + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Clock().now(); + marker.header.frame_id = "map"; + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + marker.ns = "my_namespace"; + //! @todo(srmainwaring) understand why the mesh is not displayed. + //! @note https://answers.ros.org/question/282745/rviz-doesnt-load-dae-mesh-cannot-locate-it/ + // marker.mesh_resource = "package://terrain_planner/" + mesh_resource_path; + marker.mesh_resource = "file://" + mesh_resource_path; + marker.scale.x = 10.0; + marker.scale.y = 10.0; + marker.scale.z = 10.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + marker.pose = vehicle_pose; + pub->publish(marker); +} + +visualization_msgs::msg::Marker Viewpoint2MarkerMsg(int id, ViewPoint &viewpoint, Eigen::Vector3d color) { + double scale{15}; // Size of the viewpoint markers + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = "my_namespace"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + const Eigen::Vector3d position = viewpoint.getCenterLocal(); + std::vector points; + std::vector corner_ray_vectors = viewpoint.getCornerRayVectors(); + std::vector vertex; + for (auto &corner_ray : corner_ray_vectors) { + vertex.push_back(position + scale * corner_ray); + } + + for (size_t i = 0; i < vertex.size(); i++) { + points.push_back(toPoint(position)); // Viewpoint center + points.push_back(toPoint(vertex[i])); + points.push_back(toPoint(vertex[i])); + points.push_back(toPoint(vertex[(i + 1) % vertex.size()])); + } + + marker.points = points; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + return marker; +} + +void publishCameraView(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d &position, const Eigen::Vector4d &attitude) { + visualization_msgs::msg::Marker marker; + ViewPoint viewpoint(-1, position, attitude); + marker = Viewpoint2MarkerMsg(viewpoint.getIndex(), viewpoint); + pub->publish(marker); +} + +void publishViewpoints(rclcpp::Publisher::SharedPtr pub, + std::vector &viewpoint_vector, Eigen::Vector3d color) { + visualization_msgs::msg::MarkerArray msg; + + std::vector marker; + visualization_msgs::msg::Marker mark; + mark.action = visualization_msgs::msg::Marker::DELETEALL; + marker.push_back(mark); + msg.markers = marker; + pub->publish(msg); + + std::vector viewpoint_marker_vector; + int i = 0; + for (auto viewpoint : viewpoint_vector) { + viewpoint_marker_vector.insert(viewpoint_marker_vector.begin(), Viewpoint2MarkerMsg(i, viewpoint, color)); + i++; + } + msg.markers = viewpoint_marker_vector; + pub->publish(msg); +} diff --git a/terrain_planner/include/terrain_planner/DubinsAirplane.hpp b/terrain_planner/include/terrain_planner/DubinsAirplane.hpp index 1a9a94cb..27bbae0e 100644 --- a/terrain_planner/include/terrain_planner/DubinsAirplane.hpp +++ b/terrain_planner/include/terrain_planner/DubinsAirplane.hpp @@ -13,17 +13,17 @@ #ifndef FW_PLANNING_PLANNING__SPACES__DUBINS_AIRPLANE_HPP_ #define FW_PLANNING_PLANNING__SPACES__DUBINS_AIRPLANE_HPP_ +#include +#include +#include + +#include #include #include #include #include #include -#include -#include -#include -#include - #include "terrain_planner/DubinsPath.hpp" namespace ob = ompl::base; diff --git a/terrain_planner/include/terrain_planner/common.h b/terrain_planner/include/terrain_planner/common.h index b19a37d3..3bd5c5bd 100644 --- a/terrain_planner/include/terrain_planner/common.h +++ b/terrain_planner/include/terrain_planner/common.h @@ -35,22 +35,19 @@ #ifndef COMMON_H #define COMMON_H -#include "terrain_navigation/path.h" - #include #include - #include #include #include +#include #include +#include +#include #include #include -#include -#include -#include - +#include "terrain_navigation/path.h" const std::map>> colorMap{ {"viridis", {{0.267004, 0.004874, 0.329415}, {0.268510, 0.009605, 0.335427}, {0.269944, 0.014625, 0.341379}, @@ -408,30 +405,31 @@ geometry_msgs::msg::Vector3 toVector3(const Eigen::Vector3d &p); geometry_msgs::msg::Pose vector3d2PoseMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation); -geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation); +geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, + const Eigen::Vector4d orientation); visualization_msgs::msg::Marker utility2MarkerMsg(const double utility, const Eigen::Vector3d position, int id); visualization_msgs::msg::Marker vector2ArrowsMsg(const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0), - const std::string marker_namespace = "normals"); + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0), + const std::string marker_namespace = "normals"); visualization_msgs::msg::Marker trajectory2MarkerMsg(Path &trajectory, const int id); visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, - std::string color_map = "plasma"); + std::string color_map = "plasma"); visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, - std::vector &trajectory_color); + std::vector &trajectory_color); visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); visualization_msgs::msg::Marker trajectory2MarkerMsg(PathSegment &trajectory, const int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); visualization_msgs::msg::Marker point2MarkerMsg(Eigen::Vector3d &position, const int id, - Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)); double GetTimeInSeconds(std::string date_time); diff --git a/terrain_planner/include/terrain_planner/ompl_setup.h b/terrain_planner/include/terrain_planner/ompl_setup.h index 6f996a8b..67fe65f3 100644 --- a/terrain_planner/include/terrain_planner/ompl_setup.h +++ b/terrain_planner/include/terrain_planner/ompl_setup.h @@ -1,11 +1,7 @@ #ifndef TERRAIN_PLANNER_OMPL_SETUP_H_ #define TERRAIN_PLANNER_OMPL_SETUP_H_ -#include "terrain_planner/DubinsAirplane.hpp" -#include "terrain_planner/terrain_ompl.h" - #include - #include #include #include @@ -13,7 +9,10 @@ #include #include #include + #include "ompl/base/SpaceInformation.h" +#include "terrain_planner/DubinsAirplane.hpp" +#include "terrain_planner/terrain_ompl.h" enum PlannerType { RRTSTAR, INFORMED_RRTSTAR, RRTCONNECT, BITSTAR, FMTSTAR }; diff --git a/terrain_planner/include/terrain_planner/planner.h b/terrain_planner/include/terrain_planner/planner.h index d70dba94..cca78825 100644 --- a/terrain_planner/include/terrain_planner/planner.h +++ b/terrain_planner/include/terrain_planner/planner.h @@ -1,11 +1,11 @@ #ifndef TERRAIN_PLANNER_PLANNER_H #define TERRAIN_PLANNER_PLANNER_H -#include - #include #include +#include + /** * @brief Planner base class * diff --git a/terrain_planner/include/terrain_planner/terrain_ompl.h b/terrain_planner/include/terrain_planner/terrain_ompl.h index f4bc7b26..663fc05d 100644 --- a/terrain_planner/include/terrain_planner/terrain_ompl.h +++ b/terrain_planner/include/terrain_planner/terrain_ompl.h @@ -4,10 +4,10 @@ #include #include -#include "terrain_planner/DubinsAirplane.hpp" - #include +#include "terrain_planner/DubinsAirplane.hpp" + namespace ompl { class TerrainValidityChecker : public base::StateValidityChecker { diff --git a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h index ab3965de..2f386aee 100644 --- a/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h +++ b/terrain_planner/include/terrain_planner/terrain_ompl_rrt.h @@ -35,20 +35,18 @@ #ifndef TERRAIN_PLANNER_TERRAIN_OMPL_RRT_H #define TERRAIN_PLANNER_TERRAIN_OMPL_RRT_H +#include #include +#include + +#include #include #include #include -#include - -#include #include "terrain_navigation/path.h" - #include "terrain_planner/ompl_setup.h" -#include - class TerrainOmplRrt { public: TerrainOmplRrt(); diff --git a/terrain_planner/include/terrain_planner/visualization.h b/terrain_planner/include/terrain_planner/visualization.h index ae1d3180..330d7a86 100644 --- a/terrain_planner/include/terrain_planner/visualization.h +++ b/terrain_planner/include/terrain_planner/visualization.h @@ -1,52 +1,39 @@ #ifndef TERRAIN_PLANNER_VISUALIZATION_H #define TERRAIN_PLANNER_VISUALIZATION_H -#include +#include +#include #include - +#include #include #include #include #include - -#include +#include +#include #include #include #include #include -#include -#include - -#include - #include "terrain_planner/common.h" #include "terrain_planner/ompl_setup.h" -void publishCandidateManeuvers( - rclcpp::Publisher::SharedPtr pub, - const std::vector& candidate_maneuvers, - bool visualize_invalid_trajectories = false); - -void publishPositionSetpoints( - rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d& position, - const Eigen::Vector3d& velocity, - const Eigen::Vector3d scale = Eigen::Vector3d(10.0, 2.0, 2.0)); - -void publishPath( - rclcpp::Publisher::SharedPtr pub, - std::vector path, - Eigen::Vector3d color); - -void publishTrajectory( - rclcpp::Publisher::SharedPtr pub, - std::vector trajectory); - -void publishTree( - rclcpp::Publisher::SharedPtr pub, - std::shared_ptr planner_data, - std::shared_ptr problem_setup); +void publishCandidateManeuvers(rclcpp::Publisher::SharedPtr pub, + const std::vector& candidate_maneuvers, + bool visualize_invalid_trajectories = false); + +void publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, + const Eigen::Vector3d scale = Eigen::Vector3d(10.0, 2.0, 2.0)); + +void publishPath(rclcpp::Publisher::SharedPtr pub, std::vector path, + Eigen::Vector3d color); + +void publishTrajectory(rclcpp::Publisher::SharedPtr pub, std::vector trajectory); + +void publishTree(rclcpp::Publisher::SharedPtr pub, + std::shared_ptr planner_data, std::shared_ptr problem_setup); #endif diff --git a/terrain_planner/launch/config.rviz b/terrain_planner/launch/config.rviz index b0debb13..1d34f1bf 100644 --- a/terrain_planner/launch/config.rviz +++ b/terrain_planner/launch/config.rviz @@ -1,24 +1,21 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - - /Grid1 - - /GridMap1 - - /MarkerArray3 - - /MarkerArray4 - - /Marker5 - - /GridMap4 + - /Planning1 + - /Vehicle1 + - /Vehicle1/VehiclePoseMarker1/Namespaces1 Splitter Ratio: 0.5 - Tree Height: 531 + Tree Height: 285 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Pose Estimate1 - - /2D Nav Goal1 - /Publish Point1 + - /Interact1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views @@ -27,6 +24,7 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz_common/Time + Experimental: false Name: Time SyncMode: 0 SyncSource: "" @@ -37,12 +35,8 @@ Panels: Name: PlanningPanel namespace: "" odometry_topic: "" - planner_name: sertig - planning_budget: 4 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 + planner_name: davosdorf + planning_budget: 10 Visualization Manager: Class: "" Displays: @@ -64,345 +58,436 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 255; 255 + - Class: rviz_default_plugins/Axes Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /vehicle_path - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /geometric_controller/path - Unreliable: false - Value: true - - Alpha: 0.8999999761581421 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 200; 200; 200 - Color Layer: color - Color Transformer: ColorLayer - ColorMap: default - Enabled: true - Grid Cell Decimation: 1 - Grid Line Thickness: 0.10000000149011612 - Height Layer: elevation - Height Transformer: GridMapLayer - History Length: 1 - Invert ColorMap: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: GridMap - Show Grid Lines: false - Topic: /grid_map - Unreliable: false - Use ColorMap: true - Value: true - - Alpha: 0.5 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 85; 170; 255 - Color Layer: elevation - Color Transformer: FlatColor - ColorMap: default - Enabled: true - Grid Cell Decimation: 1 - Grid Line Thickness: 0.10000000149011612 - Height Layer: safety - Height Transformer: GridMapLayer - History Length: 1 - Invert ColorMap: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: GridMap - Show Grid Lines: false - Topic: /grid_map - Unreliable: false - Use ColorMap: true - Value: true - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 255; 0; 0 - Color Layer: elevation - Color Transformer: FlatColor - ColorMap: default - Enabled: false - Grid Cell Decimation: 1 - Grid Line Thickness: 0.10000000149011612 - Height Layer: max_elevation - Height Transformer: Layer - History Length: 1 - Invert ColorMap: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: GridMap - Show Grid Lines: false - Topic: /grid_map - Unreliable: false - Use ColorMap: true - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Axes - Enabled: false Length: 100 - Name: Axes + Name: MapAxes Radius: 10 Reference Frame: map - Show Trail: false - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Marker Topic: /visualization_marker - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /position_target - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /vehicle_pose_marker - Name: Marker - Namespaces: - my_namespace: true - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: InteractiveMarkers - Show Axes: true - Show Descriptions: true - Show Visual Aids: true - Update Topic: /goal/update - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /arc_center - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /goal_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /reference/path - Unreliable: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Marker Topic: /viewpoints - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Marker Topic: /tree - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz_default_plugins/MarkerArray + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: ColorLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ElevationGridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map + Use Rainbow: true + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 85; 170; 255 + Color Layer: elevation + Color Transformer: FlatColor + Enabled: false + Height Layer: safety + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: SafetyGridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map + Use Rainbow: true + Value: false + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 255; 0; 0 + Color Layer: elevation + Color Transformer: FlatColor + Enabled: false + Height Layer: max_elevation + Height Transformer: Layer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MaxElevationGridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grid_map + Use Rainbow: true + Value: false + - Alpha: 0.8999999761581421 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: false + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: AvalancheGridMap + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /avalanche_map + Use Rainbow: true + Value: false Enabled: true - Marker Topic: /path_segments - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /candidate_start_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /candidate_goal_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Marker Topic: /vehicle_velocity - Name: Marker - Namespaces: - normals: true - Queue Size: 100 - Value: true - - Alpha: 0.8999999761581421 - Autocompute Intensity Bounds: true - Class: grid_map_rviz_plugin/GridMap - Color: 200; 200; 200 - Color Layer: elevation - Color Transformer: GridMapLayer - ColorMap: default - Enabled: true - Grid Cell Decimation: 1 - Grid Line Thickness: 0.10000000149011612 - Height Layer: elevation - Height Transformer: GridMapLayer - History Length: 1 - Invert ColorMap: false - Max Color: 255; 255; 255 - Max Intensity: 10 - Min Color: 0; 0; 0 - Min Intensity: 0 Name: GridMap - Show Grid Lines: false - Topic: /avalanche_map - Unreliable: false - Use ColorMap: true - Value: true - - Class: rviz_default_plugins/MarkerArray + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: CandidateGoalMarker + Namespaces: + goal: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /candidate_goal_marker + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: CandidateStartMarker + Namespaces: + start: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /candidate_start_marker + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: GoalMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_marker + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: PositionTargetMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /position_target + Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /goal + Name: GoalMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: GoalMarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PathSegmentsMarkerArray + Namespaces: + normals: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path_segments + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RallyPointsMarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rallypoints_marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TreeMarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /tree + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GeometricControllerPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /geometric_controller/path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: ReferencePath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /reference/path + Value: true Enabled: true - Marker Topic: /planned_viewpoints - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: VehiclePoseMarker + Namespaces: + my_namespace: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle_pose_marker + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: VehicleVelocityMarker + Namespaces: + normals: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle_velocity + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VehicleVelocityMarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle_velocity_array + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: VehiclePath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle_path + Value: true + Enabled: true + Name: Vehicle Enabled: true Global Options: Background Color: 255; 255; 255 - Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose - Class: rviz_default_plugins/SetGoal - Topic: /move_base_simple/goal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3920.333984375 + Distance: 4404.001953125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: 55.76494216918945 - Y: 2079.845703125 - Z: 315.5940856933594 + X: -1531.7305908203125 + Y: -52.1274299621582 + Z: 1152.6519775390625 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.49479860067367554 + Pitch: 0.25479769706726074 Target Frame: map - Yaw: 4.263692378997803 + Value: Orbit (rviz_default_plugins) + Yaw: 6.203699588775635 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1163 + Height: 857 Hide Left Dock: false Hide Right Dock: true PlanningPanel: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000247000003f6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000007a0000029c000000c700fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c010000031c000001540000014e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000004500ffffff000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000078000000039fc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d0065010000000000000450000000000000000000000533000003f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002ad000002dafc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000640000016c000000e300fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c01000001d10000016d0000016d00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000007a00ffffff000000010000010f00000317fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007c00000317000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000052b00000037fc0100000002fb0000000800540069006d006501000000000000052b0000023d00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d006501000000000000045000000000000000000000027d000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Teleop: @@ -413,6 +498,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 1920 - Y: 0 + Width: 1323 + X: 0 + Y: 25 diff --git a/terrain_planner/src/DubinsAirplane.cpp b/terrain_planner/src/DubinsAirplane.cpp index 8ed53962..a5c2e341 100644 --- a/terrain_planner/src/DubinsAirplane.cpp +++ b/terrain_planner/src/DubinsAirplane.cpp @@ -12,17 +12,17 @@ #include "terrain_planner/DubinsAirplane.hpp" #include +#include +#include +#include + +#include #include #include #include #include #include -#include -#include -#include -#include - namespace ob = ompl::base; namespace fw_planning { diff --git a/terrain_planner/src/DubinsPath.cpp b/terrain_planner/src/DubinsPath.cpp index 658534fa..5c8f36f4 100644 --- a/terrain_planner/src/DubinsPath.cpp +++ b/terrain_planner/src/DubinsPath.cpp @@ -11,9 +11,9 @@ #include "terrain_planner/DubinsPath.hpp" #include -#include #include +#include namespace fw_planning { @@ -27,8 +27,7 @@ const DubinsPath::DubinsPathSegmentType DubinsPath::dubinsPathType[6][3] = { {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT}, {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}}; DubinsPath::DubinsPath(Index type, double t, double p, double q, double gam, unsigned int ks, unsigned int ke, double r) - : - length_{{0, t, 0, p, q, 0}}, + : length_{{0, t, 0, p, q, 0}}, length_2D_(t + p + q), radiusRatio_{{r, r, r, r, r, r}}, radiusRatioInverse_{{1.0 / r, 1.0 / r, 1.0 / r, 1.0 / r, 1.0 / r, 1.0 / r}}, diff --git a/terrain_planner/src/common.cpp b/terrain_planner/src/common.cpp index 8f58a8a8..35a91a1c 100644 --- a/terrain_planner/src/common.cpp +++ b/terrain_planner/src/common.cpp @@ -402,8 +402,7 @@ geometry_msgs::msg::Vector3 toVector3(const Eigen::Vector3d &p) { return vector; } -geometry_msgs::msg::Pose vector3d2PoseMsg(const Eigen::Vector3d position, - const Eigen::Vector4d orientation) { +geometry_msgs::msg::Pose vector3d2PoseMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation) { geometry_msgs::msg::Pose encode_msg; encode_msg.orientation.w = orientation(0); @@ -416,8 +415,8 @@ geometry_msgs::msg::Pose vector3d2PoseMsg(const Eigen::Vector3d position, return encode_msg; } -geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg( - const Eigen::Vector3d position, const Eigen::Vector4d orientation) { +geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, + const Eigen::Vector4d orientation) { geometry_msgs::msg::PoseStamped encode_msg; encode_msg.header.stamp = rclcpp::Clock().now(); @@ -432,8 +431,7 @@ geometry_msgs::msg::PoseStamped vector3d2PoseStampedMsg( return encode_msg; } -visualization_msgs::msg::Marker utility2MarkerMsg(const double utility, - const Eigen::Vector3d position, int id) { +visualization_msgs::msg::Marker utility2MarkerMsg(const double utility, const Eigen::Vector3d position, int id) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -464,9 +462,8 @@ visualization_msgs::msg::Marker utility2MarkerMsg(const double utility, return marker; } -visualization_msgs::msg::Marker vector2ArrowsMsg( - const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, - Eigen::Vector3d color, const std::string marker_namespace) { +visualization_msgs::msg::Marker vector2ArrowsMsg(const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, + Eigen::Vector3d color, const std::string marker_namespace) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -497,8 +494,7 @@ visualization_msgs::msg::Marker vector2ArrowsMsg( return marker; } -visualization_msgs::msg::Marker trajectory2MarkerMsg(Path &trajectory, - const int id) { +visualization_msgs::msg::Marker trajectory2MarkerMsg(Path &trajectory, const int id) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -535,9 +531,8 @@ visualization_msgs::msg::Marker trajectory2MarkerMsg(Path &trajectory, return marker; } -visualization_msgs::msg::Marker trajectory2MarkerMsg( - std::vector &trajectory, const int id, - std::string color_map) { +visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, + std::string color_map) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -596,9 +591,8 @@ visualization_msgs::msg::Marker trajectory2MarkerMsg( return marker; } -visualization_msgs::msg::Marker trajectory2MarkerMsg( - std::vector &trajectory, const int id, - std::vector &trajectory_color) { +visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, + std::vector &trajectory_color) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -644,9 +638,8 @@ visualization_msgs::msg::Marker trajectory2MarkerMsg( return marker; } -visualization_msgs::msg::Marker trajectory2MarkerMsg( - std::vector &trajectory, const int id, - Eigen::Vector3d color) { +visualization_msgs::msg::Marker trajectory2MarkerMsg(std::vector &trajectory, const int id, + Eigen::Vector3d color) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -678,9 +671,7 @@ visualization_msgs::msg::Marker trajectory2MarkerMsg( return marker; } -visualization_msgs::msg::Marker trajectory2MarkerMsg( - PathSegment &trajectory, const int id, - Eigen::Vector3d color) { +visualization_msgs::msg::Marker trajectory2MarkerMsg(PathSegment &trajectory, const int id, Eigen::Vector3d color) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -712,8 +703,7 @@ visualization_msgs::msg::Marker trajectory2MarkerMsg( return marker; } -visualization_msgs::msg::Marker point2MarkerMsg(Eigen::Vector3d &position, - const int id, Eigen::Vector3d color) { +visualization_msgs::msg::Marker point2MarkerMsg(Eigen::Vector3d &position, const int id, Eigen::Vector3d color) { visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = rclcpp::Clock().now(); @@ -778,8 +768,7 @@ double StringToGeoReference(std::string &exif_tag) { return output; } -bool parseAttitudeFromText(std::string text_path, std::string image_file, - Eigen::Vector4d &attitude) { +bool parseAttitudeFromText(std::string text_path, std::string image_file, Eigen::Vector4d &attitude) { std::ifstream file(text_path); std::string str; @@ -803,16 +792,14 @@ double getRandom(double min, double max) { return std::abs(max - min) * static_cast(rand()) / static_cast(RAND_MAX) + std::min(max, min); } -Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, - const Eigen::Vector4d &p) { +Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p) { Eigen::Vector4d quat; quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3), p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2), p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1), p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0); return quat; } -bool validatePosition(const grid_map::GridMap &map, const Eigen::Vector3d goal, - Eigen::Vector3d &valid_goal) { +bool validatePosition(const grid_map::GridMap &map, const Eigen::Vector3d goal, Eigen::Vector3d &valid_goal) { double upper_surface = map.atPosition("ics_+", goal.head(2)); double lower_surface = map.atPosition("ics_-", goal.head(2)); const bool is_goal_valid = (upper_surface < lower_surface) ? true : false; diff --git a/terrain_planner/src/test_ompl_dubins.cpp b/terrain_planner/src/test_ompl_dubins.cpp index 95582942..4cee3f36 100644 --- a/terrain_planner/src/test_ompl_dubins.cpp +++ b/terrain_planner/src/test_ompl_dubins.cpp @@ -39,13 +39,13 @@ * @author Jaeyoung Lim */ -#include -#include #include #include -#include #include +#include +#include +#include #include #include @@ -55,12 +55,8 @@ using namespace std::chrono_literals; -void getDubinsShortestPath( - const Eigen::Vector3d start_pos, - const double start_yaw, - const Eigen::Vector3d goal_pos, - const double goal_yaw, - std::vector& path) { +void getDubinsShortestPath(const Eigen::Vector3d start_pos, const double start_yaw, const Eigen::Vector3d goal_pos, + const double goal_yaw, std::vector& path) { auto dubins_ss = std::make_shared(); ompl::base::State* from = dubins_ss->allocState(); @@ -101,56 +97,49 @@ void getDubinsShortestPath( } } -class OmplRrtPlanner : public rclcpp::Node -{ - public: - OmplRrtPlanner() - : Node("ompl_rrt_planner") - { - // Initialize ROS related publishers for visualization - start_pos_pub = this->create_publisher("start_position", 1); - goal_pos_pub = this->create_publisher("goal_position", 1); - path_pub = this->create_publisher("path", 1); - trajectory_pub = this->create_publisher("tree", 1); - - std::vector start_yaw{0.0, 2.51681, 2.71681, 3.71681, 3.91681}; - std::vector goal_yaw{3.53454, 6.17454, 6.23454, 0.25135, 0.31135}; - - timer = this->create_wall_timer( - 1s, std::bind(&OmplRrtPlanner::timer_callback, this)); - } - - void timer_callback() { - Eigen::Vector3d start_pos(0.0, 0.0, 0.0); - Eigen::Vector3d goal_pos(152.15508, 0.0, 0.0); - - std::vector path; - - getDubinsShortestPath(start_pos, start_yaw[idx % start_yaw.size()], - goal_pos, goal_yaw[idx % goal_yaw.size()], - path); - - publishTrajectory(path_pub, path); - Eigen::Vector3d start_velocity(std::cos(start_yaw[idx % start_yaw.size()]), - std::sin(start_yaw[idx % start_yaw.size()]), - 0.0); - publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); - Eigen::Vector3d goal_velocity(std::cos(goal_yaw[idx % goal_yaw.size()]), - std::sin(goal_yaw[idx % goal_yaw.size()]), - 0.0); - publishPositionSetpoints(goal_pos_pub, goal_pos, goal_velocity); - idx++; - } +class OmplRrtPlanner : public rclcpp::Node { + public: + OmplRrtPlanner() : Node("ompl_rrt_planner") { + // Initialize ROS related publishers for visualization + start_pos_pub = this->create_publisher("start_position", 1); + goal_pos_pub = this->create_publisher("goal_position", 1); + path_pub = this->create_publisher("path", 1); + trajectory_pub = this->create_publisher("tree", 1); - private: std::vector start_yaw{0.0, 2.51681, 2.71681, 3.71681, 3.91681}; std::vector goal_yaw{3.53454, 6.17454, 6.23454, 0.25135, 0.31135}; - int idx{0}; - rclcpp::TimerBase::SharedPtr timer; - rclcpp::Publisher::SharedPtr start_pos_pub; - rclcpp::Publisher::SharedPtr goal_pos_pub; - rclcpp::Publisher::SharedPtr path_pub; - rclcpp::Publisher::SharedPtr trajectory_pub; + + timer = this->create_wall_timer(1s, std::bind(&OmplRrtPlanner::timer_callback, this)); + } + + void timer_callback() { + Eigen::Vector3d start_pos(0.0, 0.0, 0.0); + Eigen::Vector3d goal_pos(152.15508, 0.0, 0.0); + + std::vector path; + + getDubinsShortestPath(start_pos, start_yaw[idx % start_yaw.size()], goal_pos, goal_yaw[idx % goal_yaw.size()], + path); + + publishTrajectory(path_pub, path); + Eigen::Vector3d start_velocity(std::cos(start_yaw[idx % start_yaw.size()]), + std::sin(start_yaw[idx % start_yaw.size()]), 0.0); + publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); + Eigen::Vector3d goal_velocity(std::cos(goal_yaw[idx % goal_yaw.size()]), std::sin(goal_yaw[idx % goal_yaw.size()]), + 0.0); + publishPositionSetpoints(goal_pos_pub, goal_pos, goal_velocity); + idx++; + } + + private: + std::vector start_yaw{0.0, 2.51681, 2.71681, 3.71681, 3.91681}; + std::vector goal_yaw{3.53454, 6.17454, 6.23454, 0.25135, 0.31135}; + int idx{0}; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr start_pos_pub; + rclcpp::Publisher::SharedPtr goal_pos_pub; + rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr trajectory_pub; }; int main(int argc, char** argv) { diff --git a/terrain_planner/src/test_ompl_dubins_to_circle.cpp b/terrain_planner/src/test_ompl_dubins_to_circle.cpp index 2dc35ab7..2c570de1 100644 --- a/terrain_planner/src/test_ompl_dubins_to_circle.cpp +++ b/terrain_planner/src/test_ompl_dubins_to_circle.cpp @@ -38,13 +38,13 @@ * @author Jaeyoung Lim */ -#include -#include #include #include -#include #include +#include +#include +#include #include #include @@ -56,10 +56,8 @@ using namespace std::chrono_literals; double mod2pi(double x) { return x - 2 * M_PI * floor(x * (0.5 / M_PI)); } -void publishCircleSetpoints( - rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d& position, - const double radius) { +void publishCircleSetpoints(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d& position, const double radius) { visualization_msgs::msg::Marker marker; marker.header.stamp = rclcpp::Clock().now(); marker.type = visualization_msgs::msg::Marker::LINE_STRIP; @@ -177,65 +175,61 @@ double getDubinsTangentPoint(std::shared_ptrcreate_publisher("start_position", 1); - goal_pos_pub = this->create_publisher("goal_position", 1); - tangent_pos_pub = this->create_publisher("tangent_position", 1); - path_pub = this->create_publisher("path", 1); - path_pub_2 = this->create_publisher("candidate_path", 1); - trajectory_pub = this->create_publisher("tree", 1); - - timer = this->create_wall_timer( - 1s, std::bind(&OmplRrtPlanner::timer_callback, this)); - } +class OmplRrtPlanner : public rclcpp::Node { + public: + OmplRrtPlanner() : Node("ompl_rrt_planner") { + // Initialize ROS related publishers for visualization + start_pos_pub = this->create_publisher("start_position", 1); + goal_pos_pub = this->create_publisher("goal_position", 1); + tangent_pos_pub = this->create_publisher("tangent_position", 1); + path_pub = this->create_publisher("path", 1); + path_pub_2 = this->create_publisher("candidate_path", 1); + trajectory_pub = this->create_publisher("tree", 1); + + timer = this->create_wall_timer(1s, std::bind(&OmplRrtPlanner::timer_callback, this)); + } - void timer_callback() { - auto dubins_ss = std::make_shared(); - Eigen::Vector3d start_pos(0.0, 0.0, 0.0); - /// Goal circular radius - Eigen::Vector3d goal_pos(400.0, 0.0, 0.0); - double goal_radius = 66.6667; - double tangent_yaw = getDubinsTangentPoint(dubins_ss, start_pos, start_yaw, goal_pos, goal_radius); - - /// TODO: Compare two different positions with same tangent yaw - Eigen::Vector3d tangent_pos; - tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw - 0.5 * M_PI), - goal_pos.y() + goal_radius * std::sin(tangent_yaw - 0.5 * M_PI), goal_pos.z(); - std::vector path_candidate_1; - getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_1); - - tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw + 0.5 * M_PI), - goal_pos.y() + goal_radius * std::sin(tangent_yaw + 0.5 * M_PI), goal_pos.z(); - std::vector path_candidate_2; - getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_2); - - // Visualize - publishTrajectory(path_pub, path_candidate_1); - publishTrajectory(path_pub_2, path_candidate_2); - Eigen::Vector3d start_velocity(std::cos(start_yaw), std::sin(start_yaw), 0.0); - publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); - Eigen::Vector3d tangent_velocity(std::cos(tangent_yaw), std::sin(tangent_yaw), 0.0); - publishPositionSetpoints(tangent_pos_pub, tangent_pos, tangent_velocity); - publishCircleSetpoints(goal_pos_pub, goal_pos, goal_radius); - start_yaw += 0.1; - } + void timer_callback() { + auto dubins_ss = std::make_shared(); + Eigen::Vector3d start_pos(0.0, 0.0, 0.0); + /// Goal circular radius + Eigen::Vector3d goal_pos(400.0, 0.0, 0.0); + double goal_radius = 66.6667; + double tangent_yaw = getDubinsTangentPoint(dubins_ss, start_pos, start_yaw, goal_pos, goal_radius); + + /// TODO: Compare two different positions with same tangent yaw + Eigen::Vector3d tangent_pos; + tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw - 0.5 * M_PI), + goal_pos.y() + goal_radius * std::sin(tangent_yaw - 0.5 * M_PI), goal_pos.z(); + std::vector path_candidate_1; + getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_1); + + tangent_pos << goal_pos.x() + goal_radius * std::cos(tangent_yaw + 0.5 * M_PI), + goal_pos.y() + goal_radius * std::sin(tangent_yaw + 0.5 * M_PI), goal_pos.z(); + std::vector path_candidate_2; + getDubinsShortestPath(dubins_ss, start_pos, start_yaw, tangent_pos, tangent_yaw, path_candidate_2); + + // Visualize + publishTrajectory(path_pub, path_candidate_1); + publishTrajectory(path_pub_2, path_candidate_2); + Eigen::Vector3d start_velocity(std::cos(start_yaw), std::sin(start_yaw), 0.0); + publishPositionSetpoints(start_pos_pub, start_pos, start_velocity); + Eigen::Vector3d tangent_velocity(std::cos(tangent_yaw), std::sin(tangent_yaw), 0.0); + publishPositionSetpoints(tangent_pos_pub, tangent_pos, tangent_velocity); + publishCircleSetpoints(goal_pos_pub, goal_pos, goal_radius); + start_yaw += 0.1; + } - private: - double start_yaw{M_PI_2}; + private: + double start_yaw{M_PI_2}; - rclcpp::TimerBase::SharedPtr timer; - rclcpp::Publisher::SharedPtr start_pos_pub; - rclcpp::Publisher::SharedPtr goal_pos_pub; - rclcpp::Publisher::SharedPtr tangent_pos_pub; - rclcpp::Publisher::SharedPtr path_pub; - rclcpp::Publisher::SharedPtr path_pub_2; - rclcpp::Publisher::SharedPtr trajectory_pub; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr start_pos_pub; + rclcpp::Publisher::SharedPtr goal_pos_pub; + rclcpp::Publisher::SharedPtr tangent_pos_pub; + rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr path_pub_2; + rclcpp::Publisher::SharedPtr trajectory_pub; }; int main(int argc, char** argv) { diff --git a/terrain_planner/src/test_rrt_node.cpp b/terrain_planner/src/test_rrt_node.cpp index f6b51965..21cb367b 100644 --- a/terrain_planner/src/test_rrt_node.cpp +++ b/terrain_planner/src/test_rrt_node.cpp @@ -39,18 +39,17 @@ * @author Jaeyoung Lim */ +#include +#include + #include #include -#include -#include - +#include #include +#include #include -#include -#include +#include #include - -#include #include #include @@ -71,9 +70,7 @@ using namespace std::chrono_literals; -void publishPathSegments( - rclcpp::Publisher::SharedPtr pub, - Path& trajectory) { +void publishPathSegments(rclcpp::Publisher::SharedPtr pub, Path& trajectory) { visualization_msgs::msg::MarkerArray msg; std::vector marker; @@ -101,114 +98,109 @@ void publishPathSegments( pub->publish(msg); } -class OmplRrtPlanner : public rclcpp::Node -{ - public: - OmplRrtPlanner() - : Node("ompl_rrt_planner") - { - // Initialize ROS related publishers for visualization - start_pos_pub = this->create_publisher("start_position", 1); - goal_pos_pub = this->create_publisher("goal_position", 1); - path_pub = this->create_publisher("path", 1); - interpolate_path_pub = this->create_publisher("interpolated_path", 1); - path_segment_pub = this->create_publisher("path_segments", 1); - grid_map_pub = this->create_publisher("grid_map", 1); - trajectory_pub = this->create_publisher("tree", 1); - - map_path = this->declare_parameter("map_path", "."); - color_file_path = this->declare_parameter("color_file_path", "."); - random = this->declare_parameter("random", random); - - RCLCPP_INFO_STREAM(get_logger(), "map_path: " << map_path); - RCLCPP_INFO_STREAM(get_logger(), "color_file_path: " << color_file_path); - RCLCPP_INFO_STREAM(get_logger(), "random: " << random); - - // Load terrain map from defined tif paths - terrain_map = std::make_shared(); - terrain_map->initializeFromGeotiff(map_path, false); - // Load color layer if the color path is nonempty - if (!color_file_path.empty()) { - terrain_map->addColorFromGeotiff(color_file_path); - } - terrain_map->AddLayerDistanceTransform(50.0, "distance_surface"); - terrain_map->AddLayerDistanceTransform(120.0, "max_elevation"); - - timer = this->create_wall_timer( - 1s, std::bind(&OmplRrtPlanner::timer_callback, this)); +class OmplRrtPlanner : public rclcpp::Node { + public: + OmplRrtPlanner() : Node("ompl_rrt_planner") { + // Initialize ROS related publishers for visualization + start_pos_pub = this->create_publisher("start_position", 1); + goal_pos_pub = this->create_publisher("goal_position", 1); + path_pub = this->create_publisher("path", 1); + interpolate_path_pub = this->create_publisher("interpolated_path", 1); + path_segment_pub = this->create_publisher("path_segments", 1); + grid_map_pub = this->create_publisher("grid_map", 1); + trajectory_pub = this->create_publisher("tree", 1); + + map_path = this->declare_parameter("map_path", "."); + color_file_path = this->declare_parameter("color_file_path", "."); + random = this->declare_parameter("random", random); + + RCLCPP_INFO_STREAM(get_logger(), "map_path: " << map_path); + RCLCPP_INFO_STREAM(get_logger(), "color_file_path: " << color_file_path); + RCLCPP_INFO_STREAM(get_logger(), "random: " << random); + + // Load terrain map from defined tif paths + terrain_map = std::make_shared(); + terrain_map->initializeFromGeotiff(map_path); + // Load color layer if the color path is nonempty + if (!color_file_path.empty()) { + terrain_map->addColorFromGeotiff(color_file_path); } + terrain_map->AddLayerDistanceTransform(50.0, "distance_surface"); + terrain_map->AddLayerDistanceTransform(120.0, "max_elevation"); - void timer_callback() { - // for (int i = 0; i < num_experiments; i++) { - if (count_experiments++ < num_experiments) { - // Initialize planner with loaded terrain map - auto planner = std::make_shared(); - planner->setMap(terrain_map); - planner->setAltitudeLimits(120.0, 50.0); - /// TODO: Get bounds from gridmap - planner->setBoundsFromMap(terrain_map->getGridMap()); - - const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition(); - const double map_width_x = terrain_map->getGridMap().getLength().x(); - const double map_width_y = terrain_map->getGridMap().getLength().y(); - - Eigen::Vector3d start{Eigen::Vector3d(map_pos(0) - 0.4 * map_width_x, map_pos(1) - 0.4 * map_width_y, 0.0)}; - start(2) = - terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(start(0), start(1))) + terrain_altitude; - double start_yaw = getRandom(-M_PI, M_PI); - Eigen::Vector3d start_vel = 10.0 * Eigen::Vector3d(std::cos(start_yaw), std::sin(start_yaw), 0.0); - Eigen::Vector3d goal{Eigen::Vector3d(map_pos(0) + 0.4 * map_width_x, map_pos(1) + 0.4 * map_width_y, 0.0)}; - goal(2) = terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(goal(0), goal(1))) + terrain_altitude; - double goal_yaw = getRandom(-M_PI, M_PI); - Eigen::Vector3d goal_vel = 10.0 * Eigen::Vector3d(std::cos(goal_yaw), std::sin(goal_yaw), 0.0); - - planner->setupProblem(start, start_vel, goal, goal_vel); - bool found_solution{false}; - while (!found_solution) { - found_solution = planner->Solve(1.0, path); - } - planner->getSolutionPath(interpolated_path); - - // Repeatedly publish results - terrain_map->getGridMap().setTimestamp(this->get_clock()->now().nanoseconds()); - auto message = grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap()); - grid_map_pub->publish(std::move(message)); - publishTrajectory(path_pub, path.position()); - publishTrajectory(interpolate_path_pub, interpolated_path); - publishPathSegments(path_segment_pub, path); - publishPositionSetpoints(start_pos_pub, start, start_vel); - publishPositionSetpoints(goal_pos_pub, goal, goal_vel); - publishTree(trajectory_pub, planner->getPlannerData(), planner->getProblemSetup()); - if (!random) { - count_experiments = num_experiments; - // break; - } - // rclcpp::sleep_for(1s); + timer = this->create_wall_timer(1s, std::bind(&OmplRrtPlanner::timer_callback, this)); + } + + void timer_callback() { + // for (int i = 0; i < num_experiments; i++) { + if (count_experiments++ < num_experiments) { + // Initialize planner with loaded terrain map + auto planner = std::make_shared(); + planner->setMap(terrain_map); + planner->setAltitudeLimits(120.0, 50.0); + /// TODO: Get bounds from gridmap + planner->setBoundsFromMap(terrain_map->getGridMap()); + + const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition(); + const double map_width_x = terrain_map->getGridMap().getLength().x(); + const double map_width_y = terrain_map->getGridMap().getLength().y(); + + Eigen::Vector3d start{Eigen::Vector3d(map_pos(0) - 0.4 * map_width_x, map_pos(1) - 0.4 * map_width_y, 0.0)}; + start(2) = + terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(start(0), start(1))) + terrain_altitude; + double start_yaw = getRandom(-M_PI, M_PI); + Eigen::Vector3d start_vel = 10.0 * Eigen::Vector3d(std::cos(start_yaw), std::sin(start_yaw), 0.0); + Eigen::Vector3d goal{Eigen::Vector3d(map_pos(0) + 0.4 * map_width_x, map_pos(1) + 0.4 * map_width_y, 0.0)}; + goal(2) = terrain_map->getGridMap().atPosition("elevation", Eigen::Vector2d(goal(0), goal(1))) + terrain_altitude; + double goal_yaw = getRandom(-M_PI, M_PI); + Eigen::Vector3d goal_vel = 10.0 * Eigen::Vector3d(std::cos(goal_yaw), std::sin(goal_yaw), 0.0); + + planner->setupProblem(start, start_vel, goal, goal_vel); + bool found_solution{false}; + while (!found_solution) { + found_solution = planner->Solve(1.0, path); + } + planner->getSolutionPath(interpolated_path); + + // Repeatedly publish results + terrain_map->getGridMap().setTimestamp(this->get_clock()->now().nanoseconds()); + auto message = grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap()); + grid_map_pub->publish(std::move(message)); + publishTrajectory(path_pub, path.position()); + publishTrajectory(interpolate_path_pub, interpolated_path); + publishPathSegments(path_segment_pub, path); + publishPositionSetpoints(start_pos_pub, start, start_vel); + publishPositionSetpoints(goal_pos_pub, goal, goal_vel); + publishTree(trajectory_pub, planner->getPlannerData(), planner->getProblemSetup()); + if (!random) { + count_experiments = num_experiments; + // break; } + // rclcpp::sleep_for(1s); } + } - private: - - std::string map_path; - std::string color_file_path; - bool random{false}; - - std::shared_ptr terrain_map; - Path path; - std::vector interpolated_path; - double terrain_altitude{100.0}; - - int num_experiments{20}; - int count_experiments{0}; - - rclcpp::TimerBase::SharedPtr timer; - rclcpp::Publisher::SharedPtr start_pos_pub; - rclcpp::Publisher::SharedPtr goal_pos_pub; - rclcpp::Publisher::SharedPtr path_pub; - rclcpp::Publisher::SharedPtr interpolate_path_pub; - rclcpp::Publisher::SharedPtr path_segment_pub; - rclcpp::Publisher::SharedPtr grid_map_pub; - rclcpp::Publisher::SharedPtr trajectory_pub; + private: + std::string map_path; + std::string color_file_path; + bool random{false}; + + std::shared_ptr terrain_map; + Path path; + std::vector interpolated_path; + double terrain_altitude{100.0}; + + int num_experiments{20}; + int count_experiments{0}; + + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Publisher::SharedPtr start_pos_pub; + rclcpp::Publisher::SharedPtr goal_pos_pub; + rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr interpolate_path_pub; + rclcpp::Publisher::SharedPtr path_segment_pub; + rclcpp::Publisher::SharedPtr grid_map_pub; + rclcpp::Publisher::SharedPtr trajectory_pub; }; int main(int argc, char** argv) { diff --git a/terrain_planner/src/visualization.cpp b/terrain_planner/src/visualization.cpp index 5797708e..a73d025f 100644 --- a/terrain_planner/src/visualization.cpp +++ b/terrain_planner/src/visualization.cpp @@ -1,9 +1,7 @@ #include "terrain_planner/visualization.h" -void publishCandidateManeuvers( - rclcpp::Publisher::SharedPtr pub, - const std::vector& candidate_maneuvers, - bool visualize_invalid_trajectories) { +void publishCandidateManeuvers(rclcpp::Publisher::SharedPtr pub, + const std::vector& candidate_maneuvers, bool visualize_invalid_trajectories) { visualization_msgs::msg::MarkerArray msg; std::vector marker; @@ -25,11 +23,9 @@ void publishCandidateManeuvers( pub->publish(msg); } -void publishPositionSetpoints( - rclcpp::Publisher::SharedPtr pub, - const Eigen::Vector3d& position, - const Eigen::Vector3d& velocity, - const Eigen::Vector3d scale) { +void publishPositionSetpoints(rclcpp::Publisher::SharedPtr pub, + const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, + const Eigen::Vector3d scale) { visualization_msgs::msg::Marker marker; marker.header.stamp = rclcpp::Clock().now(); marker.type = visualization_msgs::msg::Marker::ARROW; @@ -55,10 +51,8 @@ void publishPositionSetpoints( pub->publish(marker); } -void publishPath( - rclcpp::Publisher::SharedPtr pub, - std::vector path, - Eigen::Vector3d color) { +void publishPath(rclcpp::Publisher::SharedPtr pub, std::vector path, + Eigen::Vector3d color) { visualization_msgs::msg::Marker marker; marker.header.stamp = rclcpp::Clock().now(); marker.type = visualization_msgs::msg::Marker::LINE_STRIP; @@ -89,9 +83,7 @@ void publishPath( pub->publish(marker); } -void publishTrajectory( - rclcpp::Publisher::SharedPtr pub, - std::vector trajectory) { +void publishTrajectory(rclcpp::Publisher::SharedPtr pub, std::vector trajectory) { nav_msgs::msg::Path msg; std::vector posestampedhistory_vector; Eigen::Vector4d orientation(1.0, 0.0, 0.0, 0.0); @@ -104,10 +96,9 @@ void publishTrajectory( pub->publish(msg); } -void publishTree( - rclcpp::Publisher::SharedPtr pub, - std::shared_ptr planner_data, - std::shared_ptr problem_setup) { +void publishTree(rclcpp::Publisher::SharedPtr pub, + std::shared_ptr planner_data, + std::shared_ptr problem_setup) { visualization_msgs::msg::MarkerArray marker_array; std::vector marker; diff --git a/terrain_planner/test/main.cpp b/terrain_planner/test/main.cpp index 92739fb9..4d820af7 100644 --- a/terrain_planner/test/main.cpp +++ b/terrain_planner/test/main.cpp @@ -1,6 +1,5 @@ #include - int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h b/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h index 9b4a9b9a..eb9ced32 100644 --- a/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h +++ b/terrain_planner_benchmark/include/terrain_planner_benchmark/rviz_replay.h @@ -37,14 +37,15 @@ * * @author Jaeyoung Lim */ -#include -#include - #include #include #include +#include +#include #include + #include + #include "terrain_planner/common.h" class ReplayRunner { diff --git a/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h b/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h index 852cd0b8..e22c81db 100644 --- a/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h +++ b/terrain_planner_benchmark/include/terrain_planner_benchmark/terrain_planner_benchmark.h @@ -2,8 +2,8 @@ #define TERRAIN_PLANNER_BENCHMARK_H #include -#include "terrain_navigation/data_logger.h" +#include "terrain_navigation/data_logger.h" #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" #include "terrain_planner/visualization.h" diff --git a/terrain_planner_benchmark/src/ompl_benchmark_node.cpp b/terrain_planner_benchmark/src/ompl_benchmark_node.cpp index e054d414..91d8807d 100644 --- a/terrain_planner_benchmark/src/ompl_benchmark_node.cpp +++ b/terrain_planner_benchmark/src/ompl_benchmark_node.cpp @@ -43,7 +43,6 @@ #include #include #include - #include #include "terrain_planner/terrain_ompl_rrt.h" diff --git a/terrain_planner_benchmark/src/surface_visualization.cpp b/terrain_planner_benchmark/src/surface_visualization.cpp index 3a079a07..c835c5fe 100644 --- a/terrain_planner_benchmark/src/surface_visualization.cpp +++ b/terrain_planner_benchmark/src/surface_visualization.cpp @@ -38,14 +38,14 @@ * @author Jaeyoung Lim */ -#include "terrain_planner/visualization.h" - +#include #include -#include "terrain_navigation/data_logger.h" -#include #include +#include "terrain_navigation/data_logger.h" +#include "terrain_planner/visualization.h" + void addErrorLayer(const std::string layer_name, const std::string query_layer, const std::string reference_layer, grid_map::GridMap& map) { map.add(layer_name); diff --git a/terrain_planner_benchmark/src/terrain_planner_benchmark.cpp b/terrain_planner_benchmark/src/terrain_planner_benchmark.cpp index 06d81b11..4ab73f32 100644 --- a/terrain_planner_benchmark/src/terrain_planner_benchmark.cpp +++ b/terrain_planner_benchmark/src/terrain_planner_benchmark.cpp @@ -1,5 +1,6 @@ #include "terrain_planner_benchmark/terrain_planner_benchmark.h" + #include "terrain_planner/terrain_ompl_rrt.h" TerrainPlannerBenchmark::TerrainPlannerBenchmark() { diff --git a/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp b/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp index 0381cf87..233faab3 100644 --- a/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp +++ b/terrain_planner_benchmark/src/terrain_planner_benchmark_node.cpp @@ -38,8 +38,6 @@ * @author Jaeyoung Lim */ -#include "terrain_planner_benchmark/terrain_planner_benchmark.h" - #include #include #include @@ -47,11 +45,13 @@ #include #include #include + #include #include "terrain_planner/common.h" #include "terrain_planner/terrain_ompl_rrt.h" #include "terrain_planner/visualization.h" +#include "terrain_planner_benchmark/terrain_planner_benchmark.h" int main(int argc, char** argv) { ros::init(argc, argv, "ompl_rrt_planner"); diff --git a/terrain_planner_benchmark/src/test_dubins_classification.cpp b/terrain_planner_benchmark/src/test_dubins_classification.cpp index d64333db..605153df 100644 --- a/terrain_planner_benchmark/src/test_dubins_classification.cpp +++ b/terrain_planner_benchmark/src/test_dubins_classification.cpp @@ -44,6 +44,7 @@ #include #include #include + #include #include diff --git a/terrain_planner_benchmark/src/test_ics.cpp b/terrain_planner_benchmark/src/test_ics.cpp index 131847b9..0eea5d2a 100644 --- a/terrain_planner_benchmark/src/test_ics.cpp +++ b/terrain_planner_benchmark/src/test_ics.cpp @@ -38,14 +38,14 @@ * @author Jaeyoung Lim */ -#include "terrain_planner/visualization.h" - +#include #include -#include "terrain_navigation/data_logger.h" -#include #include +#include "terrain_navigation/data_logger.h" +#include "terrain_planner/visualization.h" + void addErrorLayer(const std::string layer_name, const std::string query_layer, const std::string reference_layer, grid_map::GridMap& map) { map.add(layer_name); diff --git a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp index 9a886408..cedc66fa 100644 --- a/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp +++ b/terrain_planner_benchmark/src/test_rrt_circle_goal.cpp @@ -40,14 +40,13 @@ #include #include -#include - #include - #include #include #include #include + +#include #include #include "terrain_navigation/data_logger.h" diff --git a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp index 0a4611db..0a084d1a 100644 --- a/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp +++ b/terrain_planner_benchmark/src/test_rrt_replanning_node.cpp @@ -45,6 +45,9 @@ #include #include #include + +#include +#include #include #include "terrain_navigation/data_logger.h" @@ -52,9 +55,6 @@ #include "terrain_planner/terrain_ompl_rrt.h" #include "terrain_planner/visualization.h" -#include -#include - void writeToFile(const std::string path, std::vector solution_path) { // Write data to files std::cout << "[DataLogger] Writing data to file! " << path << std::endl;