24 #include "ConnectGazeboToRosTopic.pb.h" 25 #include "ConnectRosToGazeboTopic.pb.h" 51 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
58 if (_sdf->HasElement(
"robotNamespace"))
59 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
61 gzerr <<
"[gazebo_motor_model] Please specify a robotNamespace.\n";
63 node_handle_ = gazebo::transport::NodePtr(
new transport::Node());
68 if (_sdf->HasElement(
"jointName"))
69 joint_name_ = _sdf->GetElement(
"jointName")->Get<std::string>();
71 gzerr <<
"[gazebo_motor_model] Please specify a jointName, where the rotor " 78 "[gazebo_motor_model] Couldn't find specified joint \"" <<
joint_name_ 81 if (_sdf->HasElement(
"linkName"))
82 link_name_ = _sdf->GetElement(
"linkName")->Get<std::string>();
84 gzerr <<
"[gazebo_motor_model] Please specify a linkName of the rotor.\n";
88 "[gazebo_motor_model] Couldn't find specified link \"" <<
link_name_ 91 if (_sdf->HasElement(
"motorNumber"))
94 gzerr <<
"[gazebo_motor_model] Please specify a motorNumber.\n";
96 if (_sdf->HasElement(
"turningDirection")) {
98 _sdf->GetElement(
"turningDirection")->Get<std::string>();
99 if (turning_direction ==
"cw")
101 else if (turning_direction ==
"ccw")
104 gzerr <<
"[gazebo_motor_model] Please only use 'cw' or 'ccw' as " 105 "turningDirection.\n";
107 gzerr <<
"[gazebo_motor_model] Please specify a turning direction ('cw' or " 110 if (_sdf->HasElement(
"motorType")) {
111 std::string motor_type = _sdf->GetElement(
"motorType")->Get<std::string>();
112 if (motor_type ==
"velocity")
114 else if (motor_type ==
"position")
116 else if (motor_type ==
"force") {
119 gzerr <<
"[gazebo_motor_model] Please only use 'velocity', 'position' or " 120 "'force' as motorType.\n";
122 gzwarn <<
"[gazebo_motor_model] motorType not specified, using velocity.\n";
128 if (_sdf->HasElement(
"joint_control_pid")) {
129 sdf::ElementPtr pid = _sdf->GetElement(
"joint_control_pid");
131 if (pid->HasElement(
"p"))
132 p = pid->Get<
double>(
"p");
134 if (pid->HasElement(
"i"))
135 i = pid->Get<
double>(
"i");
137 if (pid->HasElement(
"d"))
138 d = pid->Get<
double>(
"d");
140 if (pid->HasElement(
"iMax"))
141 iMax = pid->Get<
double>(
"iMax");
143 if (pid->HasElement(
"iMin"))
144 iMin = pid->Get<
double>(
"iMin");
146 if (pid->HasElement(
"cmdMax"))
147 cmdMax = pid->Get<
double>(
"cmdMax");
149 if (pid->HasElement(
"cmdMin"))
150 cmdMin = pid->Get<
double>(
"cmdMin");
151 pids_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin);
153 pids_.Init(0, 0, 0, 0, 0, 0, 0);
154 gzerr <<
"[gazebo_motor_model] PID values not found, Setting all values " 158 pids_.Init(0, 0, 0, 0, 0, 0, 0);
161 getSdfParam<std::string>(
163 getSdfParam<std::string>(
165 getSdfParam<std::string>(
170 if (_sdf->HasElement(
"motorPositionPubTopic")) {
173 _sdf->GetElement(
"motorPositionPubTopic")->Get<std::string>();
175 if (_sdf->HasElement(
"motorForcePubTopic")) {
178 _sdf->GetElement(
"motorForcePubTopic")->Get<std::string>();
214 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
229 gzdbg << __PRETTY_FUNCTION__ <<
" called." << std::endl;
232 gazebo::transport::PublisherPtr gz_connect_gazebo_to_ros_topic_pub =
233 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
235 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
238 gazebo::transport::PublisherPtr gz_connect_ros_to_gazebo_topic_pub =
239 node_handle_->Advertise<gz_std_msgs::ConnectRosToGazeboTopic>(
241 gz_std_msgs::ConnectRosToGazeboTopic connect_ros_to_gazebo_topic_msg;
250 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
251 "~/" +
namespace_ +
"/" + motor_speed_pub_topic_);
252 connect_gazebo_to_ros_topic_msg.set_ros_topic(
254 connect_gazebo_to_ros_topic_msg.set_msgtype(
255 gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32);
256 gz_connect_gazebo_to_ros_topic_pub->Publish(
257 connect_gazebo_to_ros_topic_msg,
true);
268 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
269 "~/" +
namespace_ +
"/" + motor_position_pub_topic_);
270 connect_gazebo_to_ros_topic_msg.set_ros_topic(
271 namespace_ +
"/" + motor_position_pub_topic_);
272 connect_gazebo_to_ros_topic_msg.set_msgtype(
273 gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32);
274 gz_connect_gazebo_to_ros_topic_pub->Publish(
275 connect_gazebo_to_ros_topic_msg,
true);
286 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
287 "~/" +
namespace_ +
"/" + motor_force_pub_topic_);
288 connect_gazebo_to_ros_topic_msg.set_ros_topic(
290 connect_gazebo_to_ros_topic_msg.set_msgtype(
291 gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32);
292 gz_connect_gazebo_to_ros_topic_pub->Publish(
293 connect_gazebo_to_ros_topic_msg,
true);
304 connect_ros_to_gazebo_topic_msg.set_ros_topic(
306 connect_ros_to_gazebo_topic_msg.set_gazebo_topic(
308 connect_ros_to_gazebo_topic_msg.set_msgtype(
309 gz_std_msgs::ConnectRosToGazeboTopic::COMMAND_MOTOR_SPEED);
310 gz_connect_ros_to_gazebo_topic_pub->Publish(
311 connect_ros_to_gazebo_topic_msg,
true);
322 connect_ros_to_gazebo_topic_msg.set_ros_topic(
324 connect_ros_to_gazebo_topic_msg.set_gazebo_topic(
326 connect_ros_to_gazebo_topic_msg.set_msgtype(
327 gz_std_msgs::ConnectRosToGazeboTopic::WIND_SPEED);
328 gz_connect_ros_to_gazebo_topic_pub->Publish(
329 connect_ros_to_gazebo_topic_msg,
true);
335 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
338 if (
motor_number_ > command_motor_input_msg->motor_speed_size() - 1) {
340 <<
" of the MotorSpeed message array which is of size " 341 << command_motor_input_msg->motor_speed_size();
361 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
373 double wrapped = std::fmod(std::abs(input), 2*
M_PI);
374 wrapped = std::copysign(wrapped, input);
378 if(std::abs(wrapped - 2*
M_PI) < 1
e-8){
403 if(std::abs(err -
M_PI) < 1
e-8){
408 joint_->SetForce(0, force);
420 <<
"] might occur. Consider making smaller simulation time " 421 "steps or raising the rotor_velocity_slowdown_sim_ param.\n";
423 double real_motor_velocity =
426 int real_motor_velocity_sign =
427 (real_motor_velocity > 0) - (real_motor_velocity < 0);
430 real_motor_velocity * real_motor_velocity *
434 link_->AddRelativeForce(ignition::math::Vector3d (0, 0, thrust));
440 ignition::math::Vector3d joint_axis =
joint_->GlobalAxis(0);
441 ignition::math::Vector3d body_velocity_W =
link_->WorldLinearVel();
442 ignition::math::Vector3d relative_wind_velocity_W = body_velocity_W -
wind_speed_W_;
443 ignition::math::Vector3d body_velocity_perpendicular =
444 relative_wind_velocity_W -
445 (relative_wind_velocity_W.Dot(joint_axis) * joint_axis);
446 ignition::math::Vector3d air_drag = -std::abs(real_motor_velocity) *
448 body_velocity_perpendicular;
451 link_->AddForce(air_drag);
454 physics::Link_V parent_links =
link_->GetParentJointsLinks();
456 ignition::math::Pose3d pose_difference =
457 link_->WorldCoGPose() - parent_links.at(0)->WorldCoGPose();
458 ignition::math::Vector3d drag_torque(
462 ignition::math::Vector3d drag_torque_parent_frame =
463 pose_difference.Rot().RotateVector(drag_torque);
464 parent_links.at(0)->AddRelativeTorque(drag_torque_parent_frame);
466 ignition::math::Vector3d rolling_moment;
468 rolling_moment = -std::abs(real_motor_velocity) *
470 body_velocity_perpendicular;
471 parent_links.at(0)->AddTorque(rolling_moment);
473 double ref_motor_rot_vel;
482 rotor_velocity_slowdown_sim_);
void WindSpeedCallback(GzWindSpeedMsgPtr &wind_speed_msg)
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
static const std::string kConnectRosToGazeboSubtopic
std::string command_sub_topic_
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
virtual void InitializeParams()
gazebo::transport::NodePtr node_handle_
std::string motor_position_pub_topic_
std::string motor_force_pub_topic_
gazebo::transport::PublisherPtr motor_position_pub_
gazebo::transport::SubscriberPtr wind_speed_sub_
gazebo::transport::PublisherPtr motor_velocity_pub_
ignition::math::Vector3d wind_speed_W_
std::string wind_speed_sub_topic_
double rotor_velocity_slowdown_sim_
double rotor_drag_coefficient_
gz_std_msgs::Float32 position_msg_
gz_std_msgs::Float32 turning_velocity_msg_
double NormalizeAngle(double input)
Ensures any input angle is element of [0..2pi)
std::unique_ptr< FirstOrderFilter< double > > rotor_velocity_filter_
static const bool kPrintOnPluginLoad
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
virtual void UpdateForcesAndMoments()
gz_std_msgs::Float32 force_msg_
virtual void OnUpdate(const common::UpdateInfo &)
gazebo::transport::PublisherPtr motor_force_pub_
void ControlCommandCallback(GzCommandMotorInputMsgPtr &command_motor_input_msg)
static const bool kPrintOnUpdates
double rolling_moment_coefficient_
double time_constant_down_
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
static const std::string kConnectGazeboToRosSubtopic
gazebo::transport::SubscriberPtr command_sub_
std::string motor_speed_pub_topic_
static const bool kPrintOnMsgCallback
This class can be used to apply a first order filter on a signal. It allows different acceleration an...
virtual ~GazeboMotorModel()