My robot moves forward, but does not turn
Hello, I have managed to make my robot move, but when it reaches a wall, it stops a little before and does not turn. Instead, it makes braking movement but nothing more.
#include "geometry_msgs/Twist.h"
#include "ros/init.h"
#include "ros/node_handle.h"
#include "ros/publisher.h"
#include <algorithm>
#include <array>
#include <boost/type_traits/integral_constant.hpp>
#include <float.h>
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <stdint.h>
float minF, minI, minD;
float matrizD[250];
float matrizF[220];
float matrizI[250];
void funct(const sensor_msgs::LaserScan::ConstPtr &msg) {
int j = 0;
float val;
for (int i = 0; i < 720; i++) {
if (std::isinf(msg->ranges[i])) {
val = msg->range_max;
} else {
val = msg->ranges[i];
val = (float)val;
}
if (i < 120) {
matrizD[i] = val;
}
if ((i >= 120) && (i < 660)) {
j = i - 120;
matrizF[j] = val;
}
if ((i >= 600) && (i < 720)) {
j = i - 600;
matrizD[j] = val;
}
}
minF = float(*std::min_element(std::begin(matrizF), std::end(matrizF)));
minD = float(*std::min_element(std::begin(matrizD), std::end(matrizD)));
minI = float(*std::min_element(std::begin(matrizI), std::end(matrizI)));
std::printf("%f - %f - %f",minI,minF, minD);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "topics_quiz_node");
ros::NodeHandle robot;
ros::Publisher vel_pub =
robot.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
ros::Subscriber laser_sub = robot.subscribe("/kobuki/laser/scan", 10, funct);
while (true) {
geometry_msgs::Twist msg;
sensor_msgs::LaserScan lsr;
msg.linear.x = 0;
msg.angular.x = 0;
msg.linear.y = 0;
msg.linear.z = 0;
msg.angular.y = 0;
msg.angular.z = 0;
std::cout << "Mínimo Iz: " << minI << std::endl;
std::cout << "Mínimo Fr: " << minF << std::endl;
std::cout << "Mínimo De: " << minD << std::endl;
if (minF >= 0.7) {
// Avanza al frente
msg.linear.x = 0.1;
msg.angular.x = 0;
} else {
msg.linear.x = 0;
msg.angular.x = 10.0;
}
if ((minI < 0.5)&& (minF <= 0.5)){
// Gira a la izquierda
msg.linear.x = 0;
msg.angular.x = -10.0;
}
if ((minD < 0.5)&& (minF <= 0.5)) {
// Gira a la derecha
msg.linear.x = 0;
msg.angular.x = 10.0;
}
ros::spinOnce();
vel_pub.publish(msg);
}
return 0;
}
I must have something wrong. It stops and does nothing. It doesn't crash, but it doesn't move, either. It doesn't even turn!
I program the robot on the www.theconstructsim.com platform.
Thanks a lot
Asked by Antonio Avezon on 2022-08-30 17:36:15 UTC
Answers
You have published your angular velocity in msg.angular.x it should be msg.angular.z, Please try that it should work.
Asked by tomarRobin on 2022-08-30 18:54:52 UTC
Comments