turtlebot3.cc
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2016 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Authors: Taehun Lim (Darby) */
18 
19 #ifndef GAZEBO_TB3_TURTLEBOT3_PLUGIN_CC_
20 #define GAZEBO_TB3_TURTLEBOT3_PLUGIN_CC_
21 
22 #include <fcntl.h>
23 #include <termios.h>
24 
25 #include "gazebo/gazebo.hh"
26 #include "gazebo/physics/physics.hh"
27 #include "gazebo/sensors/sensors.hh"
28 
29 #include <string>
30 #include <boost/bind.hpp>
31 #include <iostream>
32 
33 #define LEFT_WHEEL_JOINT 0
34 #define RIGHT_WHEEL_JOINT 1
35 
36 #define LIDAR_SENSOR 1
37 #define CAMERA_SENSOR 2
38 
39 #define ESC_ASCII_VALUE 0x1b
40 
41 using namespace gazebo;
42 
43 int getch()
44 {
45  static struct termios oldt, newt;
46  tcgetattr( STDIN_FILENO, &oldt);
47  newt = oldt;
48  newt.c_lflag &= ~(ICANON);
49  tcsetattr( STDIN_FILENO, TCSANOW, &newt);
50 
51  int c = getchar();
52 
53  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);
54  return c;
55 }
56 
57 int kbhit(void)
58 {
59  struct termios oldt, newt;
60  int ch;
61  int oldf;
62 
63  tcgetattr(STDIN_FILENO, &oldt);
64  newt = oldt;
65  newt.c_lflag &= ~(ICANON | ECHO);
66  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
67  oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
68  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
69 
70  ch = getchar();
71 
72  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
73  fcntl(STDIN_FILENO, F_SETFL, oldf);
74 
75  if (ch != EOF)
76  {
77  ungetc(ch, stdin);
78  return 1;
79  }
80 
81  return 0;
82 }
83 
84 class Turtlebot3 : public ModelPlugin
85 {
86  private:
87  physics::ModelPtr model_;
88 
89  physics::JointPtr left_wheel_joint_;
90  physics::JointPtr right_wheel_joint_;
91 
93 
94  common::PID pid_;
95 
96  event::ConnectionPtr updateConnection_;
97 
98  public:
100  virtual ~Turtlebot3(){}
101 
102  public:
103  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
104  {
105  getModel(_model);
106 
107  if (isTurtlebot3Model(_sdf) != true)
108  return;
109 
110  getJoints(_model);
111 
112  getSensors(_model);
113 
114  initWheel();
115 
116  showMsg();
117 
118  updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Turtlebot3::OnUpdate, this, _1));
119  }
120 
121  public:
122  void getModel(physics::ModelPtr model){ model_ = model; }
123 
124  public:
125  void getJoints(physics::ModelPtr model)
126  {
127  left_wheel_joint_ = model->GetJoints()[LEFT_WHEEL_JOINT];
128  right_wheel_joint_ = model->GetJoints()[RIGHT_WHEEL_JOINT];
129 
130  std::cout << "Find Joint : " << left_wheel_joint_->GetName() << std::endl;
131  std::cout << "Find Joint : " << right_wheel_joint_->GetName() << std::endl;
132  }
133 
134  public:
135  void getSensors(physics::ModelPtr model)
136  {
137  physics::LinkPtr lidar_link = model->GetLinks()[LIDAR_SENSOR];
138  std::cerr << "Find Sensor : " << lidar_link->GetScopedName() << std::endl;
139 
140  if (model_->GetName() != "burger")
141  {
142  physics::LinkPtr camera_link = model->GetLinks()[CAMERA_SENSOR];
143  std::cerr << "Find Sensor : " << camera_link->GetScopedName() << std::endl;
144  }
145  }
146 
147  public:
148  bool isTurtlebot3Model(sdf::ElementPtr sdf)
149  {
150  std::string get_tb3_model;
151 
152  if (sdf->HasElement("tb3_model"))
153  {
154  get_tb3_model = sdf->Get<std::string>("tb3_model");
155 
156  if (get_tb3_model == "burger")
157  {
158  wheel_separation_ = 0.160;
159  }
160  else if (get_tb3_model == "waffle" || get_tb3_model == "waffle_pi")
161  {
162  wheel_separation_ = 0.287;
163  }
164  else
165  {
166  std::cerr << "Invalid model name, TB3 plugin is not loaded" << std::endl;
167  return false;
168  }
169  }
170  else
171  {
172  std::cerr << "Please put a tb3 model(burger, waffle or waffle_pi) in turtlebot3_.world file, TB3 plugin is not loaded" << std::endl;
173  return false;
174  }
175 
176  std::cout << "The turtlebot3 plugin is attach to model : " << model_->GetName() << "/turtlebot3_" << get_tb3_model << std::endl;
177 
178  return true;
179  }
180 
181  public:
182  void writePIDparamForJointControl(double p, double i, double d)
183  {
184  pid_ = common::PID(p, i, d);
185 
186  model_->GetJointController()->SetVelocityPID(left_wheel_joint_->GetScopedName(), pid_);
187  model_->GetJointController()->SetVelocityPID(right_wheel_joint_->GetScopedName(), pid_);
188  }
189 
190  public:
191  void initWheel()
192  {
193  writePIDparamForJointControl(1.0, 0, 0);
194 
195  writeVelocityToJoint(0.0, 0.0);
196  }
197 
198  public:
199  void showMsg()
200  {
201  std::cout << " " <<std::endl;
202  std::cout << "Control Your TurtleBot3!" << std::endl;
203  std::cout << "---------------------------" << std::endl;
204  std::cout << "w - set linear velocity up" << std::endl;
205  std::cout << "x - set linear velocity down" << std::endl;
206  std::cout << "d - set angular velocity up" << std::endl;
207  std::cout << "a - set angular velocity down" << std::endl;
208  std::cout << "s - set all velocity to zero" << std::endl;
209  }
210 
211  public:
212  void controlTB3(const float wheel_separation, double lin_vel, double ang_vel)
213  {
214  // This velocities are not reliable yet due to frinction. It will be updated
215  double _lin_vel = lin_vel;
216  double _ang_vel = ang_vel;
217 
218  double right_wheel_vel = 0.0;
219  double left_wheel_vel = 0.0;
220 
221  left_wheel_vel = _lin_vel - (_ang_vel * wheel_separation / 2);
222  right_wheel_vel = _lin_vel + (_ang_vel * wheel_separation / 2);
223 
224  writeVelocityToJoint(right_wheel_vel, left_wheel_vel);
225  }
226 
227  public:
228  void writeVelocityToJoint(double right_wheel_vel, double left_wheel_vel)
229  {
230  model_->GetJointController()->SetVelocityTarget(left_wheel_joint_->GetScopedName(), left_wheel_vel);
231  model_->GetJointController()->SetVelocityTarget(right_wheel_joint_->GetScopedName(), right_wheel_vel);
232  }
233 
234  public:
236  {
237  return left_wheel_joint_->GetAngle(0).Radian();
238  }
239 
240  public:
242  {
243  return right_wheel_joint_->GetAngle(0).Radian();
244  }
245 
246  public:
247  void OnUpdate(const common::UpdateInfo &)
248  {
249  static uint8_t hit_cnt = 0;
250  static double lin_vel_cmd = 0;
251  static double ang_vel_cmd = 0;
252 
253  if (kbhit())
254  {
255  std::cout << " is pressed" << std::endl;
256 
257  if (hit_cnt > 10)
258  {
259  showMsg();
260  hit_cnt = 0;
261  }
262  else
263  hit_cnt++;
264 
265  int c = getch();
266 
267  if (c == ESC_ASCII_VALUE)
268  exit(0);
269 
270  if (c =='w')
271  lin_vel_cmd += 0.25;
272  else if (c == 'x')
273  lin_vel_cmd -= 0.25;
274  else if (c == 'a')
275  ang_vel_cmd += 0.5;
276  else if (c == 'd')
277  ang_vel_cmd -= 0.5;
278  else if (c == 's')
279  {
280  lin_vel_cmd = 0;
281  ang_vel_cmd = 0;
282  }
283  else
284  {
285  lin_vel_cmd = lin_vel_cmd;
286  ang_vel_cmd = ang_vel_cmd;
287  }
288 
289  controlTB3(wheel_separation_, lin_vel_cmd, ang_vel_cmd);
290  }
291  }
292 };
293 
294 GZ_REGISTER_MODEL_PLUGIN(Turtlebot3)
295 
296 #endif //GAZEBO_TB3_TURTLEBOT3_PLUGIN_CC_
void showMsg()
Definition: turtlebot3.cc:199
#define RIGHT_WHEEL_JOINT
Definition: turtlebot3.cc:34
bool isTurtlebot3Model(sdf::ElementPtr sdf)
Definition: turtlebot3.cc:148
void controlTB3(const float wheel_separation, double lin_vel, double ang_vel)
Definition: turtlebot3.cc:212
void initWheel()
Definition: turtlebot3.cc:191
int getch()
Definition: turtlebot3.cc:43
void writePIDparamForJointControl(double p, double i, double d)
Definition: turtlebot3.cc:182
int kbhit(void)
Definition: turtlebot3.cc:57
void writeVelocityToJoint(double right_wheel_vel, double left_wheel_vel)
Definition: turtlebot3.cc:228
common::PID pid_
Definition: turtlebot3.cc:94
double getLeftJointPosition()
Definition: turtlebot3.cc:235
void OnUpdate(const common::UpdateInfo &)
Definition: turtlebot3.cc:247
#define LEFT_WHEEL_JOINT
Definition: turtlebot3.cc:33
physics::JointPtr right_wheel_joint_
Definition: turtlebot3.cc:90
physics::JointPtr left_wheel_joint_
Definition: turtlebot3.cc:89
void getJoints(physics::ModelPtr model)
Definition: turtlebot3.cc:125
#define CAMERA_SENSOR
Definition: turtlebot3.cc:37
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Definition: turtlebot3.cc:103
void getSensors(physics::ModelPtr model)
Definition: turtlebot3.cc:135
event::ConnectionPtr updateConnection_
Definition: turtlebot3.cc:96
physics::ModelPtr model_
Definition: turtlebot3.cc:87
double getRightJointPosition()
Definition: turtlebot3.cc:241
virtual ~Turtlebot3()
Definition: turtlebot3.cc:100
double wheel_separation_
Definition: turtlebot3.cc:92
#define LIDAR_SENSOR
Definition: turtlebot3.cc:36
void getModel(physics::ModelPtr model)
Definition: turtlebot3.cc:122
#define ESC_ASCII_VALUE
Definition: turtlebot3.cc:39


turtlebot3_gazebo_plugin
Author(s): Pyo , Darby Lim
autogenerated on Tue Mar 13 2018 02:56:08