Skip to content

OutdoorNav release 2.3.0 #501

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

Draft
jmastrangelo-cpr wants to merge 1 commit into
base: development
Choose a base branch
from
Draft

OutdoorNav release 2.3.0 #501

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
110 changes: 55 additions & 55 deletions docs_outdoornav_user_manual/api/api_endpoints/all_other_endpoints.mdx
View file
Open in desktop

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions docs_outdoornav_user_manual/api/api_endpoints/autonomy/execute_task.mdx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ Execute a task as a service.
<Tabs groupId="autonomy_task_srv">
<TabItem value="Python" label="Python" default>

```python
```python showLineNumbers
import rclpy
from rclpy.node import Node

Expand Down Expand Up @@ -56,7 +56,7 @@ if __name__ == '__main__':
</TabItem>
<TabItem value="C++" label="C++">

```cpp
```cpp showLineNumbers
#include "rclcpp/rclcpp.hpp"
#include "clearpath_task_msgs/srv/execute_task.hpp"

Expand Down
263 changes: 4 additions & 259 deletions docs_outdoornav_user_manual/api/api_endpoints/autonomy/goto.mdx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,269 +17,14 @@ Send the platform to a location on the map.
<Tabs groupId="autonomy_goto">
<TabItem value="Python" label="Python" default>

```python
from action_msgs.msg import GoalStatus # Import for reference to status constants
from clearpath_config.common.utils.yaml import read_yaml
from clearpath_navigation_msgs.action import ExecuteGoTo
from clearpath_navigation_msgs.msg import Waypoint
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

ROBOT_CONFIG_PATH = '/etc/clearpath/robot.yaml'
LOGGING_NAME = 'ExecuteGoTo'

# TODO: Enter your map uuid and waypoint info
MAP_ID: str = ''
WAYPOINT = Waypoint(
uuid='',
name='',
latitude=,
longitude=,
heading=,
position_tolerance=-1.0, # -1.0 means off
yaw_tolerance=-1.0, # -1.0 means off
tasks=[]
)


class ExecuteGoToActionClient(Node):

def __init__(self, namespace: str):
super().__init__('execute_goto')
self._namespace = namespace
self._action_client = ActionClient(self, ExecuteGoTo, f'/{self._namespace}/autonomy/goto')
self._goto_goal_handle = None
self._goto_goal_cancelled = False
self._goto_running = False
self._goto_completed = False

###################################
# GoTo Action Client Functions #
###################################
def send_goto_goal(self, map_id, waypoint):
goal_msg = ExecuteGoTo.Goal()

# populate goal message
goal_msg.map_uuid = map_id
goal_msg.waypoint = waypoint

if not self._action_client.wait_for_server(timeout_sec=5.0):
self.get_logger().error(f'[{LOGGING_NAME}] /{self._namespace}/autonomy/goto action server not available!')
self.destroy_node()
rclpy.shutdown()
return

self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.goto_feedback_cb)
self._send_goal_future.add_done_callback(self.goto_goal_response_cb)

def goto_goal_response_cb(self, future):
self._goto_goal_handle = future.result()
if not self._goto_goal_handle.accepted:
self.get_logger().info(f'[{LOGGING_NAME}] GoTo Goal rejected')
self._goto_goal_handle = None
self.destroy_node()
rclpy.shutdown()
return

self._goto_running = True
self._goto_completed = False
self._goto_goal_cancelled = False
self.get_logger().info(f'[{LOGGING_NAME}] GoTo Goal accepted')

self._get_result_future = self._goto_goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.goto_get_result_cb)

def goto_feedback_cb(self, feedback_msg):
feedback = feedback_msg.feedback
self.get_logger().info(f'[{LOGGING_NAME}] GoTo has been running for: {feedback.elapsed_time:.2f}s', throttle_duration_sec=60)

def goto_cancel_response_cb(self, future):
cancel_response = future.result()
self.get_logger().info(f'[{LOGGING_NAME}] Cancel response received: {cancel_response.return_code}')
if len(cancel_response.goals_canceling) > 0:
self.get_logger().info(f'[{LOGGING_NAME}] Canceling of goto goal complete')
else:
self.get_logger().warning(f'[{LOGGING_NAME}] GoTo Goal failed to cancel')

def goto_get_result_cb(self, future):
result = future.result().result
status = future.result().status

# Compare the status integer against the constants defined in the message
if status == GoalStatus.STATUS_SUCCEEDED:
if result.success:
self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Succeeded! Result: Robot completed goto!')
self._goto_completed = True
else:
self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Succeeded! Result: Robot failed to complete goto')

elif status == GoalStatus.STATUS_CANCELED:
self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Cancelled!')
self._goto_goal_cancelled = True

elif status == GoalStatus.STATUS_ABORTED:
self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Aborted!')
else:
self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Finished with unknown status: {status}')

self._goto_goal_handle = None
self._goto_running = False

self.destroy_node()
rclpy.shutdown()


def main(args=None):
rclpy.init(args=args)
robot_config = read_yaml(ROBOT_CONFIG_PATH)
namespace = robot_config['system']['ros2']['namespace']

goto = ExecuteGoToActionClient(namespace)
goto.send_goto_goal(MAP_ID, WAYPOINT)
rclpy.spin(goto)


if __name__ == '__main__':
main()

```python reference showLineNumbers
https://github.com/cpr-application/clearpath_outdoornav/blob/ros2/python/endpoints/autonomy/goto.py
```
</TabItem>
<TabItem value="C++" label="C++">

```cpp
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <yaml-cpp/yaml.h>

