SMACC2
cb_navigate_forward.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
22
27
28namespace cl_nav2z
29{
30using ::cl_nav2z::odom_tracker::OdomTracker;
32
33using ::cl_nav2z::Pose;
34
35CbNavigateForward::CbNavigateForward(float distance_meters) : forwardDistance_(distance_meters) {}
37
38CbNavigateForward::CbNavigateForward(geometry_msgs::msg::PoseStamped goal) : goalPose_(goal) {}
39
41
42void CbNavigateForward::setForwardDistance(float distance_meters)
43{
44 if (distance_meters < 0)
45 {
46 RCLCPP_INFO_STREAM(
47 getLogger(), "[" << getName() << "] negative forward distance: " << distance_meters
48 << ". Resetting to 0.");
49 distance_meters = 0;
50 }
51 forwardDistance_ = distance_meters;
52
53 RCLCPP_INFO_STREAM(
54 getLogger(), "[" << getName() << "] setting fw motion distance: " << *forwardDistance_);
55}
56
58{
60 {
62
63 RCLCPP_INFO_STREAM(
64 getLogger(), "[" << getName() << "] Straight motion distance: " << *forwardDistance_);
65 }
66
67 // get current pose
69 auto referenceFrame = p->getReferenceFrame();
70 auto currentPoseMsg = p->toPoseMsg();
71 tf2::Transform currentPose;
72 tf2::fromMsg(currentPoseMsg, currentPose);
73
74 RCLCPP_INFO_STREAM(
75 getLogger(), "[" << getName() << "]"
76 << "current pose: " << currentPoseMsg);
77
78 // force global orientation if it is requested
80 {
81 currentPoseMsg.orientation = *forceInitialOrientation;
82 RCLCPP_WARN_STREAM(
83 getLogger(),
84 "[" << getName() << "]"
85 << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation);
86 }
87
88 tf2::Transform targetPose;
89 if (goalPose_)
90 {
91 tf2::fromMsg(goalPose_->pose, targetPose);
92 }
93 else if (forwardDistance_)
94 {
95 // compute forward goal pose
96 tf2::Transform forwardDeltaTransform;
97 forwardDeltaTransform.setIdentity();
98 forwardDeltaTransform.setOrigin(tf2::Vector3(*forwardDistance_, 0, 0));
99
100 targetPose = currentPose * forwardDeltaTransform;
101 }
102 else
103 {
104 RCLCPP_WARN_STREAM(
105 getLogger(),
106 "[" << getName() << "]"
107 << "No goal Pose or Distance is specified. Aborting. No action request is sent."
108 << currentPoseMsg.orientation);
109
110 return;
111 }
112
113 // action goal
114 ClNav2Z::Goal goal;
115 goal.pose.header.frame_id = referenceFrame;
116 goal.pose.header.stamp = getNode()->now();
117 tf2::toMsg(targetPose, goal.pose.pose);
118 RCLCPP_INFO_STREAM(
119 getLogger(), "[" << getName() << "]"
120 << " TARGET POSE FORWARD: " << goal.pose.pose);
121
122 // current pose
123 geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
124 currentStampedPoseMsg.header.frame_id = referenceFrame;
125 currentStampedPoseMsg.header.stamp = getNode()->now();
126 tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
127
129 if (odomTracker_ != nullptr)
130 {
131 auto pathname = this->getCurrentState()->getName() + " - " + getName();
132 odomTracker_->pushPath(pathname);
133 odomTracker_->setStartPoint(currentStampedPoseMsg);
135 odomTracker_->setWorkingMode(WorkingMode::RECORD_PATH);
136 }
137
138 auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
139 plannerSwitcher->setForwardPlanner();
140
141 auto goalCheckerSwitcher = moveBaseClient_->getComponent<GoalCheckerSwitcher>();
142 goalCheckerSwitcher->setGoalCheckerId("forward_goal_checker");
143
145}
146
148{
149 if (odomTracker_)
150 {
151 this->odomTracker_->setWorkingMode(WorkingMode::IDLE);
152 }
153}
154
155} // namespace cl_nav2z
std::optional< geometry_msgs::msg::PoseStamped > goalPose_
std::optional< float > forwardDistance_
void setForwardDistance(float distance_meters)
odom_tracker::OdomTracker * odomTracker_
std::optional< geometry_msgs::msg::Quaternion > forceInitialOrientation
void setGoalCheckerId(std::string goal_checker_id)
const std::string & getReferenceFrame() const
Definition: cp_pose.hpp:77
void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped &pose)
void setWorkingMode(WorkingMode workingMode)
void setStartPoint(const geometry_msgs::msg::PoseStamped &pose)
virtual rclcpp::Node::SharedPtr getNode()
TComponent * getComponent()
virtual std::string getName()=0
std::shared_future< typename GoalHandle::SharedPtr > sendGoal(Goal &goal)