Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Merge latest action server updates to Petr #1

Open
wants to merge 6 commits into
base: petr
Choose a base branch
from
Open

Merge latest action server updates to Petr #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
4981ff3
added information about bumper and obstacles to the diagnostics message
matemat13 Mar 8, 2022
1f8e582
Merge branch 'action_server' of github.com:fly4future/navigation into…
matemat13 Mar 8, 2022
6831f83
fixed deleting of pointer when previous goal is aborted due to new one
spurnvoj Mar 14, 2022
059f6c8
add debugging output with uav and waypoint heading
stibipet Mar 14, 2022
20fa224
Merge branch 'petr' into action_server
stibipet Mar 17, 2022
91de574
Revert "Merge branch 'petr' into action_server"
stibipet Mar 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions config/navigation.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@ planning:
navigation_tolerance: 0.4 # [m]
min_altitude: 1.0 # [m]
max_altitude: 10.0 # [m]
ground_cutoff: 0.7 # [m] obstacles below this threshold will not be added to the planning map
max_goal_distance: 50.0 # [m]
distance_penalty: 1.0
greedy_penalty: 1.0
Expand All @@ -17,7 +16,7 @@ planning:
replanning_distance: 15.0 # [m] Euclidean distance from first waypoint
override_previous_commands: true # new GOTO will override all previous actions
main_update_rate: 10.0 # [Hz]
diagnostics_rate: 1.0 # [Hz]
diagnostics_rate: 1.0 # [Hz]

visualization:
visualize_planner: true
Expand Down
47 changes: 20 additions & 27 deletions include/navigation/astar_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ namespace navigation

enum TreeValue
{
FREE = -1,
OCCUPIED = 1
OCCUPIED = 0,
FREE = 1
};

