Merge pull request #21 from ros-sports/ijnek-add-x-y-theta-arguments

add x,y,theta arguments
ros-sports · Sep 2, 2023 · 8e8727f · 8e8727f
2 parents a0913ac + 159dd77
commit 8e8727f
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 0 deletions.
diff --git 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<rcss3d_agent_msgs::msg::Beam>::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
@@ -36,7 +36,10 @@ Rcss3dNaoNode::Rcss3dNaoNode(const rclcpp::NodeOptions & options)
   std::string rcss3d_host = this->declare_parameter<std::string>("rcss3d/host", "127.0.0.1");
   int rcss3d_port = this->declare_parameter<int>("rcss3d/port", 3100);
   std::string team = this->declare_parameter<std::string>("team", "Anonymous");
+  double theta = this->declare_parameter<double>("theta", 0.0);
   int unum = this->declare_parameter<int>("unum", 0);
+  double x = this->declare_parameter<double>("x", 0.0);
+  double y = this->declare_parameter<double>("y", 0.0);
 
   // Create Rcss3dAgent
   params = std::make_unique<rcss3d_agent::Params>(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"