Skip to content
Snippets Groups Projects
Commit cba1e8cf authored by bpwilcox's avatar bpwilcox
Browse files

change srv name, remove msg

revert launch file change
parent d5f1dcc3
No related branches found
No related tags found
No related merge requests found
......@@ -24,12 +24,12 @@
#include "nav2_util/lifecycle_service_client.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"
#include "nav2_msgs/srv/manage_nodes.hpp"
#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
namespace nav2_lifecycle_manager
{
using nav2_msgs::srv::ManageNodes;
using nav2_msgs::srv::ManageLifecycleNodes;
class LifecycleManager : public rclcpp::Node
{
......@@ -42,12 +42,12 @@ protected:
rclcpp::Node::SharedPtr service_client_node_;
// The services provided by this node
rclcpp::Service<ManageNodes>::SharedPtr manager_srv_;
rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;
void managerCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<ManageNodes::Request> request,
std::shared_ptr<ManageNodes::Response> response);
const std::shared_ptr<ManageLifecycleNodes::Request> request,
std::shared_ptr<ManageLifecycleNodes::Response> response);
// Support functions for the service calls
bool startup();
......@@ -56,9 +56,8 @@ protected:
bool pause();
bool resume();
// Support functions for bring-up
// Support function for creating service clients
void createLifecycleServiceClients();
bool bringupNode(const std::string & node_name);
// Support functions for shutdown
void shutdownAllNodes();
......
......@@ -24,7 +24,7 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_srvs/srv/empty.hpp"
#include "nav2_msgs/srv/manage_nodes.hpp"
#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
namespace nav2_lifecycle_manager
{
......@@ -46,7 +46,7 @@ public:
bool navigate_to_pose(double x, double y, double theta);
protected:
using ManageNodes = nav2_msgs::srv::ManageNodes;
using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes;
// A generic method used to call startup, shutdown, etc.
bool callService(uint8_t command);
......@@ -54,7 +54,7 @@ protected:
// The node to use for the service call
rclcpp::Node::SharedPtr node_;
rclcpp::Client<ManageNodes>::SharedPtr manager_client_;
rclcpp::Client<ManageLifecycleNodes>::SharedPtr manager_client_;
std::string service_name_{"lifecycle_manager/manage_nodes"};
using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
......
......@@ -26,7 +26,6 @@ using namespace std::placeholders;
using lifecycle_msgs::msg::Transition;
using lifecycle_msgs::msg::State;
using nav2_msgs::msg::Command;
using nav2_util::LifecycleServiceClient;
namespace nav2_lifecycle_manager
......@@ -49,7 +48,7 @@ LifecycleManager::LifecycleManager()
get_parameter("node_names", node_names_);
get_parameter("autostart", autostart_);
manager_srv_ = create_service<ManageNodes>("lifecycle_manager/manage_nodes",
manager_srv_ = create_service<ManageLifecycleNodes>("lifecycle_manager/manage_nodes",
std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3));
auto options = rclcpp::NodeOptions().arguments(
......@@ -85,23 +84,23 @@ LifecycleManager::~LifecycleManager()
void
LifecycleManager::managerCallback(
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
const std::shared_ptr<ManageNodes::Request> request,
std::shared_ptr<ManageNodes::Response> response)
const std::shared_ptr<ManageLifecycleNodes::Request> request,
std::shared_ptr<ManageLifecycleNodes::Response> response)
{
switch (request->command.id) {
case Command::STARTUP:
switch (request->command) {
case ManageLifecycleNodes::Request::STARTUP:
response->success = startup();
break;
case Command::RESET:
case ManageLifecycleNodes::Request::RESET:
response->success = reset();
break;
case Command::SHUTDOWN:
case ManageLifecycleNodes::Request::SHUTDOWN:
response->success = shutdown();
break;
case Command::PAUSE:
case ManageLifecycleNodes::Request::PAUSE:
response->success = pause();
break;
case Command::RESUME:
case ManageLifecycleNodes::Request::RESUME:
response->success = resume();
break;
}
......
......@@ -23,7 +23,6 @@
namespace nav2_lifecycle_manager
{
using nav2_util::geometry_utils::orientationAroundZAxis;
using nav2_msgs::msg::Command;
LifecycleManagerClient::LifecycleManagerClient()
{
......@@ -31,7 +30,7 @@ LifecycleManagerClient::LifecycleManagerClient()
node_ = std::make_shared<rclcpp::Node>("lifecycle_manager_client_service_client");
// Create the service clients
manager_client_ = node_->create_client<ManageNodes>(service_name_);
manager_client_ = node_->create_client<ManageLifecycleNodes>(service_name_);
navigate_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(node_, "NavigateToPose");
......@@ -44,31 +43,31 @@ LifecycleManagerClient::LifecycleManagerClient()
bool
LifecycleManagerClient::startup()
{
return callService(Command::STARTUP);
return callService(ManageLifecycleNodes::Request::STARTUP);
}
bool
LifecycleManagerClient::shutdown()
{
return callService(Command::SHUTDOWN);
return callService(ManageLifecycleNodes::Request::SHUTDOWN);
}
bool
LifecycleManagerClient::pause()
{
return callService(Command::PAUSE);
return callService(ManageLifecycleNodes::Request::PAUSE);
}
bool
LifecycleManagerClient::resume()
{
return callService(Command::RESUME);
return callService(ManageLifecycleNodes::Request::RESUME);
}
bool
LifecycleManagerClient::reset()
{
return callService(Command::RESET);
return callService(ManageLifecycleNodes::Request::RESET);
}
void
......@@ -143,8 +142,8 @@ LifecycleManagerClient::navigate_to_pose(double x, double y, double theta)
bool
LifecycleManagerClient::callService(uint8_t command)
{
auto request = std::make_shared<ManageNodes::Request>();
request->command.id = command;
auto request = std::make_shared<ManageLifecycleNodes::Request>();
request->command = command;
RCLCPP_INFO(node_->get_logger(), "Waiting for the lifecycle_manager's %s service...",
service_name_);
......
......@@ -16,12 +16,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/CostmapMetaData.msg"
"msg/Path.msg"
"msg/VoxelGrid.msg"
"msg/Command.msg"
"srv/GetCostmap.srv"
"srv/ClearCostmapExceptRegion.srv"
"srv/ClearCostmapAroundRobot.srv"
"srv/ClearEntireCostmap.srv"
"srv/ManageNodes.srv"
"srv/ManageLifecycleNodes.srv"
"action/BackUp.action"
"action/ComputePathToPose.action"
"action/FollowPath.action"
......
......@@ -4,4 +4,6 @@ uint8 RESUME = 2
uint8 RESET = 3
uint8 SHUTDOWN = 4
uint8 id
uint8 command
---
bool success
Command command
---
bool success
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment