git commit -m "first commit for v2"

Devices/Packages/diff_wheel_plugin/src/diff_wheel_controller.cpp

@@ -0,0 +1,201 @@
+#include <ros/ros.h>
+#include <XmlRpcValue.h>
+#include <boost/thread/thread.hpp>
+#include "md_controller/md_controller.h"
+#include "md_controller/diagnostic.h"
+#include "md_controller/md_controller_subscriber.h"
+#include "libserial/udevadm_info.h"
+#include <cstdlib>
+#include <XmlRpcException.h>
+
+/* define struct */
+typedef struct device{
+  std::string _port;
+  int _baud;
+}device_t;
+
+
+bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) {  
+	ROS_INFO("loadParam() - node_name: %s",  node_name.c_str());
+	if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud)) 
+	{
+    if(!nh->getParam("baudrate", dv->_baud)) 
+      return false;
+  }
+
+  ROS_INFO("product");
+	std::string product;    //port name
+  if(!nh->param(node_name + "/product", product, product)) {}
+  else if(!nh->getParam("product", product)) {}
+  else
+  {
+    udevadmInfo *ludev = new udevadmInfo(product.c_str());
+    if(ludev->init() == EXIT_FAILURE) {}
+    if(ludev->scanDevices())
+    {
+      strcpy(dv->_port.data(), ludev->port);
+      return true;
+    }
+  }
+  
+  ROS_INFO("port_name");
+  if(!nh->param(node_name + "/port_name", dv->_port, dv->_port))
+  {
+    if(!nh->getParam("port_name", dv->_port))
+      return false;
+  }
+
+  ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str());
+  return true;
+
+}
+
+/**
+ * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
+ */
+template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
+{
+  try
+  {
+    if(value.hasMember(member))
+      return value[member];
+    return defaultvalue;
+  }
+  catch(const XmlRpc::XmlRpcException& e)
+  {
+    ROS_WARN_STREAM( "'"<<member << "' "<< e.getMessage() << '\n');
+    return defaultvalue;
+  }
+}
+/**********************************************************************
+* MAIN 
+***********************************************************************/
+int main(int argc,char **argv)
+{
+  /* create ros ndoe */
+  ros::init(argc, argv, "MD");
+  ros::NodeHandlePtr nh = boost::make_shared<ros::NodeHandle>();
+  std::string node_name = ros::this_node::getName();
+  ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str());
+
+  /* connect with the server */
+  std::string diagnostic_topic = "MD_diagnostic";
+  MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller");
+
+
+  XmlRpc::XmlRpcValue drivers;
+  if(!nh->getParam(node_name + "/drivers", drivers) || drivers.size() < 1)
+  {
+    ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str());
+    MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file");
+    return 2;
+  }
+
+  std::vector<boost::shared_ptr<MD::Subscriber>> p_md_controller_subscriber;
+  for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter)
+  {
+    std::string topic_name    = readMember<std::string>(object_iter->second, "toppic", "");
+    int subscribe_queue_size  = readMember<int>(object_iter->second, "subscribe_queue_size", 1);
+    ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size);
+    p_md_controller_subscriber.push_back(boost::make_shared<MD::Subscriber>(*nh, object_iter));
+  }
+
+  device_t dv;
+	if(findDevice(nh, node_name, &dv))
+  {
+		ROS_INFO("%s: found device successfull", node_name.c_str());
+	}
+  else 
+  {
+    ROS_ERROR("%s: Error when find device", node_name.c_str());
+    return 2;
+  }
+  
+  uint8_t rate = 50; // Hz
+  ros::Rate loop_rate(rate); 
+
+  if (p_md_controller_subscriber.empty()) {
+      ROS_WARN("No subscribers available!");
+      return 1;
+  }
+  
+  while (ros::ok())
+  {
+    ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud);
+    boost::shared_ptr<MD::MdController> md_controller = boost::make_shared<MD::MdController>(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1);
+    ros::Duration(1.0).sleep();
+    const std::string password = "robotics";  // Replace with the correct password
+    // First, set to RS232
+    std::string command = "sudo -S -k echo rs232 > /sys/class/sp339_mode_ctl/uartMode";
+    std::string full_command = "echo '" +password + "' | " + command;
+    system(full_command.c_str());
+
+    // Sleep for 0.5 seconds
+    ros::Duration(1.0).sleep();
+
+    // Now, set to RS485
+    command = "sudo -S -k echo rs485 > /sys/class/sp339_mode_ctl/uartMode";
+    full_command = "echo '" + password + "' | " + command;
+    system(full_command.c_str());
+
+    for(auto &p_sub : p_md_controller_subscriber)
+    {
+      md_controller->pid_break(p_sub->slave_id);
+      md_controller->pid_break(p_sub->slave_id);
+    }
+    int v_min = 60;
+    while(ros::ok() && md_controller->_connected)
+    {
+      for(auto it = p_md_controller_subscriber.begin(); it != p_md_controller_subscriber.end(); ++it)
+      {
+        auto &p_sub = *it;
+        if(p_sub->isWheelTriggered()) 
+        {
+          int16_t rotate_speed_wheel;
+          {
+            boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
+            rotate_speed_wheel = p_sub->speed;
+          }
+
+          if(std::abs(rotate_speed_wheel) > 0)  
+          {
+            int sign = (rotate_speed_wheel / abs(rotate_speed_wheel)) > 0 ? 1 : -1;
+            int16_t speed = abs(rotate_speed_wheel) < v_min? (int)(sign * v_min) : rotate_speed_wheel;
+            md_controller->pid_vel_cmd(p_sub->slave_id, speed);
+            // ROS_INFO("%x %d", p_sub->slave_id, speed);
+          }
+          else
+          { 
+            md_controller->pid_break(p_sub->slave_id);
+            if(abs(rotate_speed_wheel) > 0) ROS_INFO("%x Brake %d", p_sub->slave_id, rotate_speed_wheel);
+          }
+          p_sub->scheduleWheelController(false);
+        }
+
+        if(p_sub->isWheelLastestTriggered())
+        { 
+          int16_t rotate_speed_wheel;
+          {
+            boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
+            rotate_speed_wheel = p_sub->speed;
+          }
+          md_controller->pid_stop(p_sub->slave_id);
+          if(std::abs(rotate_speed_wheel) > 0) ROS_WARN("%x stop %d", p_sub->slave_id, rotate_speed_wheel);
+          p_sub->scheduleWheelController(false);
+        }
+      }
+      loop_rate.sleep();
+      ros::spinOnce();
+    }
+    ros::spinOnce();
+    for(auto &p_sub : p_md_controller_subscriber)
+    {
+      md_controller->pid_break(p_sub->slave_id);
+      md_controller->pid_break(p_sub->slave_id);
+    }
+    md_controller->close_port();
+  }
+  ros::spin();
+  return 0;
+}
+

