SMACC2
odom_tracker_node.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20#include <actionlib/server/simple_action_server.h>
21#include <odom_tracker/OdomTrackerAction.h>
22#include <memory>
24
25typedef actionlib::SimpleActionServer<odom_tracker::OdomTrackerAction> Server;
26
27using namespace odom_tracker;
28using namespace cl_nav2z::odom_tracker;
29
31{
32public:
33 std::shared_ptr<Server> as_;
35
37
43 void execute(
44 const OdomTrackerGoalConstPtr & goal) // Note: "Action" is not appended to DoDishes here
45 {
46 try
47 {
48 switch (goal->command)
49 {
50 case OdomTrackerGoal::RECORD_PATH:
51 odomTracker.setWorkingMode(WorkingMode::RECORD_PATH);
52 break;
53
54 case OdomTrackerGoal::CLEAR_PATH:
55 odomTracker.setWorkingMode(WorkingMode::CLEAR_PATH);
56 break;
57
58 case OdomTrackerGoal::IDLE:
59 odomTracker.setWorkingMode(WorkingMode::IDLE);
60 break;
61
62 case OdomTrackerGoal::START_BROADCAST_PATH:
64 break;
65
66 case OdomTrackerGoal::STOP_BROADCAST_PATH:
68 break;
69
70 case OdomTrackerGoal::PUSH_PATH:
72 break;
73
74 case OdomTrackerGoal::POP_PATH:
76 break;
77
78 default:
79
80 RCLCPP_ERROR(
81 getLogger(), "Odom Tracker Node - Action Server execute error: incorrect command - %d",
82 goal->command);
83 as_->setAborted();
84 }
85
86 // never reach succeeded because were are interested in keeping the feedback alive
87 as_->setSucceeded();
88 }
89 catch (std::exception & ex)
90 {
91 RCLCPP_ERROR(getLogger(), "Odom Tracker Node - Action Server execute error: %s", ex.what());
92 as_->setAborted();
93 }
94 }
95
101 void run()
102 {
103 rclcpp::Node::SharedPtr n;
104 RCLCPP_INFO(getLogger(), "Creating odom tracker action server");
105
106 as_ = std::make_shared<Server>(
107 n, "odom_tracker", boost::bind(&OdomTrackerActionServer::execute, this, _1), false);
108 RCLCPP_INFO(getLogger(), "Starting OdomTracker Action Server");
109
110 as_->start();
111
112 ros::spin();
113 }
114};
115
116int main(int argc, char ** argv)
117{
118 ros::init(argc, argv, "odom_tracker_node");
120
121 as.run();
122}
std::shared_ptr< Server > as_
void execute(const OdomTrackerGoalConstPtr &goal)
void setWorkingMode(WorkingMode workingMode)
void popPath(int pathCount=1, bool keepPreviousPath=false)
int main(int argc, char **argv)
actionlib::SimpleActionServer< odom_tracker::OdomTrackerAction > Server