diff --git a/src/project_file.cpp b/src/project_file.cpp index 378d213..bd1967c 100644 --- a/src/project_file.cpp +++ b/src/project_file.cpp @@ -10,6 +10,46 @@ Context: This cpp file is part of the second lab of the Advanced_Robotics course #include +#include //std::this_thread_sleep_for +#include //std::chrono::seconds + + + +//max values for lin vel 0.22 and angular vel 2.84 + +void walk(geometry_msgs::Twist twist, speed_input){ + + //intialisation of the values for the twist messages + twist.linear.x=0.0; + twist.linear.y=0.0; + twist.linear.z=0.0; + twist.angular.x=0.0; + twist.angular.y=0.0; + twist.angular.z=0.0; + + //use of the speed input + twist.linear.x=speed_input; + +} + +void turn_random(geometry_msgs::Twist twist){ + + //initialisation of the values for the twist messages + twist.linear.x=0.0; + twist.linear.y=0.0; + twist.linear.z=0.0; + twist.angular.x=0.0; + twist.angular.y=0.0; + twist.angular.z=0.0; + + //randomise the angle + int random_angular_speed=rand() % 2; + twist.angular.z=random_angular_speed; + std::this_thread_sleep_for (std::chrono::seconds(2)); + twist.angular.z=0.0; + + +} int main(int argc, char** argv) { @@ -20,10 +60,19 @@ int main(int argc, char** argv) ros::Rate loopRate(10); while(ros::ok()){ - commanderPublisher.publish(); + + geometry_msgs::Twist twist; + + walk(twist, 0.1); + commanderPublisher.publish(twist); + loopRate.sleep(3); + turn_random(twist); + commanderPublisher.publish(twist); + loopRate.sleep(3); + ros::spinOnce(); loopRate.sleep(); } return 0; -} \ No newline at end of file +}