modelec/ros2_roboclaw_driver
1
0
Fork 0
mirror of https://github.com/modelec/ros2_roboclaw_driver.git synced 2026-01-18 16:47:26 +01:00
Code Issues Packages Projects Releases Wiki Activity

Removed odom listener/transorm/broadcaster. Reformatted code

Browse Source
This commit is contained in:
Michael Wimble
2021-09-25 11:37:56 -07:00
parent 2a1b3f2ca0
commit 76a84b033a
6 changed files with 60 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -1,3 +1,4 @@
.vscode/
devel/
logs/
build/

12
CMakeLists.txt
View File

@@ -25,8 +25,6 @@ find_package(nav_msgs)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
set(msg_files
"msg/RoboClawStatus.msg"
@@ -37,10 +35,10 @@ set(srv_files
)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/RoboClawStatus.msg"
"srv/ResetEncoders.srv"
DEPENDENCIES builtin_interfaces std_msgs
)
${msg_files}
${srv_files}
# DEPENDENCIES builtin_interfaces std_msgs
)
add_executable(
ros2_roboclaw_driver_node
@@ -55,8 +53,6 @@ ament_target_dependencies(
geometry_msgs
nav_msgs
std_msgs
tf2_msgs
tf2_ros
)
rosidl_target_interfaces(

2
package.xml
View File

@@ -15,8 +15,6 @@
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>

113
src/motor_driver.cpp
View File

@@ -1,21 +1,23 @@
#include "motor_driver.h"
#include <math.h>
#include <rcutils/logging_macros.h>
#include <stdint.h>
#include <algorithm>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <math.h>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rcutils/logging_macros.h>
#include <string>
#include <stdint.h>
#include <tf2_ros/transform_broadcaster.h>
#include "motor_driver.h"
#include "roboclaw.h"
MotorDriver::MotorDriver()
: Node("motor_driver_node"), device_name_("foo_bar"), wheel_radius_(0.10169), wheel_separation_(0.345)
{
: Node("motor_driver_node"),
device_name_("foo_bar"),
wheel_radius_(0.10169),
wheel_separation_(0.345) {
this->declare_parameter<int>("accel_quad_pulses_per_second", 600);
this->declare_parameter<std::string>("device_name", "roboclaw");
this->declare_parameter<int>("device_port", 123);
@@ -40,50 +42,40 @@ MotorDriver::MotorDriver()
this->declare_parameter<float>("wheel_separation", 0.0);
}
void MotorDriver::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = rclcpp::Node::now();
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "t265";
transformStamped.transform.translation.x = msg->pose.pose.position.x;
transformStamped.transform.translation.y = msg->pose.pose.position.y;
transformStamped.transform.translation.z = 0.0;
transformStamped.transform.rotation.x = 0.0;
transformStamped.transform.rotation.y = 0.0;
transformStamped.transform.rotation.z = msg->pose.pose.orientation.z;
transformStamped.transform.rotation.w = msg->pose.pose.orientation.w;
broadcaster_->sendTransform(transformStamped);
// RCUTILS_LOG_INFO("odom published");
}
void MotorDriver::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const
{
double x_velocity = std::min(std::max((float) msg->linear.x, -max_linear_velocity_), max_linear_velocity_);
double yaw_velocity = std::min(std::max((float) msg->angular.z, -max_angular_velocity_), max_angular_velocity_);
if ((msg->linear.x == 0) && (msg->angular.z == 0))
{
void MotorDriver::cmdVelCallback(
const geometry_msgs::msg::Twist::SharedPtr msg) const {
double x_velocity =
std::min(std::max((float)msg->linear.x, -max_linear_velocity_),
max_linear_velocity_);
double yaw_velocity =
std::min(std::max((float)msg->angular.z, -max_angular_velocity_),
max_angular_velocity_);
if ((msg->linear.x == 0) && (msg->angular.z == 0)) {
roboclaw_->doMixedSpeedDist(0, 0, 0, 0);
}
else if ((fabs(x_velocity) > 0.01) || (fabs(yaw_velocity) > 0.01))
{
const double m1_desired_velocity = x_velocity - (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const double m2_desired_velocity = x_velocity + (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
} else if ((fabs(x_velocity) > 0.01) || (fabs(yaw_velocity) > 0.01)) {
const double m1_desired_velocity =
x_velocity - (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const double m2_desired_velocity =
x_velocity + (yaw_velocity * wheel_separation_ / 2.0) / wheel_radius_;
const int32_t m1_quad_pulses_per_second = m1_desired_velocity * quad_pulses_per_meter_;
const int32_t m2_quad_pulses_per_second = m2_desired_velocity * quad_pulses_per_meter_;
const int32_t m1_max_distance = fabs(m1_quad_pulses_per_second * max_seconds_uncommanded_travel_);
const int32_t m2_max_distance = fabs(m2_quad_pulses_per_second * max_seconds_uncommanded_travel_);
roboclaw_->doMixedSpeedAccelDist(accel_quad_pulses_per_second_, m1_quad_pulses_per_second, m1_max_distance, m2_quad_pulses_per_second, m2_max_distance);
const int32_t m1_quad_pulses_per_second =
m1_desired_velocity * quad_pulses_per_meter_;
const int32_t m2_quad_pulses_per_second =
m2_desired_velocity * quad_pulses_per_meter_;
const int32_t m1_max_distance =
fabs(m1_quad_pulses_per_second * max_seconds_uncommanded_travel_);
const int32_t m2_max_distance =
fabs(m2_quad_pulses_per_second * max_seconds_uncommanded_travel_);
roboclaw_->doMixedSpeedAccelDist(
accel_quad_pulses_per_second_, m1_quad_pulses_per_second,
m1_max_distance, m2_quad_pulses_per_second, m2_max_distance);
}
}
void MotorDriver::onInit(rclcpp::Node::SharedPtr node)
{
void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
node_ = node;
broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
this->get_parameter("accel_quad_pulses_per_second", accel_quad_pulses_per_second_);
this->get_parameter("accel_quad_pulses_per_second",
accel_quad_pulses_per_second_);
this->get_parameter("device_name", device_name_);
this->get_parameter("device_port", device_port_);
this->get_parameter("m1_p", m1_p_);
@@ -99,15 +91,18 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node)
max_angular_velocity_ = -12.34;
this->get_parameter("max_angular_velocity", max_angular_velocity_);
this->get_parameter("max_linear_velocity", max_linear_velocity_);
this->get_parameter("max_seconds_uncommanded_travel", max_seconds_uncommanded_travel_);
this->get_parameter("max_seconds_uncommanded_travel",
max_seconds_uncommanded_travel_);
this->get_parameter("quad_pulses_per_meter", quad_pulses_per_meter_);
this->get_parameter("quad_pulses_per_revolution", quad_pulses_per_revolution_);
this->get_parameter("quad_pulses_per_revolution",
quad_pulses_per_revolution_);
this->get_parameter("vmin", vmin_);
this->get_parameter("vtime", vtime_);
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
RCUTILS_LOG_INFO("accel_quad_pulses_per_second: %d", accel_quad_pulses_per_second_);
RCUTILS_LOG_INFO("accel_quad_pulses_per_second: %d",
accel_quad_pulses_per_second_);
RCUTILS_LOG_INFO("device_name: %s", device_name_.c_str());
RCUTILS_LOG_INFO("device_port: %d", device_port_);
RCUTILS_LOG_INFO("m1_p: %f", m1_p_);
@@ -122,9 +117,11 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node)
RCUTILS_LOG_INFO("m2_max_current: %f", m2_max_current_);
RCUTILS_LOG_INFO("max_angular_velocity: %f", max_angular_velocity_);
RCUTILS_LOG_INFO("max_linear_velocity: %f", max_linear_velocity_);
RCUTILS_LOG_INFO("max_seconds_uncommanded_travel: %f", max_seconds_uncommanded_travel_);
RCUTILS_LOG_INFO("max_seconds_uncommanded_travel: %f",
max_seconds_uncommanded_travel_);
RCUTILS_LOG_INFO("quad_pulses_per_meter: %d", quad_pulses_per_meter_);
RCUTILS_LOG_INFO("quad_pulses_per_revolution: %d", quad_pulses_per_revolution_);
RCUTILS_LOG_INFO("quad_pulses_per_revolution: %d",
quad_pulses_per_revolution_);
RCUTILS_LOG_INFO("vmin: %d", vmin_);
RCUTILS_LOG_INFO("vtime: %d", vtime_);
RCUTILS_LOG_INFO("wheel_radius: %f", wheel_radius_);
@@ -133,7 +130,8 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node)
RoboClaw::TPIDQ m1Pid = {m1_p_, m1_i_, m1_d_, m1_qpps_, m1_max_current_};
RoboClaw::TPIDQ m2Pid = {m2_p_, m2_i_, m2_d_, m2_qpps_, m2_max_current_};
roboclaw_ = new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_, device_name_.c_str(), device_port_, vmin_, vtime_);
roboclaw_ = new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_,
device_name_.c_str(), device_port_, vmin_, vtime_);
RCUTILS_LOG_INFO("Main battery: %f", roboclaw_->getMainBatteryLevel());
rclcpp::QoS bestEffortQos(10);
@@ -142,15 +140,12 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node)
bestEffortQos.durability_volatile();
cmdVelSub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", bestEffortQos, std::bind(&MotorDriver::cmdVelCallback, this, std::placeholders::_1));
odomSub_ = node->create_subscription<nav_msgs::msg::Odometry>(
"/odom", bestEffortQos, std::bind(&MotorDriver::odomCallback, this, std::placeholders::_1));
"/cmd_vel", bestEffortQos,
std::bind(&MotorDriver::cmdVelCallback, this, std::placeholders::_1));
}
MotorDriver &MotorDriver::singleton()
{
if (!g_singleton)
{
MotorDriver &MotorDriver::singleton() {
if (!g_singleton) {
g_singleton = new MotorDriver();
}

6
src/motor_driver.h
View File

@@ -1,10 +1,8 @@
#pragma once
#include <stdint.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
@@ -23,10 +21,7 @@ class MotorDriver : public rclcpp::Node {
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) const;
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
uint32_t accel_quad_pulses_per_second_;
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
std::string device_name_;
uint8_t device_port_;
float m1_p_;
@@ -53,7 +48,6 @@ class MotorDriver : public rclcpp::Node {
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdVelSub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
static MotorDriver *g_singleton;
};

2
src/motor_driver_node.cpp
View File

@@ -8,7 +8,7 @@
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("ros2_roboclaw_driver");
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("ros2_roboclaw_driver_node");
MotorDriver &motorDriver = MotorDriver::singleton();
motorDriver.onInit(node);