four_wheel_steering.cpp
Go to the documentation of this file.
1 // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
2 
3 // ROS
4 #include <ros/ros.h>
5 #include <rosgraph_msgs/Clock.h>
6 
7 // ros_control
8 #include <controller_manager/controller_manager.h>
9 
10 #include "four_wheel_steering.h"
11 
12 int main(int argc, char **argv)
13 {
14  ros::init(argc, argv, "four_wheel_steering");
15  ros::NodeHandle nh;
16 
17  // This should be set in launch files
18  // as well
19  nh.setParam("/use_sim_time", true);
20 
21  FourWheelSteering robot;
22  ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
23  controller_manager::ControllerManager cm(&robot, nh);
24 
25  ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
26 
28  spinner.start();
29 
30  boost::chrono::system_clock::time_point begin = boost::chrono::system_clock::now();
31  boost::chrono::system_clock::time_point end = boost::chrono::system_clock::now();
32 
33  ros::Time internal_time(0);
34  const ros::Duration dt = robot.getPeriod();
35  double elapsed_secs = 0;
36 
37  while(ros::ok())
38  {
39  begin = boost::chrono::system_clock::now();
40 
41  robot.read();
42  cm.update(internal_time, dt);
43  robot.write();
44 
45  end = boost::chrono::system_clock::now();
46 
47  elapsed_secs = boost::chrono::duration_cast<boost::chrono::duration<double> >((end - begin)).count();
48 
49  if (dt.toSec() - elapsed_secs < 0.0)
50  {
52  0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
53  }
54  else
55  {
56  ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
57  usleep((dt.toSec() - elapsed_secs) * 1e6);
58  }
59 
60  rosgraph_msgs::Clock clock;
61  clock.clock = ros::Time(internal_time);
62  clock_publisher.publish(clock);
63  internal_time += dt;
64  }
65  spinner.stop();
66 
67  return 0;
68 }
#define ROS_DEBUG_STREAM_THROTTLE(rate, args)
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
ros::Duration getPeriod() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
#define ROS_WARN_STREAM_THROTTLE(rate, args)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Apr 18 2020 03:58:13