random_walk.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include "geometry_msgs/Twist.h"
00003 #include "command_line_parser.h"
00004 #include "pheeno_robot.h"
00005 
00006 int main(int argc, char **argv)
00007 {
00008   // Initial Variables
00009   std::string pheeno_name;
00010 
00011   // Parse inputs
00012   CommandLineParser cml_parser(argc, argv);
00013 
00014   // Parse input arguments for Pheeno name.
00015   if (cml_parser["-n"])
00016   {
00017     std::string pheeno_number = cml_parser("-n");
00018     pheeno_name = "/pheeno_" + pheeno_number;
00019     std::cout << pheeno_name << std::endl;
00020   }
00021   else
00022   {
00023     ROS_ERROR("Need to provide Pheeno number!");
00024   }
00025 
00026   // Initializing ROS node
00027   ros::init(argc, argv, "random_walk_node");
00028 
00029   // Create PheenoRobot object
00030   PheenoRobot pheeno = PheenoRobot(pheeno_name);
00031 
00032   // ROS Rate loop
00033   ros::Rate loop_rate(10);
00034 
00035   // Variables before loop
00036   double saved_time = ros::Time::now().toSec();
00037   double current_duration;
00038   double angular = 0.07;
00039   double turn_direction = pheeno.randomTurn(angular);
00040   geometry_msgs::Twist cmd_vel_msg;
00041 
00042   while (ros::ok())
00043   {
00044     // Find current duration of motion.
00045     current_duration = ros::Time::now().toSec() - saved_time;
00046 
00047     if (current_duration <= 10.0) {
00048       cmd_vel_msg.linear.x = 0.0;
00049       cmd_vel_msg.angular.z = turn_direction;
00050 
00051     } else if (current_duration < 20.0) {
00052       cmd_vel_msg.linear.x = 0.05;
00053       cmd_vel_msg.angular.z = 0.0;
00054 
00055     } else {
00056       // Reset Variables
00057       saved_time = ros::Time::now().toSec();
00058       turn_direction = pheeno.randomTurn(angular);
00059     }
00060 
00061     // Publish, Spin, and Sleep
00062     pheeno.publish(cmd_vel_msg);
00063     ros::spinOnce();
00064     loop_rate.sleep();
00065   }
00066 }


pheeno_ros_sim
Author(s): Zahi Kakish
autogenerated on Thu Jun 6 2019 19:59:41