00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00045 model_ = _model;
00046
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
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
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
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
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
00150 if (!wait_to_record_) StartRecording();
00151 }
00152
00153
00154 void GazeboBagPlugin::OnUpdate(const common::UpdateInfo& _info) {
00155 if (kPrintOnUpdates) {
00156 gzdbg << __FUNCTION__ << "() called." << std::endl;
00157 }
00158
00159
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
00185 bag_.open(full_bag_filename, rosbag::bagmode::Write);
00186
00187
00188 imu_sub_ = node_handle_->subscribe(imu_topic_, 10,
00189 &GazeboBagPlugin::ImuCallback, this);
00190
00191
00192 external_force_sub_ = node_handle_->subscribe(external_force_topic_, 10,
00193 &GazeboBagPlugin::ExternalForceCallback, this);
00194
00195
00196 waypoint_sub_ = node_handle_->subscribe(
00197 waypoint_topic_, 10, &GazeboBagPlugin::WaypointCallback, this);
00198
00199
00200 command_pose_sub_ = node_handle_->subscribe(
00201 command_pose_topic_, 10, &GazeboBagPlugin::CommandPoseCallback, this);
00202
00203
00204 control_attitude_thrust_sub_ =
00205 node_handle_->subscribe(control_attitude_thrust_topic_, 10,
00206 &GazeboBagPlugin::AttitudeThrustCallback, this);
00207
00208
00209 control_motor_speed_sub_ =
00210 node_handle_->subscribe(control_motor_speed_topic_, 10,
00211 &GazeboBagPlugin::ActuatorsCallback, this);
00212
00213
00214 control_rate_thrust_sub_ =
00215 node_handle_->subscribe(control_rate_thrust_topic_, 10,
00216 &GazeboBagPlugin::RateThrustCallback, this);
00217
00218
00219 wind_speed_sub_ =
00220 node_handle_->subscribe(wind_speed_topic_, 10,
00221 &GazeboBagPlugin::WindSpeedCallback, this);
00222
00223
00224
00225 update_connection_ = event::Events::ConnectWorldUpdateBegin(
00226 boost::bind(&GazeboBagPlugin::OnUpdate, this, _1));
00227
00228
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
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
00247
00248
00249
00250 bag_.close();
00251
00252
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
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
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
00378 if (body1_force < 1e-10) continue;
00379
00380
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 }