44 if (!sdf->HasElement(
"robotNamespace"))
46 ROS_INFO_NAMED(
"planar_move",
"PlanarMovePlugin missing <robotNamespace>, " 52 sdf->GetElement(
"robotNamespace")->Get<std::string>();
56 if (!sdf->HasElement (
"enableYAxis"))
58 ROS_INFO_NAMED(
"planar_move",
"PlanarMovePlugin missing <enableYAxis>, " 59 "defaults to \"%d\"", enable_y_axis_);
63 enable_y_axis_ = sdf->GetElement(
"enableYAxis")->Get<
bool>();
67 if (!sdf->HasElement(
"commandTopic"))
69 ROS_WARN_NAMED(
"planar_move",
"PlanarMovePlugin (ns = %s) missing <commandTopic>, " 75 command_topic_ = sdf->GetElement(
"commandTopic")->Get<std::string>();
79 if (!sdf->HasElement(
"odometryTopic"))
81 ROS_WARN_NAMED(
"planar_move",
"PlanarMovePlugin (ns = %s) missing <odometryTopic>, " 87 odometry_topic_ = sdf->GetElement(
"odometryTopic")->Get<std::string>();
91 if (!sdf->HasElement(
"odometryFrame"))
93 ROS_WARN_NAMED(
"planar_move",
"PlanarMovePlugin (ns = %s) missing <odometryFrame>, " 99 odometry_frame_ = sdf->GetElement(
"odometryFrame")->Get<std::string>();
103 if (!sdf->HasElement(
"robotBaseFrame"))
105 ROS_WARN_NAMED(
"planar_move",
"PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, " 106 "defaults to \"%s\"",
111 robot_base_frame_ = sdf->GetElement(
"robotBaseFrame")->Get<std::string>();
115 if (!sdf->HasElement(
"odometryRate"))
117 ROS_WARN_NAMED(
"planar_move",
"PlanarMovePlugin (ns = %s) missing <odometryRate>, " 123 odometry_rate_ = sdf->GetElement(
"odometryRate")->Get<
double>();
126 #if GAZEBO_MAJOR_VERSION >= 8 129 last_odom_publish_time_ =
parent_->GetWorld()->GetSimTime();
131 #if GAZEBO_MAJOR_VERSION >= 8 145 <<
"). A ROS node for Gazebo has not been initialized, " 146 <<
"unable to load plugin. Load the Gazebo system plugin " 147 <<
"'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
160 ros::SubscribeOptions::create<geometry_msgs::Twist>(
command_topic_, 1,
173 event::Events::ConnectWorldUpdateBegin(
181 boost::mutex::scoped_lock scoped_lock(
lock);
182 #if GAZEBO_MAJOR_VERSION >= 8 183 ignition::math::Pose3d pose =
parent_->WorldPose();
185 ignition::math::Pose3d pose =
parent_->GetWorldPose().Ign();
187 float yaw = pose.Rot().Yaw();
188 parent_->SetLinearVel(ignition::math::Vector3d(
189 x_ * cosf(yaw) -
y_ * sinf(yaw),
190 y_ * cosf(yaw) +
x_ * sinf(yaw),
192 parent_->SetAngularVel(ignition::math::Vector3d(0, 0,
rot_));
194 #if GAZEBO_MAJOR_VERSION >= 8 195 common::Time current_time =
parent_->GetWorld()->SimTime();
197 common::Time current_time =
parent_->GetWorld()->GetSimTime();
199 double seconds_since_last_update =
218 const geometry_msgs::Twist::ConstPtr& cmd_msg)
220 boost::mutex::scoped_lock scoped_lock(
lock);
221 x_ = cmd_msg->linear.x;
224 y_ = cmd_msg->linear.y;
226 rot_ = cmd_msg->angular.z;
231 static const double timeout = 0.01;
243 std::string base_footprint_frame =
247 #if GAZEBO_MAJOR_VERSION >= 8 248 ignition::math::Pose3d pose = this->
parent_->WorldPose();
250 ignition::math::Pose3d pose = this->
parent_->GetWorldPose().Ign();
253 tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
254 tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
259 base_footprint_frame));
262 odom_.pose.pose.position.x = pose.Pos().X();
263 odom_.pose.pose.position.y = pose.Pos().Y();
265 odom_.pose.pose.orientation.x = pose.Rot().X();
266 odom_.pose.pose.orientation.y = pose.Rot().Y();
267 odom_.pose.pose.orientation.z = pose.Rot().Z();
268 odom_.pose.pose.orientation.w = pose.Rot().W();
269 odom_.pose.covariance[0] = 0.00001;
270 odom_.pose.covariance[7] = 0.00001;
271 odom_.pose.covariance[14] = 1000000000000.0;
272 odom_.pose.covariance[21] = 1000000000000.0;
273 odom_.pose.covariance[28] = 1000000000000.0;
274 odom_.pose.covariance[35] = 0.001;
277 ignition::math::Vector3d linear;
280 if (
rot_ > M_PI / step_time)
288 float current_yaw = pose.Rot().Yaw();
289 while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI;
290 while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI;
291 float angular_diff = current_yaw - last_yaw;
292 odom_.twist.twist.angular.z = angular_diff / step_time;
297 float yaw = pose.Rot().Yaw();
298 odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y();
299 odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X();
301 odom_.header.stamp = current_time;
302 odom_.header.frame_id = odom_frame;
303 odom_.child_frame_id = base_footprint_frame;
boost::shared_ptr< ros::NodeHandle > rosnode_
ignition::math::Pose3d last_odom_pose_
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
#define ROS_WARN_NAMED(name,...)
void publishOdometry(double step_time)
ROSCPP_DECL bool isInitialized()
ros::CallbackQueue queue_
boost::thread callback_queue_thread_
virtual void UpdateChild()
std::string resolve(const std::string &prefix, const std::string &frame_name)
physics::ModelPtr parent_
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
void Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
#define ROS_FATAL_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
std::string odometry_frame_
std::string robot_base_frame_
common::Time last_odom_publish_time_
ros::Publisher odometry_pub_
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
bool enable_y_axis_
Enable Y-axis movement.
std::string odometry_topic_
std::string command_topic_
boost::shared_ptr< void > VoidPtr
std::string robot_namespace_