34 auto p = geometry_msgs::msg::Point();
44 goalYaw = tf2::getYaw(pose.orientation);
49 RCLCPP_INFO(
getLogger(),
"Entering Navigate Global position");
50 RCLCPP_INFO(
getLogger(),
"Component requirements completed");
56 plannerSwitcher->setDefaultPlanners();
62 odomTracker->pushPath(pathname);
63 odomTracker->setStartPoint(pose);
64 odomTracker->setWorkingMode(WorkingMode::RECORD_PATH);
76 RCLCPP_INFO(
getLogger(),
"Sending Goal to MoveBase");
78 goal.pose.header.frame_id = referenceFrame;
79 goal.pose.header.stamp =
getNode()->now();
84 goal.pose.pose.orientation = tf2::toMsg(q);
96 RCLCPP_INFO(
getLogger(),
"Exiting move goal Action Client");
cl_nav2z::ClNav2Z * moveBaseClient_
void setGoal(const geometry_msgs::msg::Pose &pose)
geometry_msgs::msg::Point goalPosition
virtual void onEntry() override
CbNavigateGlobalPosition()
void setGoalCheckerId(std::string goal_checker_id)
const std::string & getReferenceFrame() const
ISmaccState * getCurrentState()
std::string getName() const
virtual rclcpp::Logger getLogger()
virtual rclcpp::Node::SharedPtr getNode()
ISmaccStateMachine * getStateMachine()
TComponent * getComponent()
void setGlobalSMData(std::string name, T value)
virtual std::string getName()=0
std::shared_future< typename GoalHandle::SharedPtr > sendGoal(Goal &goal)
typename ActionClient::Goal Goal