Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added the ability to switch between cmd_vel control and induvidual motor control

Browse Source
This commit is contained in:
pimpest
2026-02-09 15:08:12 +01:00
parent e00d927a55
commit 17d19e393f
7 changed files with 87 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
toid_odometry/src/odometry.cpp
View File

@@ -23,9 +23,20 @@
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/convert.hpp"
#include "nav_msgs/msg/odometry.hpp"
namespace asio = boost::asio;
const std::array<double, 36> COVARIENCE_MATRIX{
0.0001, .0, 0, 0, 0, 0,
0, 0.0001, 0, 0, 0, 0,
0, .0, 0.0001, 0, 0, 0,
0, .0, 0, 0.0001, 0, 0,
0, .0, 0, 0, 0.0001, 0,
0, .0, 0, 0, 0, 0.0001,
};
class ToidOdomNode : public rclcpp::Node {
using SendDoubleSrv = toid_msgs::srv::SendDouble;
using ZeroSrv = std_srvs::srv::Empty;
@@ -78,6 +89,9 @@ public:
zero_service_ = create_service<ZeroSrv> (
"/zero", std::bind(&ToidOdomNode::zero, this, _1));
odometry_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>(
"/odom", 1);
}
}
@@ -179,13 +193,30 @@ private:
void send_transform(tRespState &state) {
geometry_msgs::msg::TransformStamped t;
nav_msgs::msg::Odometry o;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = this->odometry_frame_id_;
t.child_frame_id = this->base_frame_id_;
o.header = t.header;
o.child_frame_id = t.child_frame_id;
t.transform = make_transform(state.x, state.y, state.theta);
const auto& [x,y,z] = t.transform.translation;
o.pose.pose.position.x = x;
o.pose.pose.position.x = y;
o.pose.pose.position.x = z;
o.pose.pose.orientation = t.transform.rotation;
o.pose.covariance = COVARIENCE_MATRIX;
o.twist.twist.linear.x = state.vl;
o.twist.twist.angular.z = state.vr;
o.twist.covariance = COVARIENCE_MATRIX;
odometry_publisher_->publish(o);
tf_broadcaster_->sendTransform(t);
}
@@ -237,6 +268,7 @@ private:
rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr startpose_listener_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_publisher_;
};
int main(const int argc,const char** argv) {