From 7cab24604f5900532d983d4ae9ae397999381bb3 Mon Sep 17 00:00:00 2001 From: ijnek Date: Sat, 2 Sep 2023 12:07:04 +0000 Subject: [PATCH 1/2] add x,y,theta arguments Signed-off-by: ijnek --- include/rcss3d_nao/rcss3d_nao_node.hpp | 1 + src/rcss3d_nao_node.cpp | 15 +++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/include/rcss3d_nao/rcss3d_nao_node.hpp b/include/rcss3d_nao/rcss3d_nao_node.hpp index 2e6f25f..4ffe6b7 100644 --- a/include/rcss3d_nao/rcss3d_nao_node.hpp +++ b/include/rcss3d_nao/rcss3d_nao_node.hpp @@ -74,6 +74,7 @@ class Rcss3dNaoNode : public rclcpp::Node rclcpp::Subscription::SharedPtr beamSub; void perceptCallback(const rcss3d_agent_msgs::msg::Percept & percept); + void beamToInitialPose(double x, double y, double theta); }; } // namespace rcss3d_nao diff --git a/src/rcss3d_nao_node.cpp b/src/rcss3d_nao_node.cpp index 7af978b..d1389c7 100644 --- a/src/rcss3d_nao_node.cpp +++ b/src/rcss3d_nao_node.cpp @@ -37,6 +37,9 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options) int rcss3d_port = this->declare_parameter("rcss3d/port", 3100); std::string team = this->declare_parameter("team", "Anonymous"); int unum = this->declare_parameter("unum", 0); + double x = this->declare_parameter("x", 0.0); + double y = this->declare_parameter("y", 0.0); + double theta = this->declare_parameter("theta", 0.0); // Create Rcss3dAgent params = std::make_unique(model, rcss3d_host, rcss3d_port, team, unum); @@ -80,6 +83,9 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options) [this](rcss3d_agent_msgs::msg::Beam::SharedPtr cmd) { rcss3dAgent->sendBeam(*cmd); }); + + // Beam robot + beamToInitialPose(x, y, theta); } Rcss3dNaoNode::~Rcss3dNaoNode() @@ -172,6 +178,15 @@ void Rcss3dNaoNode::perceptCallback(const rcss3d_agent_msgs::msg::Percept & perc } } +void Rcss3dNaoNode::beamToInitialPose(double x, double y, double theta) +{ + rcss3d_agent_msgs::msg::Beam beam; + beam.x = x; + beam.y = y; + beam.rot = theta; + rcss3dAgent->sendBeam(beam); +} + } // namespace rcss3d_nao #include "rclcpp_components/register_node_macro.hpp" From 159dd774c848cd2bda067fc489af32fbb9fd501a Mon Sep 17 00:00:00 2001 From: ijnek Date: Sat, 2 Sep 2023 12:10:06 +0000 Subject: [PATCH 2/2] alphabetical order Signed-off-by: ijnek --- src/rcss3d_nao_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rcss3d_nao_node.cpp b/src/rcss3d_nao_node.cpp index d1389c7..7341e8d 100644 --- a/src/rcss3d_nao_node.cpp +++ b/src/rcss3d_nao_node.cpp @@ -36,10 +36,10 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options) std::string rcss3d_host = this->declare_parameter("rcss3d/host", "127.0.0.1"); int rcss3d_port = this->declare_parameter("rcss3d/port", 3100); std::string team = this->declare_parameter("team", "Anonymous"); + double theta = this->declare_parameter("theta", 0.0); int unum = this->declare_parameter("unum", 0); double x = this->declare_parameter("x", 0.0); double y = this->declare_parameter("y", 0.0); - double theta = this->declare_parameter("theta", 0.0); // Create Rcss3dAgent params = std::make_unique(model, rcss3d_host, rcss3d_port, team, unum);