gazebo_ros_create.cpp
Go to the documentation of this file.
1 #include <gazebo/sensors/SensorManager.hh>
2 #include <gazebo/sensors/RaySensor.hh>
3 #include <create_node/TurtlebotSensorState.h>
4 #include <nav_msgs/Odometry.h>
5 #include <geometry_msgs/Twist.h>
6 #include <sensor_msgs/JointState.h>
8 
9 using namespace gazebo;
10 
11 enum {LEFT= 0, RIGHT=1, FRONT=2, REAR=3};
12 
14  : gazebo_node_(new transport::Node())
15 {
16  this->spinner_thread_ = new boost::thread( boost::bind( &GazeboRosCreate::spin, this) );
17 
18  wheel_speed_ = new float[2];
19  wheel_speed_[LEFT] = 0.0;
20  wheel_speed_[RIGHT] = 0.0;
21 
22  set_joints_[0] = false;
23  set_joints_[1] = false;
24  set_joints_[2] = false;
25  set_joints_[3] = false;
26  joints_[0].reset();
27  joints_[1].reset();
28  joints_[2].reset();
29  joints_[3].reset();
30 }
31 
33 {
34  rosnode_->shutdown();
35  this->spinner_thread_->join();
36  delete this->spinner_thread_;
37  delete [] wheel_speed_;
38  delete rosnode_;
39 }
40 
41 void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
42 {
43  this->my_world_ = _parent->GetWorld();
44 
45  this->my_parent_ = _parent;
46  if (!this->my_parent_)
47  {
48  ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
49  return;
50  }
51 
52 
53  this->node_namespace_ = "";
54  if (_sdf->HasElement("node_namespace"))
55  this->node_namespace_ = _sdf->GetElement("node_namespace")->Get<std::string>() + "/";
56 
57 
58  left_wheel_joint_name_ = "left_wheel_joint";
59  if (_sdf->HasElement("left_wheel_joint"))
60  left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->Get<std::string>();
61 
62  right_wheel_joint_name_ = "right_wheel_joint";
63  if (_sdf->HasElement("right_wheel_joint"))
64  right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->Get<std::string>();
65 
66  front_castor_joint_name_ = "front_castor_joint";
67  if (_sdf->HasElement("front_castor_joint"))
68  front_castor_joint_name_ = _sdf->GetElement("front_castor_joint")->Get<std::string>();
69 
70  rear_castor_joint_name_ = "rear_castor_joint";
71  if (_sdf->HasElement("rear_castor_joint"))
72  rear_castor_joint_name_ = _sdf->GetElement("rear_castor_joint")->Get<std::string>();
73 
74  wheel_sep_ = 0.34;
75  if (_sdf->HasElement("wheel_separation"))
76  wheel_sep_ = _sdf->GetElement("wheel_separation")->Get<double>();
77 
78  wheel_sep_ = 0.34;
79  if (_sdf->HasElement("wheel_separation"))
80  wheel_sep_ = _sdf->GetElement("wheel_separation")->Get<double>();
81 
82  wheel_diam_ = 0.15;
83  if (_sdf->HasElement("wheel_diameter"))
84  wheel_diam_ = _sdf->GetElement("wheel_diameter")->Get<double>();
85 
86  torque_ = 10.0;
87  if (_sdf->HasElement("torque"))
88  torque_ = _sdf->GetElement("torque")->Get<double>();
89 
90  base_geom_name_ = "base_footprint_collision_base_link";
91  if (_sdf->HasElement("base_geom"))
92  base_geom_name_ = _sdf->GetElement("base_geom")->Get<std::string>();
93  base_geom_ = my_parent_->GetChildCollision(base_geom_name_);
94  if (!base_geom_)
95  {
96  // This is a hack for ROS Diamond back. E-turtle and future releases
97  // will not need this, because it will contain the fixed-joint reduction
98  // in urdf2gazebo
99  base_geom_ = my_parent_->GetChildCollision("base_footprint_geom");
100  if (!base_geom_)
101  {
102  ROS_ERROR("Unable to find geom[%s]",base_geom_name_.c_str());
103  return;
104  }
105  }
106 
107  //base_geom_->SetContactsEnabled(true);
108  //contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));
109  physics::ContactManager *mgr = my_world_->GetPhysicsEngine()->GetContactManager();
110  std::string topic = mgr->CreateFilter(base_geom_name_, base_geom_name_);
111  contact_sub_ = gazebo_node_->Subscribe(topic, &GazeboRosCreate::OnContact, this);
112 
113  wall_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(
114  sensors::SensorManager::Instance()->GetSensor("wall_sensor"));
115  if (!wall_sensor_)
116  {
117  ROS_ERROR("Unable to find sensor[wall_sensor]");
118  return;
119  }
120 
121  left_cliff_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(
122  sensors::SensorManager::Instance()->GetSensor("left_cliff_sensor"));
123  right_cliff_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(
124  sensors::SensorManager::Instance()->GetSensor("right_cliff_sensor"));
125  leftfront_cliff_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(
126  sensors::SensorManager::Instance()->GetSensor("leftfront_cliff_sensor"));
127  rightfront_cliff_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(
128  sensors::SensorManager::Instance()->GetSensor("rightfront_cliff_sensor"));
129 
130  wall_sensor_->SetActive(true);
131  left_cliff_sensor_->SetActive(true);
132  right_cliff_sensor_->SetActive(true);
133  rightfront_cliff_sensor_->SetActive(true);
134  leftfront_cliff_sensor_->SetActive(true);
135 
136  if (!ros::isInitialized())
137  {
138  int argc = 0;
139  char** argv = NULL;
141  }
142 
144 
145  cmd_vel_sub_ = rosnode_->subscribe("cmd_vel", 1, &GazeboRosCreate::OnCmdVel, this );
146 
147  sensor_state_pub_ = rosnode_->advertise<create_node::TurtlebotSensorState>("sensor_state", 1);
148  odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);
149 
150  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);
151 
152  js_.name.push_back( left_wheel_joint_name_ );
153  js_.position.push_back(0);
154  js_.velocity.push_back(0);
155  js_.effort.push_back(0);
156 
157  js_.name.push_back( right_wheel_joint_name_ );
158  js_.position.push_back(0);
159  js_.velocity.push_back(0);
160  js_.effort.push_back(0);
161 
162  js_.name.push_back( front_castor_joint_name_ );
163  js_.position.push_back(0);
164  js_.velocity.push_back(0);
165  js_.effort.push_back(0);
166 
167  js_.name.push_back( rear_castor_joint_name_ );
168  js_.position.push_back(0);
169  js_.velocity.push_back(0);
170  js_.effort.push_back(0);
171 
172  prev_update_time_ = 0;
173  last_cmd_vel_time_ = 0;
174 
175 
176  sensor_state_.bumps_wheeldrops = 0x0;
177 
178  //TODO: fix this
179 
180  joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
181  joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
182  joints_[FRONT] = my_parent_->GetJoint(front_castor_joint_name_);
183  joints_[REAR] = my_parent_->GetJoint(rear_castor_joint_name_);
184 
185  if (joints_[LEFT]) set_joints_[LEFT] = true;
186  if (joints_[RIGHT]) set_joints_[RIGHT] = true;
187  if (joints_[FRONT]) set_joints_[FRONT] = true;
188  if (joints_[REAR]) set_joints_[REAR] = true;
189 
190  //initialize time and odometry position
191  prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
192  odom_pose_[0] = 0.0;
193  odom_pose_[1] = 0.0;
194  odom_pose_[2] = 0.0;
195 
196  // Get then name of the parent model
197  std::string modelName = _sdf->GetParent()->Get<std::string>("name");
198 
199  // Listen to the update event. This event is broadcast every
200  // simulation iteration.
201  this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosCreate::UpdateChild, this));
202  gzdbg << "plugin model name: " << modelName << "\n";
203 }
204 
205 
206 void GazeboRosCreate::OnContact(ConstContactsPtr &contacts)
207 {
208  float y_overlap = 0.16495 * sin( 10 * (M_PI/180.0) );
209 
210  for (int i=0; i < contacts->contact_size(); i++ )
211  {
212  const msgs::Contact & contact = contacts->contact(i);
213  const int contact_count = contact.position_size();
214  for (unsigned int j=0; j < contact_count; j++)
215  {
216  // Make sure the contact is on the front bumper
217  if (contact.position(j).x() > 0.012 && contact.position(j).z() < 0.06 &&
218  contact.position(j).z() > 0.01)
219  {
220  // Right bump sensor
221  if (contact.position(j).y() <= y_overlap)
222  sensor_state_.bumps_wheeldrops |= 0x1;
223  // Left bump sensor
224  if (contact.position(j).y() >= -y_overlap)
225  sensor_state_.bumps_wheeldrops |= 0x2;
226  }
227  }
228  }
229 
230 }
231 
233 {
234  common::Time time_now = this->my_world_->GetSimTime();
235  common::Time step_time = time_now - prev_update_time_;
236  prev_update_time_ = time_now;
237 
238  double wd, ws;
239  double d1, d2;
240  double dr, da;
241 
242  wd = wheel_diam_;
243  ws = wheel_sep_;
244 
245  d1 = d2 = 0;
246  dr = da = 0;
247 
248  // Distance travelled by front wheels
249  if (set_joints_[LEFT])
250  d1 = step_time.Double() * (wd / 2) * joints_[LEFT]->GetVelocity(0);
251  if (set_joints_[RIGHT])
252  d2 = step_time.Double() * (wd / 2) * joints_[RIGHT]->GetVelocity(0);
253 
254  // Can see NaN values here, just zero them out if needed
255  if (isnan(d1)) {
256  ROS_WARN_THROTTLE(0.1, "Gazebo ROS Create plugin. NaN in d1. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[LEFT]->GetVelocity(0));
257  d1 = 0;
258  }
259 
260  if (isnan(d2)) {
261  ROS_WARN_THROTTLE(0.1, "Gazebo ROS Create plugin. NaN in d2. Step time: %.2f. WD: %.2f. Velocity: %.2f", step_time.Double(), wd, joints_[RIGHT]->GetVelocity(0));
262  d2 = 0;
263  }
264 
265  dr = (d1 + d2) / 2;
266  da = (d2 - d1) / ws;
267 
268  // Compute odometric pose
269  odom_pose_[0] += dr * cos( odom_pose_[2] );
270  odom_pose_[1] += dr * sin( odom_pose_[2] );
271  odom_pose_[2] += da;
272 
273  // Compute odometric instantaneous velocity
274  odom_vel_[0] = dr / step_time.Double();
275  odom_vel_[1] = 0.0;
276  odom_vel_[2] = da / step_time.Double();
277 
278 
279  if (set_joints_[LEFT])
280  {
281  joints_[LEFT]->SetVelocity( 0, wheel_speed_[LEFT] / (wd/2.0) );
282  joints_[LEFT]->SetMaxForce( 0, torque_ );
283  }
284  if (set_joints_[RIGHT])
285  {
286  joints_[RIGHT]->SetVelocity( 0, wheel_speed_[RIGHT] / (wd / 2.0) );
287  joints_[RIGHT]->SetMaxForce( 0, torque_ );
288  }
289 
290  nav_msgs::Odometry odom;
291  odom.header.stamp.sec = time_now.sec;
292  odom.header.stamp.nsec = time_now.nsec;
293  odom.header.frame_id = "odom";
294  odom.child_frame_id = "base_footprint";
295  odom.pose.pose.position.x = odom_pose_[0];
296  odom.pose.pose.position.y = odom_pose_[1];
297  odom.pose.pose.position.z = 0;
298 
299  tf::Quaternion qt;
300  qt.setEuler(0,0,odom_pose_[2]);
301 
302  odom.pose.pose.orientation.x = qt.getX();
303  odom.pose.pose.orientation.y = qt.getY();
304  odom.pose.pose.orientation.z = qt.getZ();
305  odom.pose.pose.orientation.w = qt.getW();
306 
307  double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
308  0, 1e-3, 0, 0, 0, 0,
309  0, 0, 1e6, 0, 0, 0,
310  0, 0, 0, 1e6, 0, 0,
311  0, 0, 0, 0, 1e6, 0,
312  0, 0, 0, 0, 0, 1e3};
313 
314  memcpy( &odom.pose.covariance[0], pose_cov, sizeof(double)*36 );
315  memcpy( &odom.twist.covariance[0], pose_cov, sizeof(double)*36 );
316 
317  odom.twist.twist.linear.x = odom_vel_[0];
318  odom.twist.twist.linear.y = 0;
319  odom.twist.twist.linear.z = 0;
320 
321  odom.twist.twist.angular.x = 0;
322  odom.twist.twist.angular.y = 0;
323  odom.twist.twist.angular.z = odom_vel_[2];
324 
325  odom_pub_.publish( odom );
326 
327  js_.header.stamp.sec = time_now.sec;
328  js_.header.stamp.nsec = time_now.nsec;
329  if (this->set_joints_[LEFT])
330  {
331  js_.position[0] = joints_[LEFT]->GetAngle(0).Radian();
332  js_.velocity[0] = joints_[LEFT]->GetVelocity(0);
333  }
334 
335  if (this->set_joints_[RIGHT])
336  {
337  js_.position[1] = joints_[RIGHT]->GetAngle(0).Radian();
338  js_.velocity[1] = joints_[RIGHT]->GetVelocity(0);
339  }
340 
341  if (this->set_joints_[FRONT])
342  {
343  js_.position[2] = joints_[FRONT]->GetAngle(0).Radian();
344  js_.velocity[2] = joints_[FRONT]->GetVelocity(0);
345  }
346 
347  if (this->set_joints_[REAR])
348  {
349  js_.position[3] = joints_[REAR]->GetAngle(0).Radian();
350  js_.velocity[3] = joints_[REAR]->GetVelocity(0);
351  }
352 
354 
355  this->UpdateSensors();
356 
357  //timeout if didn't receive cmd in a while
358  common::Time time_since_last_cmd = time_now - last_cmd_vel_time_;
359  if (time_since_last_cmd.Double() > 0.6)
360  {
361  wheel_speed_[LEFT] = 0;
362  wheel_speed_[RIGHT] = 0;
363  }
364 
365 }
366 
368 {
369  if (wall_sensor_->GetRange(0) < 0.04)
370  sensor_state_.wall = true;
371  else
372  sensor_state_.wall = false;
373 
374  if (left_cliff_sensor_->GetRange(0) > 0.02)
375  sensor_state_.cliff_left = true;
376  else
377  sensor_state_.cliff_left = false;
378 
379  if (right_cliff_sensor_->GetRange(0) > 0.02)
380  sensor_state_.cliff_right = true;
381  else
382  sensor_state_.cliff_right = false;
383 
384  if (rightfront_cliff_sensor_->GetRange(0) > 0.02)
385  sensor_state_.cliff_front_right = true;
386  else
387  sensor_state_.cliff_front_right = false;
388 
389  if (leftfront_cliff_sensor_->GetRange(0) > 0.02)
390  sensor_state_.cliff_front_left = true;
391  else
392  sensor_state_.cliff_front_left = false;
393 
395 
396  // Reset the bump sensors
397  sensor_state_.bumps_wheeldrops = 0x0;
398 }
399 
400 void GazeboRosCreate::OnCmdVel( const geometry_msgs::TwistConstPtr &msg)
401 {
402  last_cmd_vel_time_ = this->my_world_->GetSimTime();
403  double vr, va;
404  vr = msg->linear.x;
405  va = msg->angular.z;
406 
407  wheel_speed_[LEFT] = vr - va * (wheel_sep_) / 2;
408  wheel_speed_[RIGHT] = vr + va * (wheel_sep_) / 2;
409 }
410 
412 {
413  while(ros::ok()) ros::spinOnce();
414 }
415 
417 
#define ROS_FATAL(...)
Chain d2()
ros::Subscriber cmd_vel_sub_
sensors::RaySensorPtr right_cliff_sensor_
#define ROS_WARN_THROTTLE(rate,...)
void publish(const boost::shared_ptr< M > &message) const
void OnCmdVel(const geometry_msgs::TwistConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string left_wheel_joint_name_
ROSCPP_DECL bool isInitialized()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
sensors::RaySensorPtr left_cliff_sensor_
float wheel_sep_
Separation between the wheels.
std::string right_wheel_joint_name_
void OnContact(ConstContactsPtr &contact)
physics::CollisionPtr base_geom_
TFSIMD_FORCE_INLINE const tfScalar & getW() const
sensors::RaySensorPtr wall_sensor_
GZ_REGISTER_MODEL_PLUGIN(GazeboRosCreate)
float wheel_diam_
Diameter of the wheels.
transport::SubscriberPtr contact_sub_
sensors::RaySensorPtr rightfront_cliff_sensor_
create_node::TurtlebotSensorState sensor_state_
std::string rear_castor_joint_name_
physics::JointPtr joints_[4]
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
std::string front_castor_joint_name_
boost::thread * spinner_thread_
ROSCPP_DECL bool ok()
float torque_
Torque applied to the wheels.
ros::Publisher joint_state_pub_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
physics::ModelPtr my_parent_
ros::Publisher sensor_state_pub_
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
float * wheel_speed_
Speeds of the wheels.
ros::NodeHandle * rosnode_
sensor_msgs::JointState js_
physics::WorldPtr my_world_
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
std::string node_namespace_
Parameters.
transport::NodePtr gazebo_node_
ROSCPP_DECL void spinOnce()
sensors::RaySensorPtr leftfront_cliff_sensor_
#define ROS_ERROR(...)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
event::ConnectionPtr updateConnection


create_gazebo_plugins
Author(s): Nate Koenig
autogenerated on Mon Jun 10 2019 15:38:07