Devices/Packages/diff_wheel_plugin/src/diff_wheel_feedback.cpp

@@ -0,0 +1,81 @@
+#include <ros/ros.h>
+#include <boost/thread.hpp>
+#include "delta_modbus/delta_modbus_tcp.h"
+#include <std_msgs/Float32.h>
+
+double combine_uint16_to_double(uint16_t high, uint16_t low) {
+  // Chuyển đổi high thành double và dịch nó sang trái 16 bit
+  double result = (double)high * 65536.0;  // 65536 = 2^16
+  
+  // Thêm low vào kết quả
+  result += (double)low;
+  
+  return result;
+}
+
+/**********************************************************************
+ *                   		MAIN 
+***********************************************************************/
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "diff_encoder_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_node("~");
+  std::string node_name = ros::this_node::getName();
+  ROS_INFO("node_name: %s", node_name.c_str());
+  
+  std::string ip_address; 
+  int port;
+
+  if(!nh_node.getParam("ip_address", ip_address)) 
+  {
+    ROS_ERROR("Could not find 'ip_address' in node handle");
+    return 1;
+  }
+  if(!nh_node.getParam("port", port)) 
+  {
+    ROS_ERROR("Could not find 'port' in node handle");
+    return 1;
+  }
+
+  int id;
+  nh_node.param("id", id, 1);
+  double rate;
+  nh_node.param("publish_rate", rate, 20.0);
+  
+  int start, end;
+  nh_node.param("start_bit", start, 101);
+  nh_node.param("end_bit", end, 103);
+
+  ROS_INFO("%s: start_bit: %d end_bit: %d ", node_name.c_str(), start, end);
+
+  ros::Publisher left_encoder_pub   = nh.advertise<std_msgs::Float32>("left_encoder", 1);
+  ros::Publisher right_encoder_pub  = nh.advertise<std_msgs::Float32>("right_encoder", 1);
+  DELTA_NAMESPACE::PLC *plc = NULL;
+  plc = new DELTA_NAMESPACE::PLC(ip_address, port, id);
+  ros::Rate r(rate);
+  uint16_t dataD[4];
+  float left_encoder, righ_encoder;
+  while (ros::ok())
+  {   
+    plc->connect();
+    while (ros::ok() && plc->checkConnected())
+    {
+      plc->mulGetD(start, end, dataD);
+      std::memcpy(reinterpret_cast<void*>(&righ_encoder), reinterpret_cast<void const*>(&dataD[0]), sizeof left_encoder);
+      std::memcpy(reinterpret_cast<void*>(&left_encoder), reinterpret_cast<void const*>(&dataD[2]), sizeof righ_encoder);
+      std_msgs::Float32 left_encoder_msg, righ_encoder_msg;
+      left_encoder_msg.data = left_encoder * 2 * M_PI;
+      righ_encoder_msg.data = righ_encoder * 2 * M_PI;
+      left_encoder_pub.publish(left_encoder_msg);
+      right_encoder_pub.publish(righ_encoder_msg);
+      r.sleep();
+      ros::spinOnce();
+    }
+    ros::spinOnce();
+  }
+  
+  ros::spin();  
+  plc = NULL;  
+  delete(plc);
+  return 0;
+}

