Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added the ability to set postion

Browse Source
This commit is contained in:
Pimpest
2025-05-08 02:25:34 +02:00
parent 07cc056853
commit da3d1d9d15
9 changed files with 131 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
firmware/base/src/main.c
View File

@ -156,13 +156,22 @@ 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));
}
}
}
if(cmd == (((uint16_t)'g' << 8) | ';')) {
printf("%lf %lf %lf\n", base_x, base_y, base_theta);
@ -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) | ';')) {

7
mg_bt/behavior_trees/mg_bt.btproj
View File

@ -47,6 +47,13 @@
<input_port name="tolerance" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="angle" type="double"/>
<input_port name="isDegree" type="bool" default="true"/>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic" type="int"/>
<input_port name="topic_name" type="std::string" default="__default__placeholder__">Topic name</input_port>

30
mg_bt/behavior_trees/tactics.xml
View File

@ -27,6 +27,23 @@
<AlwaysFailure/>
</BehaviorTree>
<BehaviorTree ID="testbed">
<Sequence>
<SendPose x="1.0"
y="1.0"
angle="90"
isDegree="true"
service_name="/set_pose"/>
<Delay delay_msec="5000">
<SendPose x="2.0"
y="0.5"
angle="-1"
isDegree="false"
service_name="/set_pose"/>
</Delay>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="I2CSignal">
@ -41,6 +58,19 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="angle"
type="double"/>
<input_port name="isDegree"
default="true"
type="bool"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
type="int"/>

2
mg_bt/src/mg_tree_executor.cpp
View File

@ -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<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

35
mg_bt/src/tree_nodes/set_pos.hpp Normal file
View File

@ -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<mg_msgs::srv::SendPose2d> {
public:
SendPoseNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosServiceNode<mg_msgs::srv::SendPose2d>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x"),
BT::InputPort<double>("y"),
BT::InputPort<double>("angle"),
BT::InputPort<bool>("isDegree", "True", {}) });
}
bool setRequest(typename Request::SharedPtr& req) override {
req->x = getInput<double>("x").value();
req->y = getInput<double>("y").value();
req->theta = getInput<double>("angle").value();
if (getInput<bool>("isDegree").value()) {
req->theta *= M_PI / 180;
}
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
return BT::NodeStatus::SUCCESS;
}
};
}

1
mg_msgs/CMakeLists.txt
View File

@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"action/I2c.action"
"srv/CalcPath.srv"
"srv/SendDouble.srv"
"srv/SendPose2d.srv"
"srv/I2c.srv"
)

4
mg_msgs/srv/SendPose2d.srv Normal file
View File

@ -0,0 +1,4 @@
float64 x
float64 y
float64 theta
---

4
mg_navigation/include/handlers/dwa_global.hpp
View File

@ -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);

30
mg_odometry/src/mg_odom_publisher.cpp
View File

@ -9,6 +9,7 @@
#include <sys/types.h>
#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<SendDoubleSrv>(
"/set_ratio", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
set_pose_service_ = create_service<SendPoseSrv>(
"/set_pose", std::bind(&MgOdomPublisher::set_pose, this, std::placeholders::_1));
zero_service_
= create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
@ -56,6 +60,7 @@ class MgOdomPublisher : public rclcpp::Node {
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_width_service_;
rclcpp::Service<SendDoubleSrv>::SharedPtr set_ratio_service_;
rclcpp::Service<SendPoseSrv>::SharedPtr set_pose_service_;
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::Service<ZeroSrv>::SharedPtr calib_service_;
rclcpp::TimerBase::SharedPtr timer_;
@ -76,6 +81,31 @@ class MgOdomPublisher : public rclcpp::Node {
}
}
void set_pose(const std::shared_ptr<SendPoseSrv::Request> 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<u_char, sizeof(double)> 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<SendDoubleSrv::Request> request) {
RCLCPP_INFO(get_logger(), "Setting wheel ratio to: %lf", request->data);