enum PlanningResult
Expand All @@ -33,9 +33,9 @@ enum PlanningResult
struct Node
{
octomap::OcTreeKey key;
float total_cost;
float cum_dist;
float goal_dist;
float total_cost;
float cum_dist;
float goal_dist;

bool operator==(const Node &other) const;
bool operator!=(const Node &other) const;
Expand All @@ -62,8 +62,7 @@ class AstarPlanner {

public:
AstarPlanner(float safe_obstacle_distance, float euclidean_distance_cutoff, float planning_tree_resolution, float distance_penalty, float greedy_penalty,
float min_altitude, float max_altitude, float ground_cutoff, float timeout_threshold, float max_waypoint_distance, bool unknown_is_occupied,
const rclcpp::Logger &logger);
float min_altitude, float max_altitude, float timeout_threshold, float max_waypoint_distance, bool unknown_is_occupied, const rclcpp::Logger& logger);

private:
float safe_obstacle_distance;
Expand All @@ -75,55 +74,49 @@ class AstarPlanner {
float max_waypoint_distance;
float min_altitude;
float max_altitude;
float ground_cutoff;
bool unknown_is_occupied;

bool unknown_is_occupied;
rclcpp::Logger logger_;

public:
std::pair<std::vector<octomap::point3d>, PlanningResult> findPath(
const octomap::point3d &start_coord, const octomap::point3d &goal_coord, std::shared_ptr<octomap::OcTree> mapping_tree, float timeout,
std::function<void(const std::shared_ptr<octomap::OcTree> &)> visualizeTree,
std::function<void(const std::unordered_set<Node, HashFunction> &, const std::unordered_set<Node, HashFunction> &,
const std::shared_ptr<octomap::OcTree> &)>
std::function<void(const octomap::OcTree &)> visualizeTree,
std::function<void(const std::unordered_set<Node, HashFunction> &, const std::unordered_set<Node, HashFunction> &, const octomap::OcTree &)>
visualizeExpansions);

private:
const std::vector<std::vector<int>> EXPANSION_DIRECTIONS = {{-1, -1, -1}, {-1, -1, 0}, {-1, -1, 1}, {-1, 0, -1}, {-1, 0, 0}, {-1, 0, 1}, {-1, 1, -1},
{-1, 1, 0}, {-1, 1, 1}, {0, -1, -1}, {0, -1, 0}, {0, -1, 1}, {0, 0, -1}, {0, 0, 1},
{0, 1, -1}, {0, 1, 0}, {0, 1, 1}, {1, -1, -1}, {1, -1, 0}, {1, -1, 1}, {1, 0, -1},
{1, 0, 0}, {1, 0, 1}, {1, 1, -1}, {1, 1, 0}, {1, 1, 1}};
float getNodeDepth(const octomap::OcTreeKey &key, std::shared_ptr<octomap::OcTree> tree);
float getNodeDepth(const octomap::OcTreeKey &key, octomap::OcTree &tree);

std::vector<octomap::OcTreeKey> getNeighborhood(const octomap::OcTreeKey &key, std::shared_ptr<octomap::OcTree> tree);
std::vector<octomap::OcTreeKey> getNeighborhood(const octomap::OcTreeKey &key, octomap::OcTree &tree);

octomap::OcTreeKey expand(const octomap::OcTreeKey &key, const std::vector<int> &direction);

float distEuclidean(const octomap::point3d &p1, const octomap::point3d &p2);

float distEuclidean(const octomap::OcTreeKey &k1, const octomap::OcTreeKey &k2, std::shared_ptr<octomap::OcTree> tree);
float distEuclidean(const octomap::OcTreeKey &k1, const octomap::OcTreeKey &k2, octomap::OcTree &tree);

bool freeStraightPath(const octomap::point3d p1, const octomap::point3d p2, std::shared_ptr<octomap::OcTree> tree);
bool freeStraightPath(const octomap::point3d p1, const octomap::point3d p2, octomap::OcTree &tree);

std::vector<octomap::OcTreeKey> backtrackPathKeys(const Node &start, const Node &end, std::unordered_map<Node, Node, HashFunction> &parent_map);

std::vector<octomap::point3d> keysToCoords(std::vector<octomap::OcTreeKey> keys, std::shared_ptr<octomap::OcTree> tree);
std::vector<octomap::point3d> keysToCoords(std::vector<octomap::OcTreeKey> keys, octomap::OcTree &tree);

DynamicEDTOctomap euclideanDistanceTransform(std::shared_ptr<octomap::OcTree> tree);

std::shared_ptr<octomap::OcTree> createPlanningTree(std::shared_ptr<octomap::OcTree> tree, float resolution);

std::vector<octomap::point3d> createEscapeTunnel(const std::shared_ptr<octomap::OcTree> mapping_tree, const std::shared_ptr<octomap::OcTree> planning_tree,
const octomap::point3d &start);

std::vector<octomap::point3d> createVerticalTunnel(const std::shared_ptr<octomap::OcTree> mapping_tree, const octomap::point3d &start);
std::optional<std::pair<octomap::OcTree, std::vector<octomap::point3d>>> createPlanningTree(std::shared_ptr<octomap::OcTree> tree,
const octomap::point3d &start, float resolution);

std::pair<octomap::point3d, bool> generateTemporaryGoal(const octomap::point3d &start, const octomap::point3d &goal, std::shared_ptr<octomap::OcTree> tree);
std::pair<octomap::point3d, bool> generateTemporaryGoal(const octomap::point3d &start, const octomap::point3d &goal, octomap::OcTree &tree);

std::vector<octomap::point3d> filterPath(const std::vector<octomap::point3d> &waypoints, std::shared_ptr<octomap::OcTree> tree, bool append_endpoint);
std::vector<octomap::point3d> filterPath(const std::vector<octomap::point3d> &waypoints, octomap::OcTree &tree);

std::vector<octomap::point3d> prepareOutputPath(const std::vector<octomap::OcTreeKey> &keys, std::shared_ptr<octomap::OcTree> tree, bool append_endpoint);
std::vector<octomap::point3d> prepareOutputPath(const std::vector<octomap::OcTreeKey> &keys, octomap::OcTree &tree);

/* geometry_msgs::msg::Quaternion yawToQuaternionMsg(const float &yaw); */
};

} // namespace navigation
Expand Down
2 changes: 1 addition & 1 deletion launch/navigation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ def generate_launch_description():
{"use_sim_time": launch.substitutions.LaunchConfiguration("use_sim_time")},
],
remappings=[
("~/octomap_in", "/" + DRONE_DEVICE_ID + "/octomap_server/octomap/local/full"),
("~/octomap_in", "/" + DRONE_DEVICE_ID + "/octomap_server/octomap_full"),
("~/odometry_in", "/" + DRONE_DEVICE_ID + "/odometry/local_odom"),
("~/cmd_pose_in", "/" + DRONE_DEVICE_ID + "/control_interface/cmd_pose"),
("~/hover_in", "~/hover"),
Expand Down
3 changes: 2 additions & 1 deletion scripts/action_client_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,8 @@ def get_result_callback(self, future):
elif status == GoalStatus.STATUS_CANCELED:
self.get_logger().error('Goal canceled! Result: {0}'.format(result.message))

self._goal_handle = None
if self._goal_handle.goal_id == future.result().goal_id:
self._goal_handle = None

def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback
Expand Down
Loading