18using namespace std::chrono_literals;
25 using SmaccState::SmaccState;
40 configure_orthogonal<OrNavigation, CbWaitTransform>(
"odom",
"map", rclcpp::Duration(30000
s));
70 geometry_msgs::msg::PoseWithCovarianceStamped initialposemsg;
74 initialposemsg.header.stamp =
getNode()->now();
75 initialposemsg.header.frame_id =
"map";
77 initialposemsg.pose.pose.position.x = 3.415412425994873;
78 initialposemsg.pose.pose.position.y = 2.0;
79 initialposemsg.pose.pose.position.z = 0;
81 initialposemsg.pose.pose.orientation.x = 0;
82 initialposemsg.pose.pose.orientation.y = 0;
83 initialposemsg.pose.pose.orientation.z = 1;
84 initialposemsg.pose.pose.orientation.w = 0;
89 initialposemsg.pose.covariance = {
90 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
91 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
92 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891909122467};
void setInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped &initialpose)
rclcpp::Node::SharedPtr & getNode()
static void staticConfigure()
mpl::list< Transition< EvCbSuccess< CbWaitTransform, OrNavigation >, StStartNavigation, SUCCESS > > reactions
void sendInitialPoseEstimation()