adrien.lasserre
/
pkg_lab2
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

final version working

This commit is contained in:
Adrien LASSERRE 2022-11-22 17:28:03 +01:00
parent ec243c7975
commit 9d73b7d914
1 changed files with 39 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/project_file.cpp
View File

@ -4,16 +4,43 @@ Date: 22nd of November 2022
Authors: Adrien Lasserre (adrien.lasserre@ecam.fr) and Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr) Authors: Adrien Lasserre (adrien.lasserre@ecam.fr) and Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
Context: This cpp file is part of the second lab of the Advanced_Robotics course at ECAM. This course is taught by Guillaume Gibert (guillaume.gibert@ecam.fr). Context: This cpp file is part of the second lab of the Advanced_Robotics course at ECAM. This course is taught by Guillaume Gibert (guillaume.gibert@ecam.fr).
The main goal of this file is to create a ROS node publishing on the /cmd_vel topic of a Turtlebot3 educational robot. The main goal of this file is to create a ROS node publishing on the /cmd_vel topic of a Turtlebot3 educational robot. It also subscribes to /scan to detect
the obstacles on the way of the robot.
*/ */
#include <ros/ros.h> #include <ros/ros.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
//max values for lin vel 0.22 and angular vel 2.84 //max values for lin vel 0.22 and angular vel 2.84
bool obstacle_present=false;
void obstacleCallback(const sensor_msgs::LaserScan& message){
float thresh_min=0.12;
float thresh_max=0.25;
// this for-loop constrains the values of the scan using tresh_min and tresh_max
for(int count = 0; count < 15; count++){
if( (message.ranges[count] >=thresh_min && message.ranges[count]<= thresh_max) || (message.ranges[359-count] >=thresh_min && message.ranges[359-count]<= thresh_max)){
obstacle_present=true;
//break;
}
else{
obstacle_present=false;
}
}
}
void walk(geometry_msgs::Twist& twist, float speed_input){ void walk(geometry_msgs::Twist& twist, float speed_input){
//intialisation of the values for the twist messages //intialisation of the values for the twist messages
@ -50,14 +77,21 @@ int main(int argc, char** argv)
ros::NodeHandle nh; ros::NodeHandle nh;
ros::Publisher commanderPublisher = ros::Publisher commanderPublisher =
nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::Rate loopRate(10); ros::Subscriber commanderSubscriber =
nh.subscribe("scan", 1, obstacleCallback);
ros::Rate loopRate(5);
while(ros::ok()){ while(ros::ok()){
geometry_msgs::Twist twist; geometry_msgs::Twist twist;
turn_random(twist, 1); if(obstacle_present==true){
turn_random(twist, 2);
}
else{
walk(twist, 0.1);
}
commanderPublisher.publish(twist); commanderPublisher.publish(twist);
ros::spinOnce(); ros::spinOnce();
loopRate.sleep(); loopRate.sleep();