random_walk.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include "geometry_msgs/Twist.h"
5 
6 int main(int argc, char **argv)
7 {
8  // Initial Variables
9  std::string pheeno_name;
10 
11  // Parse inputs
12  CommandLineParser cml_parser(argc, argv);
13 
14  // Parse input arguments for Pheeno name.
15  if (cml_parser["-n"])
16  {
17  std::string pheeno_number = cml_parser("-n");
18  pheeno_name = "/pheeno_" + pheeno_number;
19  std::cout << pheeno_name << std::endl;
20  }
21  else
22  {
23  ROS_ERROR("Need to provide Pheeno number!");
24  }
25 
26  // Initializing ROS node
27  ros::init(argc, argv, "random_walk_node");
28 
29  // Create PheenoRobot object
30  PheenoRobot pheeno = PheenoRobot(pheeno_name);
31 
32  // ROS Rate loop
33  ros::Rate loop_rate(10);
34 
35  // Variables before loop
36  double saved_time = ros::Time::now().toSec();
37  double current_duration;
38  double angular = 0.07;
39  double turn_direction = pheeno.randomTurn(angular);
40  geometry_msgs::Twist cmd_vel_msg;
41 
42  while (ros::ok())
43  {
44  // Find current duration of motion.
45  current_duration = ros::Time::now().toSec() - saved_time;
46 
47  if (current_duration <= 10.0) {
48  cmd_vel_msg.linear.x = 0.0;
49  cmd_vel_msg.angular.z = turn_direction;
50 
51  } else if (current_duration < 20.0) {
52  cmd_vel_msg.linear.x = 0.05;
53  cmd_vel_msg.angular.z = 0.0;
54 
55  } else {
56  // Reset Variables
57  saved_time = ros::Time::now().toSec();
58  turn_direction = pheeno.randomTurn(angular);
59  }
60 
61  // Publish, Spin, and Sleep
62  pheeno.publish(cmd_vel_msg);
63  ros::spinOnce();
64  loop_rate.sleep();
65  }
66 }
void publish(geometry_msgs::Twist velocity)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL bool ok()
bool sleep()
static Time now()
double randomTurn(float angular=0.06)
ROSCPP_DECL void spinOnce()
int main(int argc, char **argv)
Definition: random_walk.cpp:6
#define ROS_ERROR(...)


pheeno_ros
Author(s): Zahi Kakish, Sean Wilson
autogenerated on Mon Jun 10 2019 14:10:48