gazebo_bag_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 #include "rotors_gazebo_plugins/gazebo_bag_plugin.h"
00023 
00024 #include <ctime>
00025 
00026 #include <mav_msgs/Actuators.h>
00027 
00028 namespace gazebo {
00029 
00030 GazeboBagPlugin::~GazeboBagPlugin() {
00031   
00032   if (node_handle_) {
00033     node_handle_->shutdown();
00034     delete node_handle_;
00035   }
00036   bag_.close();
00037 }
00038 
00039 void GazeboBagPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
00040   if (kPrintOnPluginLoad) {
00041     gzdbg << __FUNCTION__ << "() called." << std::endl;
00042   }
00043 
00044   // Store the pointer to the model
00045   model_ = _model;
00046   // world_ = physics::get_world(model_->world.name);
00047   world_ = model_->GetWorld();
00048 
00049   if (_sdf->HasElement("robotNamespace")) {
00050     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00051   } else {
00052     gzerr << "[gazebo_bag_plugin] Please specify a robotNamespace.\n";
00053   }
00054   node_handle_ = new ros::NodeHandle(namespace_);
00055 
00056   if (_sdf->HasElement("bagFileName")) {
00057     bag_filename_ = _sdf->GetElement("bagFileName")->Get<std::string>();
00058   } else {
00059     gzerr << "[gazebo_bag_plugin] Please specify a bagFileName.\n";
00060   }
00061 
00062   if (_sdf->HasElement("linkName")) {
00063     link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
00064   } else {
00065     gzwarn << "[gazebo_bag_plugin] No linkName specified, using default "
00066            << link_name_ << ".\n";
00067   }
00068 
00069   // Get the pointer to the link
00070   link_ = model_->GetLink(link_name_);
00071   if (link_ == NULL) {
00072     gzthrow("[gazebo_bag_plugin] Couldn't find specified link \"" << link_name_
00073                                                                   << "\".");
00074   }
00075 
00076   getSdfParam<std::string>(_sdf, "frameId", frame_id_, frame_id_);
00077   getSdfParam<std::string>(_sdf, "imuTopic", imu_topic_, imu_topic_);
00078   getSdfParam<std::string>(_sdf, "commandAttitudeThrustTopic",
00079                            control_attitude_thrust_topic_,
00080                            control_attitude_thrust_topic_);
00081   getSdfParam<std::string>(_sdf, "commandMotorSpeedTopic",
00082                            control_motor_speed_topic_,
00083                            control_motor_speed_topic_);
00084   getSdfParam<std::string>(_sdf, "commandRateThrustTopic",
00085                            control_rate_thrust_topic_,
00086                            control_rate_thrust_topic_);
00087   getSdfParam<std::string>(_sdf, "windSpeedTopic",
00088                            wind_speed_topic_, wind_speed_topic_);
00089   getSdfParam<std::string>(_sdf, "motorTopic", motor_topic_, motor_topic_);
00090   getSdfParam<std::string>(_sdf, "poseTopic", ground_truth_pose_topic_,
00091                            ground_truth_pose_topic_);
00092   getSdfParam<std::string>(_sdf, "twistTopic", ground_truth_twist_topic_,
00093                            ground_truth_twist_topic_);
00094   getSdfParam<std::string>(_sdf, "wrenchesTopic", wrench_topic_,
00095                            wrench_topic_);
00096   getSdfParam<std::string>(_sdf, "externalForceTopic", external_force_topic_,
00097                            external_force_topic_);
00098   getSdfParam<std::string>(_sdf, "waypointTopic", waypoint_topic_,
00099                            waypoint_topic_);
00100   getSdfParam<std::string>(_sdf, "commandPoseTopic", command_pose_topic_,
00101                            command_pose_topic_);
00102   getSdfParam<std::string>(_sdf, "recordingServiceName",
00103                            recording_service_name_, recording_service_name_);
00104 
00105   getSdfParam<double>(_sdf, "rotorVelocitySlowdownSim",
00106                       rotor_velocity_slowdown_sim_,
00107                       rotor_velocity_slowdown_sim_);
00108 
00109   getSdfParam<bool>(_sdf, "waitToRecordBag", wait_to_record_, wait_to_record_);
00110 
00111   recording_service_ = node_handle_->advertiseService(
00112       recording_service_name_, &GazeboBagPlugin::RecordingServiceCallback,
00113       this);
00114 
00115   // Get the motor joints.
00116   child_links_ = link_->GetChildJointsLinks();
00117   for (unsigned int i = 0; i < child_links_.size(); i++) {
00118     std::string link_name = child_links_[i]->GetScopedName();
00119 
00120     // Check if the link contains rotor_ in its name.
00121     int pos = link_name.find("rotor_");
00122     if (pos != link_name.npos) {
00123       std::string motor_number_str = link_name.substr(pos + 6);
00124       unsigned int motor_number = std::stoi(motor_number_str);
00125       std::string joint_name = child_links_[i]->GetName() + "_joint";
00126       physics::JointPtr joint = model_->GetJoint(joint_name);
00127       motor_joints_.insert(MotorNumberToJointPair(motor_number, joint));
00128     }
00129   }
00130 
00131   // Get the contact manager.
00132   std::vector<std::string> collisions;
00133   contact_mgr_ = world_->Physics()->GetContactManager();
00134   for (unsigned int i = 0; i < link_->GetCollisions().size(); ++i) {
00135     physics::CollisionPtr collision = link_->GetCollision(i);
00136     collisions.push_back(collision->GetScopedName());
00137   }
00138   for (unsigned int j = 0; j < child_links_.size(); ++j) {
00139     unsigned int zero = 0;
00140     for (unsigned int i = 0; i < child_links_[j]->GetCollisions().size(); ++i) {
00141       collisions.push_back(child_links_[j]->GetCollision(i)->GetScopedName());
00142     }
00143   }
00144 
00145   if (!collisions.empty()) {
00146     contact_mgr_->CreateFilter(link_->GetName(), collisions);
00147   }
00148 
00149   // If we do not need to wait for user command, we start recording right away
00150   if (!wait_to_record_) StartRecording();
00151 }
00152 
00153 // This gets called by the world update start event.
00154 void GazeboBagPlugin::OnUpdate(const common::UpdateInfo& _info) {
00155   if (kPrintOnUpdates) {
00156     gzdbg << __FUNCTION__ << "() called." << std::endl;
00157   }
00158 
00159   // Get the current simulation time.
00160   common::Time now = world_->SimTime();
00161   LogWrenches(now);
00162   LogGroundTruth(now);
00163   LogMotorVelocities(now);
00164 }
00165 
00166 void GazeboBagPlugin::StartRecording() {
00167   time_t rawtime;
00168   struct tm* timeinfo;
00169   char buffer[80];
00170 
00171   time(&rawtime);
00172   timeinfo = localtime(&rawtime);
00173 
00174   strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", timeinfo);
00175   std::string date_time_str(buffer);
00176 
00177   std::string key(".bag");
00178   size_t pos = bag_filename_.rfind(key);
00179   if (pos != std::string::npos) {
00180     bag_filename_.erase(pos, key.length());
00181   }
00182   std::string full_bag_filename = bag_filename_ + "_" + date_time_str + ".bag";
00183 
00184   // Open a bag file and store it in ~/.ros/<full_bag_filename>.
00185   bag_.open(full_bag_filename, rosbag::bagmode::Write);
00186 
00187   // Subscriber to IMU sensor_msgs::Imu Message.
00188   imu_sub_ = node_handle_->subscribe(imu_topic_, 10,
00189                                      &GazeboBagPlugin::ImuCallback, this);
00190 
00191   // Subscriber to External Force WrenchStamped Message.
00192   external_force_sub_ = node_handle_->subscribe(external_force_topic_, 10,
00193       &GazeboBagPlugin::ExternalForceCallback, this);
00194 
00195   // Subscriber to Waypoint MultiDOFJointTrajectory Message.
00196   waypoint_sub_ = node_handle_->subscribe(
00197       waypoint_topic_, 10, &GazeboBagPlugin::WaypointCallback, this);
00198 
00199   // Subscriber to PoseStamped pose command message.
00200   command_pose_sub_ = node_handle_->subscribe(
00201       command_pose_topic_, 10, &GazeboBagPlugin::CommandPoseCallback, this);
00202 
00203   // Subscriber to Control Attitude Thrust Message.
00204   control_attitude_thrust_sub_ =
00205       node_handle_->subscribe(control_attitude_thrust_topic_, 10,
00206                               &GazeboBagPlugin::AttitudeThrustCallback, this);
00207 
00208   // Subscriber to Control Motor Speed Message.
00209   control_motor_speed_sub_ =
00210       node_handle_->subscribe(control_motor_speed_topic_, 10,
00211                               &GazeboBagPlugin::ActuatorsCallback, this);
00212 
00213   // Subscriber to Control Rate Thrust Message.
00214   control_rate_thrust_sub_ =
00215       node_handle_->subscribe(control_rate_thrust_topic_, 10,
00216                               &GazeboBagPlugin::RateThrustCallback, this);
00217 
00218   // Subscriber to Wind Speed Message.
00219   wind_speed_sub_ =
00220       node_handle_->subscribe(wind_speed_topic_, 10,
00221                               &GazeboBagPlugin::WindSpeedCallback, this);
00222 
00223   // Listen to the update event. This event is broadcast every
00224   // simulation iteration.
00225   update_connection_ = event::Events::ConnectWorldUpdateBegin(
00226       boost::bind(&GazeboBagPlugin::OnUpdate, this, _1));
00227 
00228   // Set the flag that we are actively recording.
00229   is_recording_ = true;
00230 
00231   ROS_INFO("GazeboBagPlugin START recording bagfile %s",
00232            full_bag_filename.c_str());
00233 }
00234 
00235 void GazeboBagPlugin::StopRecording() {
00236   // Shutdown all the subscribers.
00237   imu_sub_.shutdown();
00238   external_force_sub_.shutdown();
00239   waypoint_sub_.shutdown();
00240   command_pose_sub_.shutdown();
00241   control_attitude_thrust_sub_.shutdown();
00242   control_motor_speed_sub_.shutdown();
00243   control_rate_thrust_sub_.shutdown();
00244   wind_speed_sub_.shutdown();
00245 
00246   // Disconnect the update event.
00247   
00248 
00249   // Close the bag.
00250   bag_.close();
00251 
00252   // Clear the flag to show that we are not actively recording
00253   is_recording_ = false;
00254 
00255   ROS_INFO("GazeboBagPlugin STOP recording bagfile");
00256 }
00257 
00258 void GazeboBagPlugin::ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg) {
00259   common::Time now = world_->SimTime();
00260   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00261   writeBag(namespace_ + "/" + imu_topic_, ros_now, imu_msg);
00262 }
00263 
00264 void GazeboBagPlugin::ExternalForceCallback(
00265     const geometry_msgs::WrenchStampedConstPtr& force_msg) {
00266   common::Time now = world_->SimTime();
00267   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00268   writeBag(namespace_ + "/" + external_force_topic_, ros_now, force_msg);
00269 }
00270 
00271 void GazeboBagPlugin::WaypointCallback(
00272     const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_msg) {
00273   common::Time now = world_->SimTime();
00274   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00275   writeBag(namespace_ + "/" + waypoint_topic_, ros_now, trajectory_msg);
00276 }
00277 
00278 void GazeboBagPlugin::CommandPoseCallback(
00279     const geometry_msgs::PoseStampedConstPtr& pose_msg) {
00280   common::Time now = world_->SimTime();
00281   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00282   writeBag(namespace_ + "/" + command_pose_topic_, ros_now, pose_msg);
00283 }
00284 
00285 void GazeboBagPlugin::AttitudeThrustCallback(
00286     const mav_msgs::AttitudeThrustConstPtr& control_msg) {
00287   common::Time now = world_->SimTime();
00288   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00289   writeBag(namespace_ + "/" + control_attitude_thrust_topic_, ros_now,
00290            control_msg);
00291 }
00292 
00293 void GazeboBagPlugin::ActuatorsCallback(
00294     const mav_msgs::ActuatorsConstPtr& control_msg) {
00295   common::Time now = world_->SimTime();
00296   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00297   writeBag(namespace_ + "/" + control_motor_speed_topic_, ros_now, control_msg);
00298 }
00299 
00300 void GazeboBagPlugin::RateThrustCallback(
00301     const mav_msgs::RateThrustConstPtr& control_msg) {
00302   common::Time now = world_->SimTime();
00303   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00304   writeBag(namespace_ + "/" + control_rate_thrust_topic_, ros_now, control_msg);
00305 }
00306 
00307 void GazeboBagPlugin::WindSpeedCallback(
00308     const rotors_comm::WindSpeedConstPtr& wind_speed_msg) {
00309   common::Time now = world_->SimTime();
00310   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00311   writeBag(namespace_ + "/" + wind_speed_topic_, ros_now, wind_speed_msg);
00312 }
00313 
00314 void GazeboBagPlugin::LogMotorVelocities(const common::Time now) {
00315   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00316 
00317   mav_msgs::Actuators rot_velocities_msg;
00318   rot_velocities_msg.angular_velocities.resize(motor_joints_.size());
00319 
00320   MotorNumberToJointMap::iterator m;
00321   for (m = motor_joints_.begin(); m != motor_joints_.end(); ++m) {
00322     double motor_rot_vel =
00323         m->second->GetVelocity(0) * rotor_velocity_slowdown_sim_;
00324     rot_velocities_msg.angular_velocities[m->first] = motor_rot_vel;
00325   }
00326   rot_velocities_msg.header.stamp.sec = now.sec;
00327   rot_velocities_msg.header.stamp.nsec = now.nsec;
00328 
00329   writeBag(namespace_ + "/" + motor_topic_, ros_now, rot_velocities_msg);
00330 }
00331 
00332 void GazeboBagPlugin::LogGroundTruth(const common::Time now) {
00333   ros::Time ros_now = ros::Time(now.sec, now.nsec);
00334 
00335   geometry_msgs::PoseStamped pose_msg;
00336   geometry_msgs::TwistStamped twist_msg;
00337 
00338   // Get pose and update the message.
00339   ignition::math::Pose3d pose = link_->WorldPose();
00340   pose_msg.header.frame_id = frame_id_;
00341   pose_msg.header.stamp.sec = now.sec;
00342   pose_msg.header.stamp.nsec = now.nsec;
00343   pose_msg.pose.position.x = pose.Pos().X();
00344   pose_msg.pose.position.y = pose.Pos().Y();
00345   pose_msg.pose.position.z = pose.Pos().Z();
00346   pose_msg.pose.orientation.w = pose.Rot().W();
00347   pose_msg.pose.orientation.x = pose.Rot().X();
00348   pose_msg.pose.orientation.y = pose.Rot().Y();
00349   pose_msg.pose.orientation.z = pose.Rot().Z();
00350 
00351   writeBag(namespace_ + "/" + ground_truth_pose_topic_, ros_now, pose_msg);
00352 
00353   // Get twist and update the message.
00354   ignition::math::Vector3d linear_veloctiy = link_->WorldLinearVel();
00355   ignition::math::Vector3d angular_veloctiy = link_->WorldAngularVel();
00356   twist_msg.header.frame_id = frame_id_;
00357   twist_msg.header.stamp.sec = now.sec;
00358   twist_msg.header.stamp.nsec = now.nsec;
00359   twist_msg.twist.linear.x = linear_veloctiy.X();
00360   twist_msg.twist.linear.y = linear_veloctiy.Y();
00361   twist_msg.twist.linear.z = linear_veloctiy.Z();
00362   twist_msg.twist.angular.x = angular_veloctiy.X();
00363   twist_msg.twist.angular.y = angular_veloctiy.Y();
00364   twist_msg.twist.angular.z = angular_veloctiy.Z();
00365 
00366   writeBag(namespace_ + "/" + ground_truth_twist_topic_, ros_now, twist_msg);
00367 }
00368 
00369 void GazeboBagPlugin::LogWrenches(const common::Time now) {
00370   geometry_msgs::WrenchStamped wrench_msg;
00371   std::vector<physics::Contact*> contacts = contact_mgr_->GetContacts();
00372   for (int i = 0; i < contact_mgr_->GetContactCount(); ++i) {
00373     std::string collision2_name =
00374         contacts[i]->collision2->GetLink()->GetScopedName();
00375     double body1_force = contacts[i]->wrench->body1Force.Length();
00376 
00377     // Exclude extremely small forces.
00378     if (body1_force < 1e-10) continue;
00379     // Do this, such that all the contacts are logged.
00380     // (publishing on the same topic with the same time stamp is impossible)
00381     ros::Time ros_now = ros::Time(now.sec, now.nsec + i * 1000);
00382     std::string collision1_name =
00383         contacts[i]->collision1->GetLink()->GetScopedName();
00384     wrench_msg.header.frame_id = collision1_name + "--" + collision2_name;
00385     wrench_msg.header.stamp.sec = now.sec;
00386     wrench_msg.header.stamp.nsec = now.nsec;
00387     wrench_msg.wrench.force.x = contacts[i]->wrench->body1Force.X();
00388     wrench_msg.wrench.force.y = contacts[i]->wrench->body1Force.Y();
00389     wrench_msg.wrench.force.z = contacts[i]->wrench->body1Force.Z();
00390     wrench_msg.wrench.torque.x = contacts[i]->wrench->body1Torque.X();
00391     wrench_msg.wrench.torque.y = contacts[i]->wrench->body1Torque.Y();
00392     wrench_msg.wrench.torque.z = contacts[i]->wrench->body1Torque.Z();
00393 
00394     writeBag(namespace_ + "/" + wrench_topic_, ros_now, wrench_msg);
00395   }
00396 }
00397 
00398 bool GazeboBagPlugin::RecordingServiceCallback(
00399     rotors_comm::RecordRosbag::Request& req,
00400     rotors_comm::RecordRosbag::Response& res) {
00401   if (req.record && !is_recording_) {
00402     StartRecording();
00403     res.success = true;
00404   } else if (req.record && is_recording_) {
00405     gzwarn << "[gazebo_bag_plugin] Already recording rosbag, ignoring start "
00406               "command.\n";
00407     res.success = false;
00408   } else if (!req.record && is_recording_) {
00409     StopRecording();
00410     res.success = true;
00411   } else if (!req.record && !is_recording_) {
00412     gzwarn << "[gazebo_bag_plugin] Already not recording rosbag, ignoring stop "
00413               "command.\n";
00414     res.success = false;
00415   }
00416 
00417   return res.success;
00418 }
00419 
00420 GZ_REGISTER_MODEL_PLUGIN(GazeboBagPlugin);
00421 
00422 }  // namespace gazebo


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43