Skip to content

Commit

Permalink
Merge pull request #1 from pixel-robotics/fix-topics
Browse files Browse the repository at this point in the history 
Fix topics
  • Loading branch information
Tony Najjar committed May 5, 2022
2 parents fd6e001 + 79cb822 commit 351f0d4
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 102 deletions.
23 changes: 7 additions & 16 deletions tricycle_controller/include/tricycle_controller/tricycle_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface

class TricycleController : public controller_interface::ControllerInterface
{
using Twist = geometry_msgs::msg::TwistStamped;
using Twist = geometry_msgs::msg::Twist;
using TwistStamped = geometry_msgs::msg::TwistStamped;
using AckermannDrive = ackermann_msgs::msg::AckermannDrive;

public:
Expand Down Expand Up @@ -104,9 +105,7 @@ class TricycleController : public controller_interface::ControllerInterface
CallbackReturn get_steering(
const std::string & steering_joint_name, std::vector<SteeringHandle> & joint);
double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase);
int sgn(double num);
std::tuple<double, double> process_twist_command(double linear_command, double angular_command);
controller_interface::return_type send_commands(const AckermannDrive::SharedPtr msg);

std::string traction_joint_name_;
std::string steering_joint_name_;
Expand All @@ -125,13 +124,14 @@ class TricycleController : public controller_interface::ControllerInterface
{
bool open_loop = false;
bool enable_odom_tf = false;
bool odom_only_twist = false; // for doing the pose integration in seperate node
bool odom_only_twist = false; // for doing the pose integration in seperate node
std::string base_frame_id = "base_link";
std::string odom_frame_id = "odom";
std::array<double, 6> pose_covariance_diagonal;
std::array<double, 6> twist_covariance_diagonal;
} odom_params_;

bool publish_ackermann_command_ = false;
std::shared_ptr<rclcpp::Publisher<AckermannDrive>> ackermann_command_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<AckermannDrive>>
realtime_ackermann_command_publisher_ = nullptr;
Expand All @@ -151,27 +151,18 @@ class TricycleController : public controller_interface::ControllerInterface
std::chrono::milliseconds cmd_vel_timeout_{500};

bool subscriber_is_active_ = false;
rclcpp::Subscription<AckermannDrive>::SharedPtr validated_command_subscriber_ = nullptr;
rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
velocity_command_unstamped_subscriber_ = nullptr;

realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{nullptr};
realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};

std::queue<Twist> previous_commands_; // last two commands
std::queue<AckermannDrive> previous_ackermann_command_; // last two steering angles

bool is_turning_on_spot_ = false;
std::queue<TwistStamped> previous_commands_; // last two commands

// speed limiters
SpeedLimiter limiter_linear_;
SpeedLimiter limiter_angular_;

bool publish_limited_velocity_ = false;
std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ =
nullptr;

rclcpp::Time previous_update_timestamp_{0};

// publish rate limiter
Expand Down
131 changes: 45 additions & 86 deletions tricycle_controller/src/tricycle_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,15 +31,10 @@

namespace
{
// TODO: Find out how to remap these topics from launch files and keep defaults
constexpr auto DEFAULT_COMMAND_TOPIC = "cmd_vel";
constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC =
"cmd_vel_smoothed";
constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "cmd_vel_out";
constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "ackermann";
constexpr auto DEFAULT_VALIDATED_ACKERMANN_TOPIC = "ackermann";
constexpr auto DEFAULT_ODOMETRY_TOPIC = "odom";
constexpr auto DEFAULT_TRANSFORM_TOPIC = "tf";
constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel";
constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann";
constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
} // namespace

namespace tricycle_controller
Expand Down Expand Up @@ -73,7 +68,7 @@ CallbackReturn TricycleController::on_init()
auto_declare<bool>("odom_only_twist", odom_params_.odom_only_twist);

auto_declare<double>("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0);
auto_declare<bool>("publish_limited_velocity", publish_limited_velocity_);
auto_declare<bool>("publish_ackermann_command", publish_ackermann_command_);
auto_declare<int>("velocity_rolling_window_size", 10);
auto_declare<bool>("use_stamped_vel", use_stamped_vel_);

Expand Down Expand Up @@ -133,7 +128,7 @@ controller_interface::return_type TricycleController::update(
->get_clock()
->now(); // original is current_time = time; time is always sim time when using gazebo_ros2_control. This is a hack to use system time instead.

std::shared_ptr<Twist> last_command_msg;
std::shared_ptr<TwistStamped> last_command_msg;
received_velocity_msg_ptr_.get(last_command_msg);
if (last_command_msg == nullptr)
{
Expand All @@ -151,7 +146,7 @@ controller_interface::return_type TricycleController::update(

// command may be limited further by SpeedLimit,
// without affecting the stored twist command
Twist command = *last_command_msg;
TwistStamped command = *last_command_msg;
double & linear_command = command.twist.linear.x;
double & angular_command = command.twist.angular.z;

Expand Down Expand Up @@ -223,34 +218,21 @@ controller_interface::return_type TricycleController::update(
previous_commands_.pop();
previous_commands_.emplace(command);

// Publish limited velocity
if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock())
{
auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
limited_velocity_command.header.stamp = current_time;
limited_velocity_command.twist = command.twist;
realtime_limited_velocity_publisher_->unlockAndPublish();
}

// Compute wheel velocity and angle
auto [alpha, Ws] = process_twist_command(linear_command, angular_command);

// Publish tricycle command
if (realtime_ackermann_command_publisher_->trylock())
// Publish ackermann command
if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock())
{
auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_;
realtime_ackermann_command.speed = Ws;
realtime_ackermann_command.speed =
Ws; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s)
realtime_ackermann_command.steering_angle = alpha;
realtime_ackermann_command_publisher_->unlockAndPublish();
}
return controller_interface::return_type::OK;
}

controller_interface::return_type TricycleController::send_commands(
const AckermannDrive::SharedPtr msg)
{
traction_joint_[0].velocity_command.get().set_value(msg->speed);
steering_joint_[0].position_command.get().set_value(msg->steering_angle);
traction_joint_[0].velocity_command.get().set_value(Ws);
steering_joint_[0].position_command.get().set_value(alpha);
return controller_interface::return_type::OK;
}

Expand Down Expand Up @@ -296,7 +278,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &

cmd_vel_timeout_ = std::chrono::milliseconds{
static_cast<int>(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)};
publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool();
publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool();
use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool();

try
Expand Down Expand Up @@ -340,42 +322,29 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
return CallbackReturn::ERROR;
}