Devices/Packages/diff_wheel_plugin/src/wheel_plugin.cpp

@@ -0,0 +1,192 @@
+#include "diff_wheel_plugin/wheel_plugin.h"
+#include <pluginlib/class_list_macros.hpp>
+#include <nav_2d_utils/parameters.h>
+#include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/tf_help.h>
+#include <nav_2d_utils/path_ops.h>
+
+
+namespace diff_wheel_plugin
+{
+
+WheelPlugin::WheelPlugin() 
+ : command_timeout_(0.5)
+{
+
+}
+
+WheelPlugin::WheelPlugin(ros::NodeHandle & nh, const std::string &name)
+ : command_timeout_(0.5) 
+{
+  initialize(nh, name);
+}
+
+
+WheelPlugin::~WheelPlugin() 
+{
+  if (m_publish_thread_)
+  {
+    m_publish_thread_->join();
+    delete (m_publish_thread_);
+    m_publish_thread_ = 0;
+  }
+}
+
+void WheelPlugin::initialize(ros::NodeHandle & nh, const std::string &name) 
+{
+  if(!initialized_)
+  {
+    nh_ = nh;
+    name_ = name;
+    nh_priv_ = ros::NodeHandle(nh, name); 
+    nh_priv_.param("command_timeout", command_timeout_, 0.5);
+    nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1); 
+    nh_priv_.param("max_publish_rate", update_rate_, 100.0);
+
+    ROS_INFO_NAMED("WheelPlugin","Initializing on %s with command_timeout %f subscribe_queue_size %d max_publish_rate %f", 
+                    name_.c_str(),
+                    command_timeout_,
+                    m_subscribe_queue_size_,
+                    update_rate_
+                  );
+    std::string mesurement_topic;
+    nh_priv_.param("mesurement_topic", mesurement_topic, std::string(""));
+    std::string wheel_topic;
+    nh_priv_.param("wheel_topic", wheel_topic, name_);
+
+    mesurement_sub_ = nh_.subscribe(mesurement_topic, m_subscribe_queue_size_, &diff_wheel_plugin::WheelPlugin::DLS_Callback, this);
+    
+    wheel_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float32>(nh_, wheel_topic , 100));
+    
+    m_publish_thread_ = new boost::thread(&diff_wheel_plugin::WheelPlugin::DoStuff,this);
+    command_struct_.stamp = ros::Time::now();
+    ROS_INFO_NAMED("WheelPlugin","Initializing on %s is successed", name_.c_str());
+    
+    initialized_ = true;
+  }
+}
+
+bool WheelPlugin::Ready()
+{
+  return is_ready_;
+}
+ 
+double WheelPlugin::GetVelocityLimit(unsigned int _index) 
+{
+  throw std::bad_function_call();
+}
+
+void WheelPlugin::SetVelocityLimit(double _velocity) 
+{
+  throw std::bad_function_call();
+}
+
+double WheelPlugin::GetVelocity() 
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(callback_mutex_);
+  double result = 0.0;
+  if(m_measurement_)
+  {
+    result = m_measurement_->data;
+  }
+  return result;
+}
+
+void WheelPlugin::SetVelocity(double _vel) 
+{
+  if (!std::isfinite(_vel))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s command. Ignoring.", name_.c_str());
+    return;
+  }
+  command_struct_.vel_wheel = _vel;
+  command_struct_.stamp = ros::Time::now();
+  command_.writeFromNonRT(command_struct_);
+  ROS_DEBUG_STREAM_NAMED(name_,
+    "Added values to command."
+    << "vel_wheel: " << command_struct_.vel_wheel << ", "
+    << "Stamp: " << command_struct_.stamp);
+
+  // ROS_INFO_STREAM_NAMED(name_,
+  //   "Added values to command."
+  //   << "vel_wheel: " << command_struct_.vel_wheel << ", "
+  //   << "Stamp: " << command_struct_.stamp);
+}
+
+double WheelPlugin::Position()
+{
+  throw std::bad_function_call();
+}
+
+void WheelPlugin::SetPosition(const double _position)
+{
+  throw std::bad_function_call();
+}
+
+void WheelPlugin::DLS_Callback(const std_msgs::Float32::ConstPtr & msg)
+{
+  boost::lock_guard<boost::mutex> publish_lockguard(callback_mutex_);
+  // check that we don't have multiple publishers on the command topic
+  if (!allow_multiple_command_publishers_ && mesurement_sub_.getNumPublishers() > 1)
+  {
+    ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << mesurement_sub_.getNumPublishers()
+      << " publishers. Only 1 publisher is allowed. Going to brake.");
+    this->brake();
+    return;
+  }
+
+  if (!std::isfinite(msg->data))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s encoder. Ignoring.", name_.c_str());
+    return;
+  }
+
+  m_measurement_ = msg;
+}
+
+void WheelPlugin::DoStuff()
+{
+  ros::Rate rate(update_rate_);
+  is_ready_ = true;
+  ROS_INFO_NAMED("WheelPlugin","...Threading on %s is ready", name_.c_str());
+  bool stoped;
+  while (ros::ok())
+  {
+    WheelCommand curr_cmd =  *(command_.readFromRT());
+    ros::Time current_time = ros::Time::now();
+    double dt = ros::Duration(current_time - command_struct_.stamp).toSec();
+    
+    if(dt <= command_timeout_ && fabs(curr_cmd.vel_wheel) > 0.001)
+    {
+      if (wheel_pub_ && wheel_pub_->trylock())
+      {
+        wheel_pub_->msg_.data = curr_cmd.vel_wheel;
+        wheel_pub_->unlockAndPublish();
+      }
+      stoped = false;
+    }
+    else
+    {
+      if(!stoped)
+      {
+        for(int i = 0; i < 3; i++)
+        {
+          if (wheel_pub_ && wheel_pub_->trylock())
+          {
+            wheel_pub_->msg_.data = 0;
+            wheel_pub_->unlockAndPublish();
+          }
+          ros::Duration(0.1).sleep();
+        }
+        stoped = true;
+      }
+    }
+
+    rate.sleep();
+    ros::spinOnce();
+  }
+}
+
+}; //namespace diff_wheel_plugin
+
+PLUGINLIB_EXPORT_CLASS(diff_wheel_plugin::WheelPlugin, models::BaseSteerDrive)