diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c
index b8c4760..c0dc683 100644
--- a/firmware/base/src/main.c
+++ b/firmware/base/src/main.c
@@ -156,11 +156,20 @@ void core2_entry()
data = (data >> 8ll) | ((uint64_t)ch<<56ll);
readNum--;
if(!readNum) {
- if(type == 'w')
+ if(type == 'w'){
wheel_separation = *((double*)&data);
- else
+ } else if(type == 'X'){
+ base_x = *((double*)&data);
+ } else if(type == 'Y'){
+ base_y = *((double*)&data);
+ } else if(type == 'T'){
+ base_theta = *((double*)&data);
+ calib_enc.left_pulse = 0;
+ calib_enc.right_pulse = 0;
+ } else{
wheel_ratio_l = 1 + (*((double *)&data));
wheel_ratio_r = 1 - (*((double *)&data));
+ }
}
}
@@ -173,6 +182,15 @@ void core2_entry()
} else if(cmd == (((uint16_t)'r' << 8) | ';')){
readNum = 8;
type = (cmd >> 8) & 0xFF;
+ } else if(cmd == (((uint16_t)'X' << 8) | ';')){
+ readNum = 8;
+ type = (cmd >> 8) & 0xFF;
+ } else if(cmd == (((uint16_t)'Y' << 8) | ';')){
+ readNum = 8;
+ type = (cmd >> 8) & 0xFF;
+ } else if(cmd == (((uint16_t)'T' << 8) | ';')){
+ readNum = 8;
+ type = (cmd >> 8) & 0xFF;
} else if(cmd == (((uint16_t)'z' << 8) | ';')) {
zero();
} else if(cmd == (((uint16_t)'c' << 8) | ';')) {
diff --git a/mg_bt/behavior_trees/mg_bt.btproj b/mg_bt/behavior_trees/mg_bt.btproj
index f23638b..7863dc6 100644
--- a/mg_bt/behavior_trees/mg_bt.btproj
+++ b/mg_bt/behavior_trees/mg_bt.btproj
@@ -47,6 +47,13 @@
Action server name
+
+
+
+
+
+ Service name
+
Topic name
diff --git a/mg_bt/behavior_trees/tactics.xml b/mg_bt/behavior_trees/tactics.xml
index 1bc7dc7..a77c040 100644
--- a/mg_bt/behavior_trees/tactics.xml
+++ b/mg_bt/behavior_trees/tactics.xml
@@ -27,6 +27,23 @@
+
+
+
+
+
+
+
+
+
@@ -41,6 +58,19 @@
Action server name
+
+
+
+
+
+ Service name
+
diff --git a/mg_bt/src/mg_tree_executor.cpp b/mg_bt/src/mg_tree_executor.cpp
index 7e1712d..e33b43f 100644
--- a/mg_bt/src/mg_tree_executor.cpp
+++ b/mg_bt/src/mg_tree_executor.cpp
@@ -7,6 +7,7 @@
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
+#include "tree_nodes/set_pos.hpp"
#include "tree_nodes/zero.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -31,6 +32,7 @@ namespace mg {
factory.registerNodeType("ZeroNode", node());
factory.registerNodeType("I2CSignal", node());
factory.registerNodeType("SideObstaclePub", node());
+ factory.registerNodeType("SendPose", node());
factory.registerNodeType("TacticChooser");
factory.registerNodeType("CalibWidth", node(), [this]() { return this->position(); });
}
diff --git a/mg_bt/src/tree_nodes/set_pos.hpp b/mg_bt/src/tree_nodes/set_pos.hpp
new file mode 100644
index 0000000..b90a03b
--- /dev/null
+++ b/mg_bt/src/tree_nodes/set_pos.hpp
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "mg_msgs/srv/send_pose2d.hpp"
+
+namespace mg {
+
+ class SendPoseNode : public BT::RosServiceNode {
+ public:
+ SendPoseNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+ BT::RosServiceNode(name, conf, params) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("x"),
+ BT::InputPort("y"),
+ BT::InputPort("angle"),
+ BT::InputPort("isDegree", "True", {}) });
+ }
+
+ bool setRequest(typename Request::SharedPtr& req) override {
+ req->x = getInput("x").value();
+ req->y = getInput("y").value();
+ req->theta = getInput("angle").value();
+ if (getInput("isDegree").value()) {
+ req->theta *= M_PI / 180;
+ }
+ return true;
+ }
+
+ BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
+ return BT::NodeStatus::SUCCESS;
+ }
+ };
+
+}
\ No newline at end of file
diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt
index 0a4fe97..e1b2318 100644
--- a/mg_msgs/CMakeLists.txt
+++ b/mg_msgs/CMakeLists.txt
@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"action/I2c.action"
"srv/CalcPath.srv"
"srv/SendDouble.srv"
+ "srv/SendPose2d.srv"
"srv/I2c.srv"
)
diff --git a/mg_msgs/srv/SendPose2d.srv b/mg_msgs/srv/SendPose2d.srv
new file mode 100644
index 0000000..e23b86e
--- /dev/null
+++ b/mg_msgs/srv/SendPose2d.srv
@@ -0,0 +1,4 @@
+float64 x
+float64 y
+float64 theta
+---
\ No newline at end of file
diff --git a/mg_navigation/include/handlers/dwa_global.hpp b/mg_navigation/include/handlers/dwa_global.hpp
index 4e57ca4..d384825 100644
--- a/mg_navigation/include/handlers/dwa_global.hpp
+++ b/mg_navigation/include/handlers/dwa_global.hpp
@@ -70,8 +70,8 @@ namespace mg {
const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);
- RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
- RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
+ // RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
+ // RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
// score += goal->ornt_mult * (angl - n_angl);
diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp
index 32ecbfb..812a8b8 100644
--- a/mg_odometry/src/mg_odom_publisher.cpp
+++ b/mg_odometry/src/mg_odom_publisher.cpp
@@ -9,6 +9,7 @@
#include
#include "mg_msgs/srv/send_double.hpp"
+#include "mg_msgs/srv/send_pose2d.hpp"
#include "std_srvs/srv/empty.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
@@ -26,6 +27,7 @@
constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
using SendDoubleSrv = mg_msgs::srv::SendDouble;
+using SendPoseSrv = mg_msgs::srv::SendPose2d;
using ZeroSrv = std_srvs::srv::Empty;
class MgOdomPublisher : public rclcpp::Node {
@@ -44,6 +46,8 @@ class MgOdomPublisher : public rclcpp::Node {
"/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
set_ratio_service_ = create_service(
"/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
+ set_pose_service_ = create_service(
+ "/set_pose", std::bind(&MgOdomPublisher::set_pose, this, std::placeholders::_1));
zero_service_
= create_service("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@@ -56,6 +60,7 @@ class MgOdomPublisher : public rclcpp::Node {
std::shared_ptr tf_broadcaster_;
rclcpp::Service::SharedPtr set_width_service_;
rclcpp::Service::SharedPtr set_ratio_service_;
+ rclcpp::Service::SharedPtr set_pose_service_;
rclcpp::Service::SharedPtr zero_service_;
rclcpp::Service::SharedPtr calib_service_;
rclcpp::TimerBase::SharedPtr timer_;
@@ -76,6 +81,31 @@ class MgOdomPublisher : public rclcpp::Node {
}
}
+ void set_pose(const std::shared_ptr request) {
+ RCLCPP_INFO(get_logger(), "Setting pose to: %lf %lf %lf", request->x, request->y, request->theta);
+
+ if (enc_serial_port_.IsOpen()) {
+ union {
+ std::array bytes;
+ double data;
+ } value{};
+ value.data = request->x;
+
+ enc_serial_port_ << "X;";
+ for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+
+ value.data = request->y;
+
+ enc_serial_port_ << "Y;";
+ for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+
+ value.data = request->theta;
+
+ enc_serial_port_ << "T;";
+ for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+ }
+ }
+
void set_ratio(const std::shared_ptr request) {
RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);