if (publish_limited_velocity_)
{
limited_velocity_publisher_ =
get_node()->create_publisher<Twist>(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
realtime_limited_velocity_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<Twist>>(limited_velocity_publisher_);
}

const Twist empty_twist;
const AckermannDrive empty_ackermann_command;
received_velocity_msg_ptr_.set(std::make_shared<Twist>(empty_twist));
const TwistStamped empty_twist;
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>(empty_twist));

// Fill last two commands with default constructed commands
previous_commands_.emplace(empty_twist);
previous_commands_.emplace(empty_twist);
previous_ackermann_command_.emplace(empty_ackermann_command);
previous_ackermann_command_.emplace(empty_ackermann_command);

// initialize simple command publisher
ackermann_command_publisher_ = get_node()->create_publisher<AckermannDrive>(
DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
realtime_ackermann_command_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<AckermannDrive>>(
ackermann_command_publisher_);

// initialize validated simple command subscriber
validated_command_subscriber_ = get_node()->create_subscription<AckermannDrive>(
DEFAULT_VALIDATED_ACKERMANN_TOPIC, rclcpp::SystemDefaultsQoS(),
std::bind(&TricycleController::send_commands, this, std::placeholders::_1));
// initialize ackermann command publisher
if (publish_ackermann_command_)
{
ackermann_command_publisher_ = get_node()->create_publisher<AckermannDrive>(
DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
realtime_ackermann_command_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<AckermannDrive>>(
ackermann_command_publisher_);
}

// initialize command subscriber
if (use_stamped_vel_)
{
velocity_command_subscriber_ = get_node()->create_subscription<Twist>(
velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<Twist> msg) -> void
[this](const std::shared_ptr<TwistStamped> msg) -> void
{
if (!subscriber_is_active_)
{
Expand All @@ -396,24 +365,23 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
}
else
{
velocity_command_unstamped_subscriber_ =
get_node()->create_subscription<geometry_msgs::msg::Twist>(
DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) -> void
velocity_command_unstamped_subscriber_ = get_node()->create_subscription<Twist>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<Twist> msg) -> void
{
if (!subscriber_is_active_)
{
if (!subscriber_is_active_)
{
RCLCPP_WARN(
get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
return;
}

// Write fake header in the stored stamped command
std::shared_ptr<Twist> twist_stamped;
received_velocity_msg_ptr_.get(twist_stamped);
twist_stamped->twist = *msg;
twist_stamped->header.stamp = get_node()->get_clock()->now();
});
RCLCPP_WARN(
get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
return;
}

// Write fake header in the stored stamped command
std::shared_ptr<TwistStamped> twist_stamped;
received_velocity_msg_ptr_.get(twist_stamped);
twist_stamped->twist = *msg;
twist_stamped->header.stamp = get_node()->get_clock()->now();
});
}

// initialize odometry publisher and messasge
Expand Down Expand Up @@ -504,7 +472,7 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &)
return CallbackReturn::ERROR;
}

received_velocity_msg_ptr_.set(std::make_shared<Twist>());
received_velocity_msg_ptr_.set(std::make_shared<TwistStamped>());
return CallbackReturn::SUCCESS;
}

Expand All @@ -522,10 +490,8 @@ bool TricycleController::reset()
odometry_.resetOdometry();

// release the old queue
std::queue<Twist> empty_twist;
std::queue<TwistStamped> empty_twist;
std::swap(previous_commands_, empty_twist);
std::queue<AckermannDrive> empty_ackermann_command;
std::swap(previous_ackermann_command_, empty_ackermann_command);

// TODO: clear handles
// traction_joint_.clear();
Expand Down Expand Up @@ -641,13 +607,6 @@ double TricycleController::convert_trans_rot_vel_to_steering_angle(
return std::atan(theta_dot / (Vx * wheelbase));
}

int TricycleController::sgn(double num)
{
if (num < 0) return -1;
if (num > 0) return 1;
return 0;
}

std::tuple<double, double> TricycleController::process_twist_command(double Vx, double theta_dot)
{
// using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf
Expand Down

0 comments on commit 351f0d4

Please sign in to comment.