git commit -m "first commit for v2"

2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
--- a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/CMakeLists.txt
+++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/CMakeLists.txt
@@ -0,0 +1,146 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(ros_lift_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_msgs 
+  roscpp 
+  rospy 
+  gazebo_plugins
+  message_generation
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+find_package(gazebo REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(Eigen3)
+if(NOT EIGEN3_FOUND)
+  # Fallback to cmake_modules
+  find_package(cmake_modules REQUIRED)
+  find_package(Eigen REQUIRED)
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+else()
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif()
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  Lift_Measurement.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs 
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
+  DEPENDS Boost
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/ros_jack_lifter.cpp
+)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${GAZEBO_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  ros_lift_controller_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_lift_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/include/ros_lift_controller/ros_jack_lifter.h
+++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/include/ros_lift_controller/ros_jack_lifter.h
@@ -0,0 +1,104 @@
+#ifndef GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_
+#define GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_
+
+#include <map>
+// Gazebo
+#include <gazebo_plugins/gazebo_ros_utils.h>
+
+// ROS
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Pose.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/JointState.h>
+
+// Custom Callback Queue
+#include <ros/callback_queue.h>
+#include <ros/advertise_options.h>
+
+// Boost
+#include <boost/thread.hpp>
+#include <boost/bind.hpp>
+
+namespace gazebo 
+{
+  // class Joint;
+  // class Entity;
+
+
+  class RosLiftLifter : public ModelPlugin
+  {
+  public:
+    RosLiftLifter();
+    ~RosLiftLifter();
+    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+  protected:
+    virtual void UpdateChild();
+    virtual void FiniChild();
+
+  private:
+    GazeboRosPtr gazebo_ros_;
+    physics::ModelPtr parent;
+    //void publishOdometry(double step_time);
+    void publishLiftSensorStates();
+    void publishTF(physics::JointPtr joint); /// publishes the wheel tf's
+    void publishJointState(physics::JointPtr joint);
+    void effortController(physics::JointPtr joint, const double &error, const double &dt);
+    void positionController(physics::JointPtr joint, const double &target, double &current , const double &dt);
+
+    event::ConnectionPtr update_connection_;
+    physics::JointPtr base_lifting_joint_, base_rotating_joint_;
+    double lift_torque_, rotate_torque_;
+
+    std::string robot_namespace_;
+    std::string command_topic_;
+    std::string robot_base_frame_;
+
+    // ROS STUFF
+    ros::Subscriber cmd_lift_subscriber_;
+    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
+    ros::Publisher joint_state_publisher_;
+    ros::Publisher state_lift_publisher_;
+
+    boost::mutex lock;
+
+    // Custom Callback Queue
+    ros::CallbackQueue queue_;
+    boost::thread callback_queue_thread_;
+    void QueueThread();
+
+    // DiffDrive stuff
+    void cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg);
+    /// updates the relative robot pose based on the wheel encoders
+    void UpdateForkEncoder();
+
+    double lift_height_cmd_, current_lift_position_, lift_step_;
+    double lift_rotate_cmd_, current_lift_angle_, rotate_step_;
+    double lift_height_maximum_;
+
+    bool alive_;
+    double lift_height_encoder_;
+    common::Time last_encoder_update_;
+    double lift_height_origin_;
+    common::PID  joint_lift_pid_, joint_rotate_pid_;
+
+    // Update Rate
+    double update_rate_;
+    double update_period_;
+    common::Time last_actuator_update_;
+
+    // Flags
+    bool publishTF_;
+    bool publishJointState_;
+
+    // bool use_velocity_control_;
+    // double max_velocity_;
+  };
+}
+
+#endif
+
--- a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/msg/Lift_Measurement.msg
+++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/msg/Lift_Measurement.msg
@@ -0,0 +1,5 @@
+Header header
+
+float32 angle_position      # Angle to the X axis's of base_link and X axis's of surface [rad]
+bool top_sensor
+bool bottom_sensor
--- a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/package.xml
+++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/package.xml
@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_lift_controller</name>
+  <version>0.0.0</version>
+  <description>The ros_lift_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ros_lift_controller</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+    <build_depend>message_generation</build_depend>
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+    <exec_depend>message_runtime</exec_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend> 
+  <build_depend>gazebo8</build_depend>
+  <build_depend>gazebo8_msgs</build_depend>
+  <build_depend>gazebo8_plugins</build_depend>
+  <build_depend>gazebo8_ros</build_depend>
+  <build_depend>orunav_generic</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>boost</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend> 
+  <build_export_depend>gazebo8</build_export_depend>
+  <build_export_depend>gazebo8_msgs</build_export_depend>
+  <build_export_depend>gazebo8_plugins</build_export_depend>
+  <build_export_depend>gazebo8_ros</build_export_depend>
+  <build_export_depend>orunav_generic</build_export_depend>
+  <build_export_depend>eigen</build_export_depend>
+  <build_export_depend>boost</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend> 
+  <exec_depend>gazebo8</exec_depend>
+  <exec_depend>gazebo8_msgs</exec_depend>
+  <exec_depend>gazebo8_plugins</exec_depend>
+  <exec_depend>gazebo8_ros</exec_depend>
+  <exec_depend>orunav_generic</exec_depend>
+  <exec_depend>eigen</exec_depend>
+  <exec_depend>boost</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <gazebo plugin_path="${prefix}/lib" />
+  </export>
+</package>
--- a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/src/ros_jack_lifter.cpp
+++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/src/ros_jack_lifter.cpp
@@ -0,0 +1,335 @@
+/**
+ * \file  gazebo_ros_steer_drive.cpp
+ * \brief A fork lifter plugin for gazebo - taken from the old SAUNA repo.
+ * \author  Henrik Andreasson <henrik.andreasson@oru.se>
+ */
+
+
+#include <algorithm>
+#include <assert.h>
+
+#include <ros_lift_controller/ros_jack_lifter.h>
+
+ //#include <gazebo/math/gzmath.hh>
+#include <sdf/sdf.hh>
+
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/GetMap.h>
+#include <nav_msgs/Odometry.h>
+#include <boost/bind.hpp>
+#include <boost/thread/mutex.hpp>
+#include <ros_lift_controller/Lift_Measurement.h>
+
+namespace gazebo
+{
+  RosLiftLifter::RosLiftLifter() {}
+
+  // Destructor
+  RosLiftLifter::~RosLiftLifter() {}
+
+  // Load the controller
+  void RosLiftLifter::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+  {
+    parent = _parent;
+    gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "ForkLifter"));
+    // Make sure the ROS node for Gazebo has already been initialized
+    gazebo_ros_->isInitialized();
+
+    gazebo_ros_->getParameter<double>(update_rate_, "updateRate", 100.0);
+
+    gazebo_ros_->getParameter<double>(lift_torque_, "liftTorque", 1000.0);
+    gazebo_ros_->getParameter<double>(rotate_torque_, "rotateTorque", 1000.0);
+    gazebo_ros_->getParameter<double>(lift_height_maximum_, "liftHeightMaximum", 0.1);
+
+    gazebo_ros_->getParameter<std::string>(robot_base_frame_, "robotBaseFrame", "base_link");
+
+    // Gazebo specific height to align the forks to the origin.
+    gazebo_ros_->getParameter<double>(lift_height_origin_, "liftHeightOrigin", 0.08);
+
+    gazebo_ros_->getParameterBoolean(publishTF_, "publishTF", false);
+    gazebo_ros_->getParameterBoolean(publishJointState_, "publishJointState", false);
+
+    gazebo_ros_->getParameter<std::string>(command_topic_, "commandTopic", "cmd_jacking");
+    // gazebo_ros_->getParameterBoolean(use_velocity_control_, "useVelocityControl", true);
+    // gazebo_ros_->getParameter<double>(max_velocity_, "maxVelocity", 0.08);
+
+    gazebo_ros_->getParameter<double>(lift_step_, "liftStep", 0.002);
+    gazebo_ros_->getParameter<double>(rotate_step_, "rotateStep", 0.05);
+
+    double pid_p, pid_i, pid_d;
+    gazebo_ros_->getParameter<double>(pid_p, "pidP", 2000);
+    gazebo_ros_->getParameter<double>(pid_i, "pidI", 0);
+    gazebo_ros_->getParameter<double>(pid_d, "pidD", 10);
+
+    base_lifting_joint_ = gazebo_ros_->getJoint(parent, "liftJoint", "base_lifting_joint");
+    base_rotating_joint_ = gazebo_ros_->getJoint(parent, "rotateJoint", "base_rotating_joint");
+#if GAZEBO_MAJOR_VERSION > 2
+    base_lifting_joint_->SetParam("fmax", 0, lift_torque_);
+    base_rotating_joint_->SetParam("fmax", 0, rotate_torque_);
+#else
+    base_lifting_joint_->SetMaxForce(0, lift_torque_);
+    base_rotating_joint_->SetMaxForce(0, rotate_torque_);
+#endif
+
+    // Initialize update rate stuff
+    if (this->update_rate_ > 0.0) this->update_period_ = 1.0 / this->update_rate_;
+    else this->update_period_ = 0.0;
+
+#if GAZEBO_MAJOR_VERSION >= 8
+    last_actuator_update_ = parent->GetWorld()->SimTime();
+#else
+    last_actuator_update_ = parent->GetWorld()->GetSimTime();
+#endif
+
+    // Initialize velocity stuff
+    alive_ = true;
+    this->joint_lift_pid_.Init(pid_p, pid_i, pid_d, 100, -100, lift_torque_, -lift_torque_);
+    this->joint_rotate_pid_.Init(pid_p, pid_i, pid_d, 100, -100, rotate_torque_, -rotate_torque_);
+
+    if (this->publishJointState_) {
+      joint_state_publisher_ = gazebo_ros_->node()->advertise<sensor_msgs::JointState>("joint_states", 1000);
+      ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info());
+    }
+
+    transform_broadcaster_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
+
+    // ROS: Subscribe to the velocity command topic (cmd_fork)
+    ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str());
+
+    ros::SubscribeOptions so =
+      ros::SubscribeOptions::create<geometry_msgs::Pose>(command_topic_, 1,
+        boost::bind(&RosLiftLifter::cmdCallback, this, _1),
+        ros::VoidPtr(), &queue_);
+
+    cmd_lift_subscriber_ = gazebo_ros_->node()->subscribe(so);
+    ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str());
+    state_lift_publisher_ = gazebo_ros_->node()->advertise<ros_lift_controller::Lift_Measurement>("lift_sensor_states", 100);
+
+    // start custom queue for diff drive
+    this->callback_queue_thread_ = boost::thread(boost::bind(&RosLiftLifter::QueueThread, this));
+
+    // listen to the update event (broadcast every simulation iteration)
+    this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&RosLiftLifter::UpdateChild, this));
+
+    lift_height_cmd_ = 0.0;
+    current_lift_position_ = 0.0;
+    lift_rotate_cmd_= 0.0;
+    current_lift_angle_ = 0.0;
+  }
+
+  void RosLiftLifter::publishLiftSensorStates()
+  {
+    ros_lift_controller::Lift_Measurement lm;
+    lm.header.stamp = ros::Time::now();
+    lm.header.frame_id = robot_base_frame_;
+
+#if GAZEBO_MAJOR_VERSION >= 8
+      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose();
+#else
+      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign();
+#endif
+
+    lm.angle_position = pose_rotate.Rot().Euler().Z();
+
+    lm.top_sensor = fabs(current_lift_position_ - lift_height_maximum_) < 0.01? true : false;
+    lm.bottom_sensor = fabs(current_lift_position_ - 0.0) < 0.01? true : false;
+    state_lift_publisher_.publish(lm);
+  }
+
+  void RosLiftLifter::publishJointState(physics::JointPtr joint)
+  {
+    std::vector<physics::JointPtr> joints;
+    joints.push_back(joint);
+
+    ros::Time current_time = ros::Time::now();
+    sensor_msgs::JointState joint_state;
+    joint_state.header.stamp = current_time;
+    joint_state.name.resize(joints.size());
+    joint_state.position.resize(joints.size());
+    joint_state.velocity.resize(joints.size());
+    joint_state.effort.resize(joints.size());
+    for (std::size_t i = 0; i < joints.size(); i++) {
+      joint_state.name[i] = joints[i]->GetName();
+#if GAZEBO_MAJOR_VERSION >= 8
+      joint_state.position[i] = joints[i]->Position(0);
+#else
+      joint_state.position[i] = joints[i]->GetAngle(0).Radian();
+#endif
+      joint_state.velocity[i] = joints[i]->GetVelocity(0);
+      joint_state.effort[i] = joints[i]->GetForce(0);
+    }
+    joint_state_publisher_.publish(joint_state);
+  }
+
+  void RosLiftLifter::publishTF(physics::JointPtr joint)
+  {
+    ros::Time current_time = ros::Time::now();
+    std::vector<physics::JointPtr> joints;
+    joints.push_back(joint);
+
+    for (std::size_t i = 0; i < joints.size(); i++) {
+      std::string frame = gazebo_ros_->resolveTF(joints[i]->GetName());
+      std::string parent_frame = gazebo_ros_->resolveTF(joints[i]->GetParent()->GetName());
+
+#if GAZEBO_MAJOR_VERSION >= 8
+      ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose();
+#else
+      ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign();
+#endif
+
+      tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W());
+      tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
+
+      tf::Transform transform(qt, vt);
+      transform_broadcaster_->sendTransform(tf::StampedTransform(transform, current_time, parent_frame, frame));
+    }
+
+  }
+  // Update the controller
+  void RosLiftLifter::UpdateChild()
+  {
+    UpdateForkEncoder();
+#if GAZEBO_MAJOR_VERSION >= 8
+    common::Time current_time = parent->GetWorld()->SimTime();
+#else
+    common::Time current_time = parent->GetWorld()->GetSimTime();
+#endif
+
+    double seconds_since_last_update = (current_time - last_actuator_update_).Double();
+    if (seconds_since_last_update > update_period_) {
+
+      if (publishJointState_) 
+      {
+        publishJointState(base_lifting_joint_);
+        publishJointState(base_rotating_joint_);
+      }
+      publishLiftSensorStates();
+
+// Use the PID class...
+#if GAZEBO_MAJOR_VERSION >= 8
+      ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->RelativePose();
+#else
+      ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->GetRelativePose().Ign();
+#endif
+      double target_lift_height = lift_height_cmd_ + this->lift_height_origin_;
+      double current_lift_height = pose_lift.Pos().Z();
+      double error_lift_height = current_lift_height - target_lift_height;
+      this->effortController(base_lifting_joint_, error_lift_height, seconds_since_last_update);
+      this->positionController(base_lifting_joint_, lift_height_cmd_, current_lift_position_, lift_step_);
+
+#if GAZEBO_MAJOR_VERSION >= 8
+      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose();
+#else
+      ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign();
+#endif
+
+      double target_lift_yaw = lift_rotate_cmd_;
+      double current_lift_yaw = pose_rotate.Rot().Euler().Z();
+      double error_lift_yaw = current_lift_yaw - target_lift_yaw;
+      this->effortController(base_rotating_joint_, error_lift_yaw, seconds_since_last_update);
+      this->positionController(base_rotating_joint_, lift_rotate_cmd_, current_lift_angle_, rotate_step_);
+
+      last_actuator_update_ += common::Time(update_period_);
+    }
+
+#if 0
+    if (odom_source_ == ENCODER) UpdateOdometryEncoder();
+    common::Time current_time = parent->GetWorld()->GetSimTime();
+    double seconds_since_last_update = (current_time - last_actuator_update_).Double();
+    if (seconds_since_last_update > update_period_) {
+
+      publishOdometry(seconds_since_last_update);
+      if (publishWheelTF_) publishWheelTF();
+      if (publishWheelJointState_) publishWheelJointState();
+
+      double target_wheel_roation_speed = cmd_.speed / (wheel_diameter_ / 2.0);
+      double target_steering_angle_speed = cmd_.angle;
+
+      motorController(target_wheel_roation_speed, target_steering_angle_speed, seconds_since_last_update);
+
+      //ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle);
+
+      last_actuator_update_ += common::Time(update_period_);
+    }
+#endif
+  }
+
+  void RosLiftLifter::effortController(physics::JointPtr joint, const double &error, const double &dt)
+  {
+    double control_value = this->joint_lift_pid_.Update(error, dt);
+#if GAZEBO_MAJOR_VERSION > 2
+    joint->SetForce(0, control_value);
+#else
+    joint->SetForce(0, control_value);
+#endif
+  }
+
+  void RosLiftLifter::positionController(physics::JointPtr joint, const double &target, double &current , const double &dt)
+  {
+    double error = target - current;
+    double step = dt;
+    // Nếu sai số lớn, tăng/giảm dần
+    if (std::abs(error) > step)
+    {
+      current += step * (error > 0 ? 1 : -1);
+    }
+    else
+    {
+      current = target;  // gần bằng rồi, gán luôn
+    }
+
+#if GAZEBO_MAJOR_VERSION > 2
+    joint->SetPosition(0, current);
+#else
+    joint->SetPosition(0, current);
+#endif
+  }
+
+  // Finalize the controller
+  void RosLiftLifter::FiniChild()
+  {
+    alive_ = false;
+    queue_.clear();
+    queue_.disable();
+    gazebo_ros_->node()->shutdown();
+    callback_queue_thread_.join();
+  }
+
+  void RosLiftLifter::cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg)
+  {
+    boost::mutex::scoped_lock scoped_lock(lock);
+    lift_height_cmd_ = std::min(lift_height_maximum_, cmd_msg->position.z);
+    lift_rotate_cmd_ = tf::getYaw(cmd_msg->orientation);
+  }
+
+  void RosLiftLifter::QueueThread()
+  {
+    static const double timeout = 0.01;
+
+    while (alive_ && gazebo_ros_->node()->ok()) {
+      queue_.callAvailable(ros::WallDuration(timeout));
+    }
+  }
+
+  void RosLiftLifter::UpdateForkEncoder()
+  {
+#if GAZEBO_MAJOR_VERSION >= 8
+    common::Time current_time = parent->GetWorld()->SimTime();
+#else
+    common::Time current_time = parent->GetWorld()->GetSimTime();
+#endif
+    double step_time = (current_time - last_encoder_update_).Double();
+    last_encoder_update_ = current_time;
+#if GAZEBO_MAJOR_VERSION >= 8
+    ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->RelativePose();
+#else
+    ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->GetRelativePose().Ign();
+#endif
+    lift_height_encoder_ = pose.Pos().Z();
+  }
+  GZ_REGISTER_MODEL_PLUGIN(RosLiftLifter)
+}
+