Skip to content

Commit

Permalink
fix assisted teleop test
Browse files Browse the repository at this point in the history
Signed-off-by: Steve Macenski <[email protected]>
  • Loading branch information
SteveMacenski committed Dec 3, 2024
1 parent 4a6c6a8 commit df4d394
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -54,13 +54,13 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester()
node_->create_publisher<std_msgs::msg::Empty>("preempt_teleop", 10);

cmd_vel_pub_ =
node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_teleop", 10);
node_->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel_teleop", 10);

subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1));

filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
"cmd_vel",
rclcpp::SystemDefaultsQoS(),
std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1));
Expand Down Expand Up @@ -167,9 +167,9 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
counter_ = 0;
auto start_time = std::chrono::system_clock::now();
while (rclcpp::ok()) {
geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist();
cmd_vel.linear.x = lin_vel;
cmd_vel.angular.z = ang_vel;
geometry_msgs::msg::TwistStamped cmd_vel = geometry_msgs::msg::TwistStamped();
cmd_vel.twist.linear.x = lin_vel;
cmd_vel.twist.angular.z = ang_vel;
cmd_vel_pub_->publish(cmd_vel);

if (counter_ > 1) {
Expand Down Expand Up @@ -265,9 +265,9 @@ void AssistedTeleopBehaviorTester::amclPoseCallback(
}

void AssistedTeleopBehaviorTester::filteredVelCallback(
geometry_msgs::msg::Twist::SharedPtr msg)
geometry_msgs::msg::TwistStamped::SharedPtr msg)
{
if (msg->linear.x == 0.0f) {
if (msg->twist.linear.x == 0.0f) {
counter_++;
} else {
counter_ = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
#include "nav2_msgs/action/assisted_teleop.hpp"
Expand Down Expand Up @@ -68,7 +68,7 @@ class AssistedTeleopBehaviorTester

void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);

void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg);
void filteredVelCallback(geometry_msgs::msg::TwistStamped::SharedPtr msg);

unsigned int counter_;
bool is_active_;
Expand All @@ -83,11 +83,11 @@ class AssistedTeleopBehaviorTester
// Publishers
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr preempt_pub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_pub_;

// Subscribers
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr filtered_vel_sub_;
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr filtered_vel_sub_;

// Action client to call AssistedTeleop action
rclcpp_action::Client<AssistedTeleop>::SharedPtr client_ptr_;
Expand Down

0 comments on commit df4d394

Please sign in to comment.