#include "clearpath_navigation_msgs/action/execute_go_to.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

// TODO: Enter your map id and waypoint information
const std::string MAP_ID = "";
const std::string WAYPOINT_ID = "";
const std::string WAYPOINT_NAME = "";
const double WAYPOINT_LAT = ;
const double WAYPOINT_LON = ;
const double WAYPOINT_HEADING = ;
const double WAYPOINT_POSITION_TOLERANCE = -1.0; // -1.0 means off
const double WAYPOINT_YAW_TOLERANCE = -1.0; // -1.0 means off


namespace goto_action_client
{
class ExecuteGoToActionClient : public rclcpp::Node
{
public:
using ExecuteGoTo = clearpath_navigation_msgs::action::ExecuteGoTo;
using GoalHandleExecuteGoTo = rclcpp_action::ClientGoalHandle<ExecuteGoTo>;

explicit ExecuteGoToActionClient(const rclcpp::NodeOptions & options)
: Node("execute_goto", options)
{
YAML::Node robot_config = YAML::LoadFile("/etc/clearpath/robot.yaml");
namespace_ = robot_config["system"]["ros2"]["namespace"].as<std::string>();

this->client_ptr_ = rclcpp_action::create_client<ExecuteGoTo>(
this, "/" + namespace_ + "/" + std::string("autonomy/goto"));

this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&ExecuteGoToActionClient::send_goal, this));
}

void send_goal()
{
using namespace std::placeholders;

this->timer_->cancel();

if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "%s/autonomy/goto action server not available after waiting", namespace_.c_str());
rclcpp::shutdown();
}

auto goal_msg = ExecuteGoTo::Goal();
goal_msg.map_uuid = MAP_ID;

auto waypoint = clearpath_navigation_msgs::msg::Waypoint();
waypoint.uuid = WAYPOINT_ID;
waypoint.name = WAYPOINT_NAME;
waypoint.uuid = WAYPOINT_ID;
waypoint.latitude = WAYPOINT_LAT;
waypoint.longitude = WAYPOINT_LON;
waypoint.heading = WAYPOINT_HEADING;
waypoint.position_tolerance = WAYPOINT_POSITION_TOLERANCE;
waypoint.yaw_tolerance = WAYPOINT_YAW_TOLERANCE;
goal_msg.waypoint = waypoint;

auto send_goal_options = rclcpp_action::Client<ExecuteGoTo>::SendGoalOptions();

send_goal_options.goal_response_callback = [this](const GoalHandleExecuteGoTo::SharedPtr & goal_handle)
{
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "GoTo Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "GoTo Goal accepted by server, waiting for result");
}
};

send_goal_options.feedback_callback = [this](
GoalHandleExecuteGoTo::SharedPtr,
const std::shared_ptr<const ExecuteGoTo::Feedback> feedback)
{
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 60000, "GoTo has been running for: %.3f", feedback->elapsed_time);
};

send_goal_options.result_callback = [this](const GoalHandleExecuteGoTo::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
if (result.result->success)
{
RCLCPP_INFO(this->get_logger(), "[GoTo Goal Result] Succeeded! Result: Robot arrived at goto position successfully!");
}
else
{
RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Succeeded! Result: Robot failed to arrive to goto position");
}
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Aborted!");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Cancelled!");
return;
default:
RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Finished with unknown code");
return;
}
rclcpp::shutdown();
};

this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}

private:
rclcpp_action::Client<ExecuteGoTo>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
std::string namespace_;

}; // class ExecuteGoToActionClient

} // namespace goto_action_client

int main()
{
}

RCLCPP_COMPONENTS_REGISTER_NODE(goto_action_client::ExecuteGoToActionClient)

```cpp reference showLineNumbers
https://github.com/cpr-application/clearpath_outdoornav/blob/ros2/cpp/endpoints/autonomy/goto.cpp
```
</TabItem>

Expand Down
Loading