skidsteerbot.cpp
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
28 // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
29 
30 // ROS
31 #include <ros/ros.h>
32 #include <rosgraph_msgs/Clock.h>
33 
34 // ros_control
35 #include <controller_manager/controller_manager.h>
36 
37 #include "diffbot.h"
38 
39 int main(int argc, char **argv)
40 {
41  ros::init(argc, argv, "skidsteerbot");
42  ros::NodeHandle nh;
43 
44  // This should be set in launch files
45  // as well
46  nh.setParam("/use_sim_time", true);
47 
48  Diffbot<6> robot;
49  ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
50  controller_manager::ControllerManager cm(&robot, nh);
51 
52  ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
53 
54  //ros::Rate rate(1.0 / robot.getPeriod().toSec());
56  spinner.start();
57 
58  boost::chrono::system_clock::time_point begin = boost::chrono::system_clock::now();
59  boost::chrono::system_clock::time_point end = boost::chrono::system_clock::now();
60 
61  ros::Time internal_time(0);
62  const ros::Duration dt = robot.getPeriod();
63  double elapsed_secs = 0;
64 
65  while(ros::ok())
66  {
67  begin = boost::chrono::system_clock::now();
68 
69  robot.read();
70  cm.update(internal_time, dt);
71  robot.write();
72 
73  end = boost::chrono::system_clock::now();
74 
75  elapsed_secs = boost::chrono::duration_cast<boost::chrono::duration<double> >((end - begin)).count();
76 
77  if (dt.toSec() - elapsed_secs < 0.0)
78  {
80  0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
81  }
82  else
83  {
84  ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
85  usleep((dt.toSec() - elapsed_secs) * 1e6);
86  }
87 
88  rosgraph_msgs::Clock clock;
89  clock.clock = ros::Time(internal_time);
90  clock_publisher.publish(clock);
91  internal_time += dt;
92  }
93  spinner.stop();
94 
95  return 0;
96 }
#define ROS_DEBUG_STREAM_THROTTLE(rate, args)
void publish(const boost::shared_ptr< M > &message) const
ros::Duration getPeriod() const
Definition: diffbot.h:81
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 write()
Definition: diffbot.h:95
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
int main(int argc, char **argv)
void read()
Definition: diffbot.h:83


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Apr 18 2020 03:58:05