#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/detail/laser_scan__struct.hpp"
#include "std_msgs/msg/int32.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "geometry_msgs/msg/twist.hpp" // Include Twist message type
using std::placeholders::_1;
using namespace std::chrono_literals;
class LaserVeloControl : public rclcpp::Node
{
public:
LaserVeloControl()
: Node("LaserVelocityController")
{
// Create subscription to LaserScan topic
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", 10, std::bind(&LaserVeloControl::laser_callback, this, _1));
// Create publisher for Twist messages to control velocity
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
// Create timer for periodic callback
timer_ = this->create_wall_timer(
100ms, std::bind(&LaserVeloControl::timer_callback, this));
}
private:
float front_laser = 10.0;
float right_laser = 10.0;
float left_laser = 10.0;
void laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg){
// Update laser scan values
front_laser = msg.ranges[89];
left_laser = msg.ranges[179];
right_laser = msg.ranges[0];
// Log laser scan values
RCLCPP_INFO(this->get_logger(), "Front Laser: %f", front_laser);
RCLCPP_INFO(this->get_logger(), "Left Laser: %f", left_laser);
RCLCPP_INFO(this->get_logger(), "Right Laser: %f", right_laser);
}
void timer_callback(){
auto message = geometry_msgs::msg::Twist(); // Create Twist message
// Determine velocity commands based on laser scan values
if(front_laser < 30.0){
if (front_laser < 1.0 && front_laser > 0.0) {
message.linear.x = 0.0;
message.angular.z = 1.0;
}
else if (right_laser < 1.0) {
message.linear.x = 0.0;
message.angular.z = 1.0;
}
else if (left_laser < 1.0) {
message.linear.x = 0.0;
message.angular.z = -1.0;
}
else {
message.linear.x = 1.0;
message.angular.z = 0.0;
}
}
else{
}
// Publish velocity command
publisher_->publish(message);
}
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char* argv[]){
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<LaserVeloControl>());
rclcpp::shutdown();
return 0;
}
I see that you have moved past this point already. Please let us know if you need help with another issue.
By the way, this problem usually happens if you use a name other than topics_quiz_node in your launch file.