diff --git a/gazebo/omniRobot_plugin/include/gazebo_omni_drive.hpp b/gazebo/omniRobot_plugin/include/gazebo_omni_drive.hpp index d5571ad..e2bf1a6 100644 --- a/gazebo/omniRobot_plugin/include/gazebo_omni_drive.hpp +++ b/gazebo/omniRobot_plugin/include/gazebo_omni_drive.hpp @@ -7,25 +7,25 @@ namespace gazebo_omni_drive_plugins { - class GazeboOmniRosDrive : public gazebo::ModelPlugin - { - public: - /// Constructor - GazeboOmniRosDrive(); - - /// Destructor - ~GazeboOmniRosDrive(); - - protected: - // Documentation inherited - void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override; +class GazeboOmniRosDrive : public gazebo::ModelPlugin +{ + public: + /// Constructor + GazeboOmniRosDrive(); + + /// Destructor + ~GazeboOmniRosDrive(); + + protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override; - // Documentation inherited - void Reset() override; - private: - /// Private data pointer - std::unique_ptr impl_; - }; + // Documentation inherited + void Reset() override; + private: + /// Private data pointer + std::unique_ptr impl_; +}; } // namespace gazebo_plugins diff --git a/gazebo/omniRobot_plugin/include/gazebo_omni_drive_private.hpp b/gazebo/omniRobot_plugin/include/gazebo_omni_drive_private.hpp index c6febf9..e3e84e8 100644 --- a/gazebo/omniRobot_plugin/include/gazebo_omni_drive_private.hpp +++ b/gazebo/omniRobot_plugin/include/gazebo_omni_drive_private.hpp @@ -35,171 +35,171 @@ namespace gazebo_omni_drive_plugins { - class GazeboOmniRosDrivePrivate +class GazeboOmniRosDrivePrivate +{ + public: + + /// Indicates which wheel + enum { - public: + NORTH = 0, //left rear + WEST = 1, //left front + SOUTH = 2, //right front + EAST = 3, //right rear + }; - /// Indicates which wheel - enum - { - NORTH = 0, //left rear - WEST = 1, //left front - SOUTH = 2, //right front - EAST = 3, //right rear - }; - - /// Callback to be called at every simulation iteration. - /// \param[in] _info Updated simulation info. - void OnUpdate(const gazebo::common::UpdateInfo & _info); + /// Callback to be called at every simulation iteration. + /// \param[in] _info Updated simulation info. + void OnUpdate(const gazebo::common::UpdateInfo & _info); - /// Callback when a velocity command is received. - /// \param[in] _msg Twist command message. - void OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg); + /// Callback when a velocity command is received. + /// \param[in] _msg Twist command message. + void OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg); - /// Update wheel velocities according to latest target velocities. - void UpdateWheelVelocities(); + /// Update wheel velocities according to latest target velocities. + void UpdateWheelVelocities(); - /// Publish odometry transforms - /// \param[in] _current_time Current simulation time - void PublishOdometryTf(const gazebo::common::Time & _current_time); + /// Publish odometry transforms + /// \param[in] _current_time Current simulation time + void PublishOdometryTf(const gazebo::common::Time & _current_time); - /// Publish trasforms for the wheels - /// \param[in] _current_time Current simulation time - void PublishWheelsTf(const gazebo::common::Time & _current_time); + /// Publish trasforms for the wheels + /// \param[in] _current_time Current simulation time + void PublishWheelsTf(const gazebo::common::Time & _current_time); - /// Publish odometry messages - /// \param[in] _current_time Current simulation time - void PublishOdometryMsg(const gazebo::common::Time & _current_time); + /// Publish odometry messages + /// \param[in] _current_time Current simulation time + void PublishOdometryMsg(const gazebo::common::Time & _current_time); - /// A pointer to the GazeboROS node. - gazebo_ros::Node::SharedPtr ros_node_; + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; - /// Subscriber to command velocities - rclcpp::Subscription::SharedPtr cmd_vel_sub_; + /// Subscriber to command velocities + rclcpp::Subscription::SharedPtr cmd_vel_sub_; - /// Odometry publisher - rclcpp::Publisher::SharedPtr odometry_pub_; + /// Odometry publisher + rclcpp::Publisher::SharedPtr odometry_pub_; - /// Connection to event called at every world iteration. - gazebo::event::ConnectionPtr update_connection_; + /// Connection to event called at every world iteration. + gazebo::event::ConnectionPtr update_connection_; - /// Distance between the wheels, in meters. - std::vector wheel_w_separation_; - std::vector wheel_l_separation_; + /// Distance between the wheels, in meters. + std::vector wheel_w_separation_; + std::vector wheel_l_separation_; - /// Diameter of wheels, in meters. - std::vector wheel_diameter_; + /// Diameter of wheels, in meters. + std::vector wheel_diameter_; - /// Maximum wheel torque, in Nm. - double max_wheel_torque_; + /// Maximum wheel torque, in Nm. + double max_wheel_torque_; - /// Maximum wheel acceleration - double max_wheel_accel_; + /// Maximum wheel acceleration + double max_wheel_accel_; - /// Desired wheel speed. - std::vector desired_wheel_speed_; + /// Desired wheel speed. + std::vector desired_wheel_speed_; - /// Speed sent to wheel. - std::vector wheel_speed_instr_;// Unless required by applicable law or agreed to in writing, software + /// Speed sent to wheel. + std::vector wheel_speed_instr_;// Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. - /// Pointers to wheel joints. - std::vector joints_; + /// Pointers to wheel joints. + std::vector joints_; - /// Pointer to model. - gazebo::physics::ModelPtr model_; + /// Pointer to model. + gazebo::physics::ModelPtr model_; - /// To broadcast TFs - std::shared_ptr transform_broadcaster_; + /// To broadcast TFs + std::shared_ptr transform_broadcaster_; - /// Protect variables accessed on callbacks. - std::mutex lock_; + /// Protect variables accessed on callbacks. + std::mutex lock_; - /// Linear velocity in X received on command (m/s). - double target_x_{0.0}; + /// Linear velocity in X received on command (m/s). + double target_x_{0.0}; - /// Linear velocity in Y received on command (m/s). - double target_y_{0.0}; + /// Linear velocity in Y received on command (m/s). + double target_y_{0.0}; - /// Angular velocity in Z received on command (rad/s). - double target_rot_{0.0}; + /// Angular velocity in Z received on command (rad/s). + double target_rot_{0.0}; - /// Update period in seconds. - double update_period_; + /// Update period in seconds. + double update_period_; - /// Last update time. - gazebo::common::Time last_update_time_; + /// Last update time. + gazebo::common::Time last_update_time_; - /// Keep encoder data. - geometry_msgs::msg::Pose2D pose_encoder_; + /// Keep encoder data. + geometry_msgs::msg::Pose2D pose_encoder_; - /// Odometry frame ID - std::string odometry_frame_; + /// Odometry frame ID + std::string odometry_frame_; - /// Odometry topic - std::string odometry_topic_; + /// Odometry topic + std::string odometry_topic_; - /// cmd topic - std::string cmd_topic_; + /// cmd topic + std::string cmd_topic_; - /// Last time the encoder was updated - gazebo::common::Time last_encoder_update_; + /// Last time the encoder was updated + gazebo::common::Time last_encoder_update_; - /// Keep latest odometry message - nav_msgs::msg::Odometry odom_; + /// Keep latest odometry message + nav_msgs::msg::Odometry odom_; - /// Robot base frame ID - std::string robot_base_frame_; + /// Robot base frame ID + std::string robot_base_frame_; - /// True to publish odometry messages. - bool publish_odom_; + /// True to publish odometry messages. + bool publish_odom_; - /// True to publish wheel-to-base transforms. - bool publish_wheel_tf_; + /// True to publish wheel-to-base transforms. + bool publish_wheel_tf_; - /// True to publish odom-to-world transforms. - bool publish_odom_tf_; + /// True to publish odom-to-world transforms. + bool publish_odom_tf_; - /// Store number of wheel pairs - unsigned int num_wheel_pairs_; + /// Store number of wheel pairs + unsigned int num_wheel_pairs_; - /// Covariance in odometry - double covariance_[3]; + /// Covariance in odometry + double covariance_[3]; - /// Has the agv model roller model. - bool isRollerModel_; + /// Has the agv model roller model. + bool isRollerModel_; - /// Get linear velocity after calculating mecanum kinematics. - struct linear_vel - { - double vel_x; - double vel_y; - double vel_th; - }; + /// Get linear velocity after calculating mecanum kinematics. + struct linear_vel + { + double vel_x; + double vel_y; + double vel_th; + }; - linear_vel cal_LineVel_; + linear_vel cal_LineVel_; - struct pid_parameter - { - double p_gain; - double i_gain; - double d_gain; - }; + struct pid_parameter + { + double p_gain; + double i_gain; + double d_gain; + }; - pid_parameter pid_lr, pid_lf, pid_rf, pid_rr; + pid_parameter pid_lr, pid_lf, pid_rf, pid_rr; - /** - * @brief PID control for wheel velocity. - * - * TODO : Update calculation if wheel velocities from mecanum to omniwheel - */ - void calkinematics(linear_vel &line_vel); - void gazebo_PID_WHEEL_(gazebo::physics::JointPtr joint, pid_parameter &pid, double vel); - }; + /** + * @brief PID control for wheel velocity. + * + * TODO : Update calculation if wheel velocities from mecanum to omniwheel + */ + void calkinematics(linear_vel &line_vel); + void gazebo_PID_WHEEL_(gazebo::physics::JointPtr joint, pid_parameter &pid, double vel); +}; } // namespace gazebo_plugins diff --git a/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp b/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp index 808e001..8e96de4 100644 --- a/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp +++ b/gazebo/omniRobot_plugin/src/gazebo_omni_drive.cpp @@ -3,494 +3,494 @@ namespace gazebo_omni_drive_plugins { - GazeboOmniRosDrive::GazeboOmniRosDrive() : impl_(std::make_unique()) - { +GazeboOmniRosDrive::GazeboOmniRosDrive() : impl_(std::make_unique()) +{ - } - - GazeboOmniRosDrive::~GazeboOmniRosDrive() - { +} - } +GazeboOmniRosDrive::~GazeboOmniRosDrive() +{ + +} + +void GazeboOmniRosDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + impl_->model_ = _model; - void GazeboOmniRosDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) - { - impl_->model_ = _model; - - // Initialize ROS node - impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); - // Get QoS profiles - const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos(); + // Get QoS profiles + const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos(); - // Get number of wheel pairs in the model - impl_->num_wheel_pairs_ = static_cast(_sdf->Get("num_wheel_pairs", 1).first); + // Get number of wheel pairs in the model + impl_->num_wheel_pairs_ = static_cast(_sdf->Get("num_wheel_pairs", 1).first); - if (impl_->num_wheel_pairs_ < 1) - { - impl_->num_wheel_pairs_ = 1; - RCLCPP_WARN(impl_->ros_node_->get_logger(), "Drive requires at least one pair of wheels. Setting [num_wheel_pairs] to 1"); - } + if (impl_->num_wheel_pairs_ < 1) + { + impl_->num_wheel_pairs_ = 1; + RCLCPP_WARN(impl_->ros_node_->get_logger(), "Drive requires at least one pair of wheels. Setting [num_wheel_pairs] to 1"); + } - // Dynamic properties - impl_->max_wheel_accel_ = _sdf->Get("wheelAccel", 0.0).first; - impl_->max_wheel_torque_ = _sdf->Get("WheelTorque", 5.0).first; + // Dynamic properties + impl_->max_wheel_accel_ = _sdf->Get("wheelAccel", 0.0).first; + impl_->max_wheel_torque_ = _sdf->Get("WheelTorque", 5.0).first; - // Get joints and Kinematic properties - std::vector north_joints, west_joints, south_joints, east_joints; + // Get joints and Kinematic properties + std::vector north_joints, west_joints, south_joints, east_joints; - for (auto north_joint_elem = _sdf->GetElement("North"); - north_joint_elem != nullptr; - north_joint_elem = north_joint_elem->GetNextElement("North")) + for (auto north_joint_elem = _sdf->GetElement("North"); + north_joint_elem != nullptr; + north_joint_elem = north_joint_elem->GetNextElement("North")) + { + auto north_joint_name = north_joint_elem->Get(); + auto north_joint = _model->GetJoint(north_joint_name); + if (!north_joint) { - auto north_joint_name = north_joint_elem->Get(); - auto north_joint = _model->GetJoint(north_joint_name); - if (!north_joint) - { - RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", north_joint_name.c_str()); - impl_->ros_node_.reset(); - return; - } - - north_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); - north_joints.push_back(north_joint); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", north_joint_name.c_str()); + impl_->ros_node_.reset(); + return; } + + north_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); + north_joints.push_back(north_joint); + } - for (auto west_joint_elem = _sdf->GetElement("West"); - west_joint_elem != nullptr; - west_joint_elem = west_joint_elem->GetNextElement("West")) + for (auto west_joint_elem = _sdf->GetElement("West"); + west_joint_elem != nullptr; + west_joint_elem = west_joint_elem->GetNextElement("West")) + { + auto west_joint_name = west_joint_elem->Get(); + auto west_joint = _model->GetJoint(west_joint_name); + if (!west_joint) { - auto west_joint_name = west_joint_elem->Get(); - auto west_joint = _model->GetJoint(west_joint_name); - if (!west_joint) - { - RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", west_joint_name.c_str()); - impl_->ros_node_.reset(); - return; - } - - west_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); - west_joints.push_back(west_joint); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", west_joint_name.c_str()); + impl_->ros_node_.reset(); + return; } - for (auto south_joint_elem = _sdf->GetElement("South"); - south_joint_elem != nullptr; - south_joint_elem = south_joint_elem->GetNextElement("South")) + west_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); + west_joints.push_back(west_joint); + } + + for (auto south_joint_elem = _sdf->GetElement("South"); + south_joint_elem != nullptr; + south_joint_elem = south_joint_elem->GetNextElement("South")) + { + auto south_joint_name = south_joint_elem->Get(); + auto south_joint = _model->GetJoint(south_joint_name); + if (!south_joint) { - auto south_joint_name = south_joint_elem->Get(); - auto south_joint = _model->GetJoint(south_joint_name); - if (!south_joint) - { - RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", south_joint_name.c_str()); - impl_->ros_node_.reset(); - return; - } - - south_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); - south_joints.push_back(south_joint); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", south_joint_name.c_str()); + impl_->ros_node_.reset(); + return; } + + south_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); + south_joints.push_back(south_joint); + } - for (auto east_joint_elem = _sdf->GetElement("East"); - east_joint_elem != nullptr; - east_joint_elem = east_joint_elem->GetNextElement("East")) + for (auto east_joint_elem = _sdf->GetElement("East"); + east_joint_elem != nullptr; + east_joint_elem = east_joint_elem->GetNextElement("East")) + { + auto east_joint_name = east_joint_elem->Get(); + auto east_joint = _model->GetJoint(east_joint_name); + if (!east_joint) { - auto east_joint_name = east_joint_elem->Get(); - auto east_joint = _model->GetJoint(east_joint_name); - if (!east_joint) - { - RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", east_joint_name.c_str()); - impl_->ros_node_.reset(); - return; - } - - east_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); - east_joints.push_back(east_joint); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint [%s] not found, plugin will not work.", east_joint_name.c_str()); + impl_->ros_node_.reset(); + return; } + + east_joint->SetParam("fmax", 0, impl_->max_wheel_torque_); + east_joints.push_back(east_joint); + } - unsigned int index; - for (index = 0; index < impl_->num_wheel_pairs_; ++index) + unsigned int index; + for (index = 0; index < impl_->num_wheel_pairs_; ++index) + { + impl_->joints_.push_back(north_joints[index]); + impl_->joints_.push_back(west_joints[index]); + impl_->joints_.push_back(south_joints[index]); + impl_->joints_.push_back(east_joints[index]); + } + + index = 0; + impl_->wheel_w_separation_.assign(impl_->num_wheel_pairs_, 0.34); + for (auto wheel_w_separation = _sdf->GetElement("WheelSeparationW"); + wheel_w_separation != nullptr; + wheel_w_separation = wheel_w_separation->GetNextElement("WheelSeparationW")) + { + if (index >= impl_->num_wheel_pairs_) { - impl_->joints_.push_back(north_joints[index]); - impl_->joints_.push_back(west_joints[index]); - impl_->joints_.push_back(south_joints[index]); - impl_->joints_.push_back(east_joints[index]); + RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); + break; } - index = 0; - impl_->wheel_w_separation_.assign(impl_->num_wheel_pairs_, 0.34); - for (auto wheel_w_separation = _sdf->GetElement("WheelSeparationW"); - wheel_w_separation != nullptr; - wheel_w_separation = wheel_w_separation->GetNextElement("WheelSeparationW")) - { - if (index >= impl_->num_wheel_pairs_) - { - RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); - break; - } - - impl_->wheel_w_separation_[index] = wheel_w_separation->Get(); - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Wheel pair %i separation width set to [%fm]", index + 1, impl_->wheel_w_separation_[index]); - index++; - } + impl_->wheel_w_separation_[index] = wheel_w_separation->Get(); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Wheel pair %i separation width set to [%fm]", index + 1, impl_->wheel_w_separation_[index]); + index++; + } - index = 0; - impl_->wheel_l_separation_.assign(impl_->num_wheel_pairs_, 0.34); - for (auto wheel_l_separation = _sdf->GetElement("WheelSeparationL"); - wheel_l_separation != nullptr; - wheel_l_separation = wheel_l_separation->GetNextElement("WheelSeparationL")) + index = 0; + impl_->wheel_l_separation_.assign(impl_->num_wheel_pairs_, 0.34); + for (auto wheel_l_separation = _sdf->GetElement("WheelSeparationL"); + wheel_l_separation != nullptr; + wheel_l_separation = wheel_l_separation->GetNextElement("WheelSeparationL")) + { + if (index >= impl_->num_wheel_pairs_) { - if (index >= impl_->num_wheel_pairs_) - { - RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); - break; - } - - impl_->wheel_l_separation_[index] = wheel_l_separation->Get(); - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Wheel pair %i separation legth set to [%fm]", index + 1, impl_->wheel_l_separation_[index]); - index++; + RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); + break; } - index = 0; - impl_->wheel_diameter_.assign(impl_->num_wheel_pairs_, 0.15); - for (auto wheel_diameter = _sdf->GetElement("wheelDiameter"); - wheel_diameter != nullptr; - wheel_diameter = wheel_diameter->GetNextElement("wheelDiameter")) + impl_->wheel_l_separation_[index] = wheel_l_separation->Get(); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Wheel pair %i separation legth set to [%fm]", index + 1, impl_->wheel_l_separation_[index]); + index++; + } + + index = 0; + impl_->wheel_diameter_.assign(impl_->num_wheel_pairs_, 0.15); + for (auto wheel_diameter = _sdf->GetElement("wheelDiameter"); + wheel_diameter != nullptr; + wheel_diameter = wheel_diameter->GetNextElement("wheelDiameter")) + { + if (index >= impl_->num_wheel_pairs_) { - if (index >= impl_->num_wheel_pairs_) - { - RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); - break; - } - - impl_->wheel_diameter_[index] = wheel_diameter->Get(); - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Wheel pair %i diameter set to [%fm]", index + 1, impl_->wheel_diameter_[index]); - index++; + RCLCPP_WARN(impl_->ros_node_->get_logger(), "Ignoring rest of specified "); + break; } - impl_->wheel_speed_instr_.assign(4 * impl_->num_wheel_pairs_, 0); - impl_->desired_wheel_speed_.assign(4 * impl_->num_wheel_pairs_, 0); - - // Update rate - auto update_rate = _sdf->Get("odometryRate", 20.0).first; - if (update_rate > 0.0) - { - impl_->update_period_ = 1.0 / update_rate; - } - else - { - impl_->update_period_ = 0.0; - } + impl_->wheel_diameter_[index] = wheel_diameter->Get(); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Wheel pair %i diameter set to [%fm]", index + 1, impl_->wheel_diameter_[index]); + index++; + } + + impl_->wheel_speed_instr_.assign(4 * impl_->num_wheel_pairs_, 0); + impl_->desired_wheel_speed_.assign(4 * impl_->num_wheel_pairs_, 0); + + // Update rate + auto update_rate = _sdf->Get("odometryRate", 20.0).first; + if (update_rate > 0.0) + { + impl_->update_period_ = 1.0 / update_rate; + } + else + { + impl_->update_period_ = 0.0; + } - impl_->cmd_topic_ = _sdf->Get("commandTopic", "cmd_vel").first; - impl_->last_update_time_ = _model->GetWorld()->SimTime(); - impl_->cmd_vel_sub_ = impl_->ros_node_->create_subscription(impl_->cmd_topic_, qos.get_subscription_qos(impl_->cmd_topic_, rclcpp::QoS(1)), - std::bind(&GazeboOmniRosDrivePrivate::OnCmdVel, impl_.get(), std::placeholders::_1)); - - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", impl_->cmd_vel_sub_->get_topic_name()); - // Odometry - impl_->odometry_frame_ = _sdf->Get("odometryFrame", "odom").first; - impl_->robot_base_frame_ = _sdf->Get("robotBaseFrame", "base_footprint").first; - - // Advertise odometry topic - impl_->publish_odom_ = _sdf->Get("publishOdom", false).first; - if(impl_->publish_odom_) + impl_->cmd_topic_ = _sdf->Get("commandTopic", "cmd_vel").first; + impl_->last_update_time_ = _model->GetWorld()->SimTime(); + impl_->cmd_vel_sub_ = impl_->ros_node_->create_subscription(impl_->cmd_topic_, qos.get_subscription_qos(impl_->cmd_topic_, rclcpp::QoS(1)), + std::bind(&GazeboOmniRosDrivePrivate::OnCmdVel, impl_.get(), std::placeholders::_1)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", impl_->cmd_vel_sub_->get_topic_name()); + // Odometry + impl_->odometry_frame_ = _sdf->Get("odometryFrame", "odom").first; + impl_->robot_base_frame_ = _sdf->Get("robotBaseFrame", "base_footprint").first; + + // Advertise odometry topic + impl_->publish_odom_ = _sdf->Get("publishOdom", false).first; + if(impl_->publish_odom_) + { + impl_->odometry_topic_ = _sdf->Get("odometryTopic", "odom").first; + impl_->odometry_pub_ = impl_->ros_node_->create_publisher(impl_->odometry_topic_, qos.get_publisher_qos(impl_->odometry_topic_, rclcpp::QoS(1))); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise odometry on [%s]", impl_->odometry_pub_->get_topic_name()); + } + + // Create TF broadcaster if needed + impl_->publish_wheel_tf_ = _sdf->Get("publishWheelTF", false).first; + impl_->publish_odom_tf_ = _sdf->Get("publishOdomTF", false).first; + if(impl_->publish_wheel_tf_ || impl_->publish_odom_tf_) + { + impl_->transform_broadcaster_ = std::make_shared(impl_->ros_node_); + if(impl_->publish_odom_tf_) { - impl_->odometry_topic_ = _sdf->Get("odometryTopic", "odom").first; - impl_->odometry_pub_ = impl_->ros_node_->create_publisher(impl_->odometry_topic_, qos.get_publisher_qos(impl_->odometry_topic_, rclcpp::QoS(1))); - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise odometry on [%s]", impl_->odometry_pub_->get_topic_name()); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing odom transforms between [%s] and [%s]", impl_->odometry_frame_.c_str(), + impl_->robot_base_frame_.c_str()); } - // Create TF broadcaster if needed - impl_->publish_wheel_tf_ = _sdf->Get("publishWheelTF", false).first; - impl_->publish_odom_tf_ = _sdf->Get("publishOdomTF", false).first; - if(impl_->publish_wheel_tf_ || impl_->publish_odom_tf_) + for (index = 0; index < impl_->num_wheel_pairs_; ++index) { - impl_->transform_broadcaster_ = std::make_shared(impl_->ros_node_); - if(impl_->publish_odom_tf_) + if(impl_->publish_wheel_tf_) { - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing odom transforms between [%s] and [%s]", impl_->odometry_frame_.c_str(), - impl_->robot_base_frame_.c_str()); - } - - for (index = 0; index < impl_->num_wheel_pairs_; ++index) - { - if(impl_->publish_wheel_tf_) - { - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing wheel transforms between [%s], [%s] , [%s] , [%s], [%s]", - impl_->robot_base_frame_.c_str(), - impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::NORTH]->GetName().c_str(), - impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::WEST]->GetName().c_str(), - impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::SOUTH]->GetName().c_str(), - impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::EAST]->GetName().c_str()); - } + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing wheel transforms between [%s], [%s] , [%s] , [%s], [%s]", + impl_->robot_base_frame_.c_str(), + impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::NORTH]->GetName().c_str(), + impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::WEST]->GetName().c_str(), + impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::SOUTH]->GetName().c_str(), + impl_->joints_[2 * index + GazeboOmniRosDrivePrivate::EAST]->GetName().c_str()); } } - - impl_->isRollerModel_ = _sdf->Get("isRollerModel", true).first; - impl_->covariance_[0] = _sdf->Get("covariance_x", 0.00001).first; - impl_->covariance_[1] = _sdf->Get("covariance_y", 0.00001).first; - impl_->covariance_[2] = _sdf->Get("covariance_yaw", 0.001).first; - - // Listen to the update event (broadcast every simulation iteration) - impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(std::bind(&GazeboOmniRosDrivePrivate::OnUpdate, impl_.get(), std::placeholders::_1)); } + + impl_->isRollerModel_ = _sdf->Get("isRollerModel", true).first; + impl_->covariance_[0] = _sdf->Get("covariance_x", 0.00001).first; + impl_->covariance_[1] = _sdf->Get("covariance_y", 0.00001).first; + impl_->covariance_[2] = _sdf->Get("covariance_yaw", 0.001).first; - void GazeboOmniRosDrive::Reset() + // Listen to the update event (broadcast every simulation iteration) + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(std::bind(&GazeboOmniRosDrivePrivate::OnUpdate, impl_.get(), std::placeholders::_1)); +} + +void GazeboOmniRosDrive::Reset() +{ + impl_->last_update_time_ = impl_->joints_[GazeboOmniRosDrivePrivate::NORTH]->GetWorld()->SimTime(); + for(unsigned int i = 0; i < impl_->num_wheel_pairs_; ++i) { - impl_->last_update_time_ = impl_->joints_[GazeboOmniRosDrivePrivate::NORTH]->GetWorld()->SimTime(); - for(unsigned int i = 0; i < impl_->num_wheel_pairs_; ++i) + if (impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::NORTH] && impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::WEST] && + impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::SOUTH] && impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::EAST]) { - if (impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::NORTH] && impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::WEST] && - impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::SOUTH] && impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::EAST]) - { - impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::NORTH]->SetParam("fmax", 0, impl_->max_wheel_torque_); - impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::WEST]->SetParam("fmax", 0, impl_->max_wheel_torque_); - impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::SOUTH]->SetParam("fmax", 0, impl_->max_wheel_torque_); - impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::EAST]->SetParam("fmax", 0, impl_->max_wheel_torque_); - } + impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::NORTH]->SetParam("fmax", 0, impl_->max_wheel_torque_); + impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::WEST]->SetParam("fmax", 0, impl_->max_wheel_torque_); + impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::SOUTH]->SetParam("fmax", 0, impl_->max_wheel_torque_); + impl_->joints_[2 * i + GazeboOmniRosDrivePrivate::EAST]->SetParam("fmax", 0, impl_->max_wheel_torque_); } - - impl_->pose_encoder_.x = 0; - impl_->pose_encoder_.y = 0; - impl_->pose_encoder_.theta = 0; - impl_->target_x_ = 0; - impl_->target_rot_ = 0; + } + + impl_->pose_encoder_.x = 0; + impl_->pose_encoder_.y = 0; + impl_->pose_encoder_.theta = 0; + impl_->target_x_ = 0; + impl_->target_rot_ = 0; +} + +void GazeboOmniRosDrivePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info) +{ +#ifdef IGN_PROFILER_ENABLE + IGN_PROFILE("GazeboRosDiffDrivePrivate::OnUpdate"); +#endif + + double seconds_since_last_update = (_info.simTime - last_update_time_).Double(); + if(seconds_since_last_update < update_period_) + { + return; } - void GazeboOmniRosDrivePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info) +#ifdef IGN_PROFILER_ENABLE + IGN_PROFILE_END(); + IGN_PROFILE_BEGIN("PublishOdometryMsg"); +#endif + if(publish_odom_) { + PublishOdometryMsg(_info.simTime); + } #ifdef IGN_PROFILER_ENABLE - IGN_PROFILE("GazeboRosDiffDrivePrivate::OnUpdate"); - #endif - - double seconds_since_last_update = (_info.simTime - last_update_time_).Double(); - if(seconds_since_last_update < update_period_) - { - return; - } + IGN_PROFILE_END(); + IGN_PROFILE_BEGIN("PublishWheelsTf"); +#endif + if(publish_wheel_tf_) + { + PublishWheelsTf(_info.simTime); + } +#ifdef IGN_PROFILER_ENABLE + IGN_PROFILE_END(); + IGN_PROFILE_BEGIN("PublishOdometryTf"); +#endif + if(publish_odom_tf_) + { + PublishOdometryTf(_info.simTime); + } +#ifdef IGN_PROFILER_ENABLE + IGN_PROFILE_END(); + IGN_PROFILE_BEGIN("UpdateWheelVelocities"); +#endif +// Update robot in case new velocities have been requested +UpdateWheelVelocities(); +#ifdef IGN_PROFILER_ENABLE + IGN_PROFILE_END(); +#endif + // Current speed + std::vector current_speed(4 * num_wheel_pairs_); + for(unsigned int i = 0; i < num_wheel_pairs_; ++i) + { + current_speed[2 * i + NORTH] = joints_[2 * i + NORTH]->GetVelocity(0); + current_speed[2 * i + WEST] = joints_[2 * i + WEST]->GetVelocity(0); + current_speed[2 * i + SOUTH] = joints_[2 * i + SOUTH]->GetVelocity(0); + current_speed[2 * i + EAST] = joints_[2 * i + EAST]->GetVelocity(0); + } - #ifdef IGN_PROFILER_ENABLE - IGN_PROFILE_END(); - IGN_PROFILE_BEGIN("PublishOdometryMsg"); - #endif - if(publish_odom_) - { - PublishOdometryMsg(_info.simTime); - } - #ifdef IGN_PROFILER_ENABLE - IGN_PROFILE_END(); - IGN_PROFILE_BEGIN("PublishWheelsTf"); - #endif - if(publish_wheel_tf_) - { - PublishWheelsTf(_info.simTime); - } - #ifdef IGN_PROFILER_ENABLE - IGN_PROFILE_END(); - IGN_PROFILE_BEGIN("PublishOdometryTf"); - #endif - if(publish_odom_tf_) - { - PublishOdometryTf(_info.simTime); - } - #ifdef IGN_PROFILER_ENABLE - IGN_PROFILE_END(); - IGN_PROFILE_BEGIN("UpdateWheelVelocities"); - #endif - // Update robot in case new velocities have been requested - UpdateWheelVelocities(); - #ifdef IGN_PROFILER_ENABLE - IGN_PROFILE_END(); - #endif - // Current speed - std::vector current_speed(4 * num_wheel_pairs_); - for(unsigned int i = 0; i < num_wheel_pairs_; ++i) + // If max_accel == 0, or target speed is reached + for(unsigned int i = 0; i < num_wheel_pairs_; ++i) + { + if(max_wheel_accel_ == 0 || + ((fabs(desired_wheel_speed_[2 * i + NORTH] - current_speed[2 * i + NORTH]) < 0.01) && + (fabs(desired_wheel_speed_[2 * i + WEST] - current_speed[2 * i + WEST]) < 0.01) && + (fabs(desired_wheel_speed_[2 * i + SOUTH] - current_speed[2 * i + SOUTH]) < 0.01) && + (fabs(desired_wheel_speed_[2 * i + EAST] - current_speed[2 * i + EAST]) < 0.01))) { - current_speed[2 * i + NORTH] = joints_[2 * i + NORTH]->GetVelocity(0); - current_speed[2 * i + WEST] = joints_[2 * i + WEST]->GetVelocity(0); - current_speed[2 * i + SOUTH] = joints_[2 * i + SOUTH]->GetVelocity(0); - current_speed[2 * i + EAST] = joints_[2 * i + EAST]->GetVelocity(0); + joints_[2 * i + NORTH]->SetParam("vel", 0, desired_wheel_speed_[2 * i + NORTH]); + joints_[2 * i + WEST]->SetParam("vel", 0, desired_wheel_speed_[2 * i + WEST]); + joints_[2 * i + SOUTH]->SetParam("vel", 0, desired_wheel_speed_[2 * i + SOUTH]); + joints_[2 * i + EAST]->SetParam("vel", 0, desired_wheel_speed_[2 * i + EAST]); } - - // If max_accel == 0, or target speed is reached - for(unsigned int i = 0; i < num_wheel_pairs_; ++i) + else { - if(max_wheel_accel_ == 0 || - ((fabs(desired_wheel_speed_[2 * i + NORTH] - current_speed[2 * i + NORTH]) < 0.01) && - (fabs(desired_wheel_speed_[2 * i + WEST] - current_speed[2 * i + WEST]) < 0.01) && - (fabs(desired_wheel_speed_[2 * i + SOUTH] - current_speed[2 * i + SOUTH]) < 0.01) && - (fabs(desired_wheel_speed_[2 * i + EAST] - current_speed[2 * i + EAST]) < 0.01))) + if(desired_wheel_speed_[2 * i + NORTH] >= current_speed[2 * i + NORTH]) { - joints_[2 * i + NORTH]->SetParam("vel", 0, desired_wheel_speed_[2 * i + NORTH]); - joints_[2 * i + WEST]->SetParam("vel", 0, desired_wheel_speed_[2 * i + WEST]); - joints_[2 * i + SOUTH]->SetParam("vel", 0, desired_wheel_speed_[2 * i + SOUTH]); - joints_[2 * i + EAST]->SetParam("vel", 0, desired_wheel_speed_[2 * i + EAST]); + wheel_speed_instr_[2 * i + NORTH] += fmin(desired_wheel_speed_[2 * i + NORTH] - current_speed[2 * i + NORTH], max_wheel_accel_ * seconds_since_last_update); } else { - if(desired_wheel_speed_[2 * i + NORTH] >= current_speed[2 * i + NORTH]) - { - wheel_speed_instr_[2 * i + NORTH] += fmin(desired_wheel_speed_[2 * i + NORTH] - current_speed[2 * i + NORTH], max_wheel_accel_ * seconds_since_last_update); - } - else - { - wheel_speed_instr_[2 * i + NORTH] += fmax(desired_wheel_speed_[2 * i + NORTH] - current_speed[2 * i + NORTH], -max_wheel_accel_ * seconds_since_last_update); - } - - if(desired_wheel_speed_[2 * i + WEST] >= current_speed[2 * i + WEST]) - { - wheel_speed_instr_[2 * i + WEST] += fmin(desired_wheel_speed_[2 * i + WEST] - current_speed[2 * i + WEST], max_wheel_accel_ * seconds_since_last_update); - } - else - { - wheel_speed_instr_[2 * i + WEST] += fmax(desired_wheel_speed_[2 * i + WEST] - current_speed[2 * i + WEST], -max_wheel_accel_ * seconds_since_last_update); - } - - if (desired_wheel_speed_[2 * i + SOUTH] > current_speed[2 * i + SOUTH]) - { - wheel_speed_instr_[2 * i + SOUTH] += fmin(desired_wheel_speed_[2 * i + SOUTH] - current_speed[2 * i + SOUTH], max_wheel_accel_ * seconds_since_last_update); - } - else - { - wheel_speed_instr_[2 * i + SOUTH] += fmax(desired_wheel_speed_[2 * i + SOUTH] - current_speed[2 * i + SOUTH], -max_wheel_accel_ * seconds_since_last_update); - } - - if (desired_wheel_speed_[2 * i + EAST] > current_speed[2 * i + EAST]) - { - wheel_speed_instr_[2 * i + EAST] += fmin(desired_wheel_speed_[2 * i + EAST] - current_speed[2 * i + EAST], max_wheel_accel_ * seconds_since_last_update); - } - else - { - wheel_speed_instr_[2 * i + EAST] += fmax(desired_wheel_speed_[2 * i + EAST] - current_speed[2 * i + EAST], -max_wheel_accel_ * seconds_since_last_update); - } - - joints_[2 * i + NORTH]->SetParam("vel", 0, wheel_speed_instr_[2 * i + NORTH]); - joints_[2 * i + WEST]->SetParam("vel", 0, wheel_speed_instr_[2 * i + WEST]); - joints_[2 * i + SOUTH]->SetParam("vel", 0, wheel_speed_instr_[2 * i + SOUTH]); - joints_[2 * i + EAST]->SetParam("vel", 0, wheel_speed_instr_[2 * i + EAST]); + wheel_speed_instr_[2 * i + NORTH] += fmax(desired_wheel_speed_[2 * i + NORTH] - current_speed[2 * i + NORTH], -max_wheel_accel_ * seconds_since_last_update); } - } - if(!isRollerModel_) - { - ignition::math::Pose3d pose = model_->WorldPose(); - float yaw = pose.Rot().Yaw(); - calkinematics(cal_LineVel_); - model_->SetLinearVel(ignition::math::Vector3d(cal_LineVel_.vel_x * cosf(yaw) - cal_LineVel_.vel_y * sinf(yaw), - cal_LineVel_.vel_y * cosf(yaw) + cal_LineVel_.vel_x * sinf(yaw), 0)); - model_->SetAngularVel(ignition::math::Vector3d(0, 0, cal_LineVel_.vel_th)); - } - - last_update_time_ = _info.simTime; - } + if(desired_wheel_speed_[2 * i + WEST] >= current_speed[2 * i + WEST]) + { + wheel_speed_instr_[2 * i + WEST] += fmin(desired_wheel_speed_[2 * i + WEST] - current_speed[2 * i + WEST], max_wheel_accel_ * seconds_since_last_update); + } + else + { + wheel_speed_instr_[2 * i + WEST] += fmax(desired_wheel_speed_[2 * i + WEST] - current_speed[2 * i + WEST], -max_wheel_accel_ * seconds_since_last_update); + } - void GazeboOmniRosDrivePrivate::UpdateWheelVelocities() - { - std::lock_guard scoped_lock(lock_); + if (desired_wheel_speed_[2 * i + SOUTH] > current_speed[2 * i + SOUTH]) + { + wheel_speed_instr_[2 * i + SOUTH] += fmin(desired_wheel_speed_[2 * i + SOUTH] - current_speed[2 * i + SOUTH], max_wheel_accel_ * seconds_since_last_update); + } + else + { + wheel_speed_instr_[2 * i + SOUTH] += fmax(desired_wheel_speed_[2 * i + SOUTH] - current_speed[2 * i + SOUTH], -max_wheel_accel_ * seconds_since_last_update); + } - double vx = target_x_; - double vy = target_y_; - double va = target_rot_; + if (desired_wheel_speed_[2 * i + EAST] > current_speed[2 * i + EAST]) + { + wheel_speed_instr_[2 * i + EAST] += fmin(desired_wheel_speed_[2 * i + EAST] - current_speed[2 * i + EAST], max_wheel_accel_ * seconds_since_last_update); + } + else + { + wheel_speed_instr_[2 * i + EAST] += fmax(desired_wheel_speed_[2 * i + EAST] - current_speed[2 * i + EAST], -max_wheel_accel_ * seconds_since_last_update); + } - for (unsigned int i = 0; i < num_wheel_pairs_; ++i) - { - desired_wheel_speed_[2 * i + NORTH] = (vx + vy - va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; - desired_wheel_speed_[2 * i + WEST] = (vx - vy - va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; - desired_wheel_speed_[2 * i + SOUTH] = (vx + vy + va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; - desired_wheel_speed_[2 * i + EAST] = (vx - vy + va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; + joints_[2 * i + NORTH]->SetParam("vel", 0, wheel_speed_instr_[2 * i + NORTH]); + joints_[2 * i + WEST]->SetParam("vel", 0, wheel_speed_instr_[2 * i + WEST]); + joints_[2 * i + SOUTH]->SetParam("vel", 0, wheel_speed_instr_[2 * i + SOUTH]); + joints_[2 * i + EAST]->SetParam("vel", 0, wheel_speed_instr_[2 * i + EAST]); } } - void GazeboOmniRosDrivePrivate::OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg) + if(!isRollerModel_) { - std::lock_guard scoped_lock(lock_); - target_x_ = _msg->linear.x; - target_y_ = _msg->linear.y; - target_rot_ = _msg->angular.z; + ignition::math::Pose3d pose = model_->WorldPose(); + float yaw = pose.Rot().Yaw(); + calkinematics(cal_LineVel_); + model_->SetLinearVel(ignition::math::Vector3d(cal_LineVel_.vel_x * cosf(yaw) - cal_LineVel_.vel_y * sinf(yaw), + cal_LineVel_.vel_y * cosf(yaw) + cal_LineVel_.vel_x * sinf(yaw), 0)); + model_->SetAngularVel(ignition::math::Vector3d(0, 0, cal_LineVel_.vel_th)); } - void GazeboOmniRosDrivePrivate::PublishOdometryTf(const gazebo::common::Time & _current_time) - { - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = gazebo_ros::Convert(_current_time); - msg.header.frame_id = odometry_frame_; - msg.child_frame_id = robot_base_frame_; - msg.transform.translation = - gazebo_ros::Convert(odom_.pose.pose.position); - msg.transform.rotation = odom_.pose.pose.orientation; + last_update_time_ = _info.simTime; +} - transform_broadcaster_->sendTransform(msg); - } - - void GazeboOmniRosDrivePrivate::PublishWheelsTf(const gazebo::common::Time & _current_time) - { - for (unsigned int i = 0; i < 4 * num_wheel_pairs_; ++i) - { - auto pose_wheel = joints_[i]->GetChild()->RelativePose(); +void GazeboOmniRosDrivePrivate::UpdateWheelVelocities() +{ + std::lock_guard scoped_lock(lock_); - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = gazebo_ros::Convert(_current_time); - msg.header.frame_id = joints_[i]->GetParent()->GetName(); - msg.child_frame_id = joints_[i]->GetChild()->GetName(); - msg.transform.translation = gazebo_ros::Convert(pose_wheel.Pos()); - msg.transform.rotation = gazebo_ros::Convert(pose_wheel.Rot()); + double vx = target_x_; + double vy = target_y_; + double va = target_rot_; - transform_broadcaster_->sendTransform(msg); - } + for (unsigned int i = 0; i < num_wheel_pairs_; ++i) + { + desired_wheel_speed_[2 * i + NORTH] = (vx + vy - va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; + desired_wheel_speed_[2 * i + WEST] = (vx - vy - va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; + desired_wheel_speed_[2 * i + SOUTH] = (vx + vy + va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; + desired_wheel_speed_[2 * i + EAST] = (vx - vy + va * (wheel_w_separation_[i] + wheel_l_separation_[i]) / 2.0) / wheel_diameter_[i] * 2; } +} + +void GazeboOmniRosDrivePrivate::OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg) +{ + std::lock_guard scoped_lock(lock_); + target_x_ = _msg->linear.x; + target_y_ = _msg->linear.y; + target_rot_ = _msg->angular.z; +} - void GazeboOmniRosDrivePrivate::PublishOdometryMsg(const gazebo::common::Time & _current_time) +void GazeboOmniRosDrivePrivate::PublishOdometryTf(const gazebo::common::Time & _current_time) +{ + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = gazebo_ros::Convert(_current_time); + msg.header.frame_id = odometry_frame_; + msg.child_frame_id = robot_base_frame_; + msg.transform.translation = + gazebo_ros::Convert(odom_.pose.pose.position); + msg.transform.rotation = odom_.pose.pose.orientation; + + transform_broadcaster_->sendTransform(msg); +} + +void GazeboOmniRosDrivePrivate::PublishWheelsTf(const gazebo::common::Time & _current_time) +{ + for (unsigned int i = 0; i < 4 * num_wheel_pairs_; ++i) { - auto pose = model_->WorldPose(); - odom_.pose.pose.position = gazebo_ros::Convert(pose.Pos()); - odom_.pose.pose.orientation = gazebo_ros::Convert(pose.Rot()); + auto pose_wheel = joints_[i]->GetChild()->RelativePose(); - // Get velocity in odom frame - auto linear = model_->WorldLinearVel(); - odom_.twist.twist.angular.z = model_->WorldAngularVel().Z(); + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = gazebo_ros::Convert(_current_time); + msg.header.frame_id = joints_[i]->GetParent()->GetName(); + msg.child_frame_id = joints_[i]->GetChild()->GetName(); + msg.transform.translation = gazebo_ros::Convert(pose_wheel.Pos()); + msg.transform.rotation = gazebo_ros::Convert(pose_wheel.Rot()); - // Convert velocity to child_frame_id(aka base_footprint) - float yaw = pose.Rot().Yaw(); - odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); - odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); - // Set covariance - odom_.pose.covariance[0] = covariance_[0]; - odom_.pose.covariance[7] = covariance_[1]; - odom_.pose.covariance[14] = 1000000000000.0; - odom_.pose.covariance[21] = 1000000000000.0; - odom_.pose.covariance[28] = 1000000000000.0; - odom_.pose.covariance[35] = covariance_[2]; - - odom_.twist.covariance[0] = covariance_[0]; - odom_.twist.covariance[7] = covariance_[1]; - odom_.twist.covariance[14] = 1000000000000.0; - odom_.twist.covariance[21] = 1000000000000.0; - odom_.twist.covariance[28] = 1000000000000.0; - odom_.twist.covariance[35] = covariance_[2]; - - // Set header - odom_.header.frame_id = odometry_frame_; - odom_.child_frame_id = robot_base_frame_; - odom_.header.stamp = gazebo_ros::Convert(_current_time); - - // Publish - odometry_pub_->publish(odom_); + transform_broadcaster_->sendTransform(msg); } +} - void GazeboOmniRosDrivePrivate::calkinematics(linear_vel &line_vel) - { - double wheel_encoder[4]; - wheel_encoder[NORTH] = joints_[NORTH]->GetVelocity(0); - wheel_encoder[WEST] = joints_[WEST]->GetVelocity(0); - wheel_encoder[SOUTH] = joints_[SOUTH]->GetVelocity(0); - wheel_encoder[EAST] = joints_[EAST]->GetVelocity(0); +void GazeboOmniRosDrivePrivate::PublishOdometryMsg(const gazebo::common::Time & _current_time) +{ + auto pose = model_->WorldPose(); + odom_.pose.pose.position = gazebo_ros::Convert(pose.Pos()); + odom_.pose.pose.orientation = gazebo_ros::Convert(pose.Rot()); + + // Get velocity in odom frame + auto linear = model_->WorldLinearVel(); + odom_.twist.twist.angular.z = model_->WorldAngularVel().Z(); + + // Convert velocity to child_frame_id(aka base_footprint) + float yaw = pose.Rot().Yaw(); + odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); + odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); + // Set covariance + odom_.pose.covariance[0] = covariance_[0]; + odom_.pose.covariance[7] = covariance_[1]; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = covariance_[2]; + + odom_.twist.covariance[0] = covariance_[0]; + odom_.twist.covariance[7] = covariance_[1]; + odom_.twist.covariance[14] = 1000000000000.0; + odom_.twist.covariance[21] = 1000000000000.0; + odom_.twist.covariance[28] = 1000000000000.0; + odom_.twist.covariance[35] = covariance_[2]; + + // Set header + odom_.header.frame_id = odometry_frame_; + odom_.child_frame_id = robot_base_frame_; + odom_.header.stamp = gazebo_ros::Convert(_current_time); + + // Publish + odometry_pub_->publish(odom_); +} + +void GazeboOmniRosDrivePrivate::calkinematics(linear_vel &line_vel) +{ + double wheel_encoder[4]; + wheel_encoder[NORTH] = joints_[NORTH]->GetVelocity(0); + wheel_encoder[WEST] = joints_[WEST]->GetVelocity(0); + wheel_encoder[SOUTH] = joints_[SOUTH]->GetVelocity(0); + wheel_encoder[EAST] = joints_[EAST]->GetVelocity(0); - double l = 1.0 / (2 * (wheel_w_separation_[0] + wheel_l_separation_[0])); + double l = 1.0 / (2 * (wheel_w_separation_[0] + wheel_l_separation_[0])); - line_vel.vel_x = (wheel_encoder[NORTH] + wheel_encoder[WEST] + wheel_encoder[SOUTH] + wheel_encoder[EAST]) * wheel_diameter_[0] / 2; - line_vel.vel_y = (wheel_encoder[NORTH] - wheel_encoder[WEST] + wheel_encoder[SOUTH] - wheel_encoder[EAST]) * wheel_diameter_[0] / 2; - line_vel.vel_th = (-wheel_encoder[NORTH] - wheel_encoder[WEST] + wheel_encoder[SOUTH] + wheel_encoder[EAST]) * l * wheel_diameter_[0] / 2; - } + line_vel.vel_x = (wheel_encoder[NORTH] + wheel_encoder[WEST] + wheel_encoder[SOUTH] + wheel_encoder[EAST]) * wheel_diameter_[0] / 2; + line_vel.vel_y = (wheel_encoder[NORTH] - wheel_encoder[WEST] + wheel_encoder[SOUTH] - wheel_encoder[EAST]) * wheel_diameter_[0] / 2; + line_vel.vel_th = (-wheel_encoder[NORTH] - wheel_encoder[WEST] + wheel_encoder[SOUTH] + wheel_encoder[EAST]) * l * wheel_diameter_[0] / 2; +} - GZ_REGISTER_MODEL_PLUGIN(GazeboOmniRosDrive) +GZ_REGISTER_MODEL_PLUGIN(GazeboOmniRosDrive) } // namespace gazebo_plugins \ No newline at end of file