阿克曼转向模型与gazebo实时仿真

原创 于 2025-06-04 17:31:00 发布 · 340 阅读 · 4 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#强化学习 #gazebo #阿克曼转向 #ros

https://github.com/KevinRichard1/scout-sim/tree/68ce509380378deb41247efe57014efae4b1cfab

https://github.com/robustify/audibot

https://github.com/osrf/car_demo

原本的car_demo是不能直接使用的,所以需要:
https://github.com/leggedrobotics/se2_navigation

https://github.com/wwenhongich/Gazebo_Robot_Sim/tree/master

阿克曼小车urdf模型搭建与gazebo仿真

需要vip https://blog.youkuaiyun.com/tianbot/category_11774891.html

https://blog.youkuaiyun.com/weixin_40599145/article/details/126929222

https://github.com/linzs-online/robot_gazebo

https://github.com/czhherry/self-driving-vehicle-101/tree/master

第一种是自定义的插件实现的,首先我们来定义一下这个插件:

#ifndef CARINTERFACEPLUGIN_H
#define CARINTERFACEPLUGIN_H

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt8.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>

#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>

namespace gazebo {

// 运动学参数
#define CAR_STEERING_RATIO      17.3  
#define CAR_LOCK_TO_LOCK_REVS   3.2   
#define CAR_MAX_STEER_ANGLE     (M_PI * CAR_LOCK_TO_LOCK_REVS / CAR_STEERING_RATIO) 
#define CAR_WHEELBASE           2.65  
#define CAR_TRACK_WIDTH         1.638 

// 车辆其他参数
// 滚动阻力系数
#define ROLLING_RESISTANCE_COEFF  0.01
// 空气阻力系数
#define AERO_DRAG_COEFF           0.35
// 重力加速度
#define GRAVITY_ACCEL             9.81
// 车辆质量
#define VEHICLE_MASS              1700.0
// 车轮半径
#define WHEEL_RADIUS              0.36
// 最大制动扭矩
#define MAX_BRAKE_TORQUE          8000.0

// 挡位状态
enum { DRIVE = 0, REVERSE = 1 };


class CarInterfacePlugin : public ModelPlugin {
public:
  CarInterfacePlugin();
  virtual ~CarInterfacePlugin();

protected:
  virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
  virtual void Reset();

private:
  // 反馈定时器回调函数，用于定期发布反馈信息
  void feedbackTimerCallback(const ros::TimerEvent& event);
  void tfTimerCallback(const ros::TimerEvent& event);
  void OnUpdate(const common::UpdateInfo& info);

  void recvSteeringCmd(const std_msgs::Float64ConstPtr& msg);
  void recvThrottleCmd(const std_msgs::Float64ConstPtr& msg);
  void recvBrakeCmd(const std_msgs::Float64ConstPtr& msg);
  void recvGearCmd(const std_msgs::UInt8ConstPtr& msg);

  void twistStateUpdate();
  void driveUpdate();
  void steeringUpdate(const common::UpdateInfo& info);
  void dragUpdate();

  void stopWheels();

  void setAllWheelTorque(double torque);
  void setRearWheelTorque(double torque);

  ros::NodeHandle* n_;
  // 发布速度
  ros::Publisher pub_twist_;
  // 发布里程计
  ros::Publisher pub_odom_;
  // 发布挡位状态
  ros::Publisher pub_gear_state_;
  // 发布转角
  ros::Publisher pub_steering_;

  // 订阅转向
  ros::Subscriber sub_steering_cmd_;
  // 订阅油门
  ros::Subscriber sub_throttle_cmd_;
  // 订阅制动
  ros::Subscriber sub_brake_cmd_;
  // 订阅挡位
  ros::Subscriber sub_gear_cmd_;
  // 订阅模型状态
  ros::Subscriber sub_model_states_;

  ros::Timer feedback_timer_;
  ros::Timer tf_timer_;

  tf2_ros::TransformBroadcaster br_;
  geometry_msgs::Twist twist_;
  bool rollover_;

#if GAZEBO_MAJOR_VERSION >= 9
  // Gazebo 9及以上版本使用的世界位姿
  ignition::math::Pose3d world_pose_;
#else
  // Gazebo 9以下版本使用的世界位姿
  gazebo::math::Pose world_pose_;
#endif
  event::ConnectionPtr update_connection_;
  physics::JointPtr steer_fl_joint_;
  physics::JointPtr steer_fr_joint_;
  physics::JointPtr wheel_rl_joint_;
  physics::JointPtr wheel_rr_joint_;
  physics::JointPtr wheel_fl_joint_;
  physics::JointPtr wheel_fr_joint_;
  physics::LinkPtr footprint_link_;
  // 上次更新时间
  common::Time last_update_time_;
  // 坐标系ID
  std::string frame_id_;

  // SDF参数
  std::string robot_name_;
  bool pub_tf_;
  // TF发布频率
  double tf_freq_;

  // 转向值
  // 右轮转角
  double right_angle_;
  // 左轮转角
  double left_angle_;
  // 目标转角
  double target_angle_;
  // 当前转向角
  double current_steering_angle_;

  // 制动
  double brake_cmd_;
  ros::Time brake_stamp_;

  // 油门
  double throttle_cmd_;
  ros::Time throttle_stamp_;

  // 挡位
  uint8_t gear_cmd_;
};

// 注册插件
GZ_REGISTER_MODEL_PLUGIN(CarInterfacePlugin)

}

#endif // CARINTERFACEPLUGIN_H

CarInterfacePlugin.cpp

#include <audibot_gazebo/CarInterfacePlugin.h>

namespace gazebo {

// 初始化变量
CarInterfacePlugin::CarInterfacePlugin() {
  target_angle_ = 0.0;
  brake_cmd_ = 0.0;
  throttle_cmd_ = 0.0;
  gear_cmd_ = DRIVE;
  current_steering_angle_ = 0.0;
  rollover_ = false;
}

// 加载插件时调用的函数，用于初始化模型和SDF元素
void CarInterfacePlugin::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
  // 获取各个关节和链接
  steer_fl_joint_ = model->GetJoint("steer_fl_joint");
  steer_fr_joint_ = model->GetJoint("steer_fr_joint");
  wheel_rl_joint_ = model->GetJoint("wheel_rl_joint");
  wheel_rr_joint_ = model->GetJoint("wheel_rr_joint");
  wheel_fl_joint_ = model->GetJoint("wheel_fl_joint");
  wheel_fr_joint_ = model->GetJoint("wheel_fr_joint");
  footprint_link_ = model->GetLink("base_footprint");

  // 加载SDF参数
  if (sdf->HasElement("pubTf")) {
    sdf->GetElement("pubTf")->GetValue()->Get(pub_tf_);
  } else {
    pub_tf_ = false;
  }

  if (sdf->HasElement("robotName")) {
    sdf::ParamPtr sdf_robot_name = sdf->GetElement("robotName")->GetValue();
    if (sdf_robot_name) {
      sdf_robot_name->Get(robot_name_);
    } else {
      robot_name_ = std::string("");
    }
  } else {
    robot_name_ = std::string("");
  }

  if (sdf->HasElement("tfFreq")) {
    sdf->GetElement("tfFreq")->GetValue()->Get(tf_freq_);
  } else {
    tf_freq_ = 100.0;
  }

  // 注册更新回调函数
  update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&CarInterfacePlugin::OnUpdate, this, _1));

  // 设置转向关节的最大力
  steer_fl_joint_->SetParam("fmax", 0, 99999.0);
  steer_fr_joint_->SetParam("fmax", 0, 99999.0);

  // ROS初始化
  n_ = new ros::NodeHandle(robot_name_);

  // 订阅各种命令消息
  sub_steering_cmd_ = n_->subscribe("steering_cmd", 1, &CarInterfacePlugin::recvSteeringCmd, this);
  sub_brake_cmd_ = n_->subscribe("brake_cmd", 1, &CarInterfacePlugin::recvBrakeCmd, this);
  sub_throttle_cmd_ = n_->subscribe("throttle_cmd", 1, &CarInterfacePlugin::recvThrottleCmd, this);
  sub_gear_cmd_ = n_->subscribe("gear_cmd", 1, &CarInterfacePlugin::recvGearCmd, this);

  // 发布各种消息
  pub_twist_ = n_->advertise<geometry_msgs::TwistStamped> ("twist", 1);
  pub_gear_state_ = n_->advertise<std_msgs::UInt8> ("gear_state", 1);
  pub_odom_ = n_->advertise<nav_msgs::Odometry>("odom", 1);
  pub_steering_ = n_->advertise<std_msgs::Float64>("steering_state", 1);
  // 创建反馈定时器
  feedback_timer_ = n_->createTimer(ros::Duration(0.02), &CarInterfacePlugin::feedbackTimerCallback, this);

  if (pub_tf_) {
    // 创建TF定时器
    tf_timer_ = n_->createTimer(ros::Duration(1.0 / tf_freq_), &CarInterfacePlugin::tfTimerCallback, this);
  }

  if (robot_name_.empty()) {
    frame_id_ = footprint_link_->GetName();
  } else {
    frame_id_ = robot_name_ + "/" + footprint_link_->GetName();
  }
}

// 每次仿真更新时调用的函数
void CarInterfacePlugin::OnUpdate(const common::UpdateInfo& info) {
  if (last_update_time_ == common::Time(0)) {
    last_update_time_ = info.simTime;
    return;
  }

  // 更新速度
  twistStateUpdate();
  // 更新驱动
  driveUpdate();
  // 更新转向
  steeringUpdate(info);
  // 更新阻力
  dragUpdate();
}

// 更新速度状态的函数
void CarInterfacePlugin::twistStateUpdate() {
#if GAZEBO_MAJOR_VERSION >= 9
  world_pose_ = footprint_link_->WorldPose();
  twist_.linear.x = footprint_link_->RelativeLinearVel().X();
  twist_.angular.z = footprint_link_->RelativeAngularVel().Z();
  rollover_ = (fabs(world_pose_.Rot().X()) > 0.2 || fabs(world_pose_.Rot().Y()) > 0.2);
#else
  world_pose_ = footprint_link_->GetWorldPose();
  twist_.linear.x = footprint_link_->GetRelativeLinearVel().x;
  twist_.angular.z = footprint_link_->GetRelativeAngularVel().z;
  rollover_ = (fabs(world_pose_.rot.x) > 0.2 || fabs(world_pose_.rot.y) > 0.2);
#endif
}

// 更新驱动状态的函数
void CarInterfacePlugin::driveUpdate() {
  // 如果车辆翻车，停止车轮转动
  if (rollover_) {
    stopWheels();
    return;
  }

  // 制动命令优先于油门命令
  ros::Time current_stamp = ros::Time::now();
  if ((brake_cmd_ > 0) && ((current_stamp - brake_stamp_).toSec() < 0.25)) {
    double brake_torque_factor = 1.0;
    if (twist_.linear.x < -0.1) {
      brake_torque_factor = -1.0;
    } else if (twist_.linear.x < 0.1) {
      brake_torque_factor = 1.0 + (twist_.linear.x - 0.1) / 0.1;
    }

    setAllWheelTorque(-brake_torque_factor * brake_cmd_);
  } else {
    if ((current_stamp - throttle_stamp_).toSec() < 0.25) {
      double throttle_torque;
      if (gear_cmd_ == DRIVE) {
        throttle_torque = throttle_cmd_ * 4000.0 - 40.1 * twist_.linear.x;
        if (throttle_torque < 0.0) {
          throttle_torque = 0.0;
        }
      } else { // REVERSE
        throttle_torque = -throttle_cmd_ * 4000.0 - 250.0 * twist_.linear.x;
        if (throttle_torque > 0.0) {
          throttle_torque = 0.0;
        }
      }
      setRearWheelTorque(throttle_torque);
    }
  }
}

// 更新转向状态的函数
void CarInterfacePlugin::steeringUpdate(const common::UpdateInfo& info) {
  double time_step = (info.simTime - last_update_time_).Double();
  last_update_time_ = info.simTime;

  // 任意设置最大转向速率为800 deg/s
  const double max_rate = 800.0 * M_PI / 180.0 / CAR_STEERING_RATIO;
  double max_inc = time_step * max_rate;

  if ((target_angle_ - current_steering_angle_) > max_inc) {
    current_steering_angle_ += max_inc;
  } else if ((target_angle_ - current_steering_angle_) < -max_inc) {
    current_steering_angle_ -= max_inc;
  }

  // 计算每个车轮的阿克曼转向角
  double t_alph = tan(current_steering_angle_);
  double left_steer = atan(CAR_WHEELBASE * t_alph / (CAR_WHEELBASE - 0.5 * CAR_TRACK_WIDTH * t_alph));
  double right_steer = atan(CAR_WHEELBASE * t_alph / (CAR_WHEELBASE + 0.5 * CAR_TRACK_WIDTH * t_alph));

#if GAZEBO_MAJOR_VERSION >= 9
  steer_fl_joint_->SetParam("vel", 0, 100.0 * (left_steer - steer_fl_joint_->Position(0)));
  steer_fr_joint_->SetParam("vel", 0, 100.0 * (right_steer - steer_fr_joint_->Position(0)));
#else
  steer_fl_joint_->SetParam("vel", 0, 100.0 * (left_steer - steer_fl_joint_->GetAngle(0).Radian()));
  steer_fr_joint_->SetParam("vel", 0, 100.0 * (right_steer - steer_fr_joint_->GetAngle(0).Radian()));
#endif
}

// 更新阻力状态的函数
void CarInterfacePlugin::dragUpdate() {
  // 应用滚动阻力和空气阻力
  double rolling_resistance_torque = ROLLING_RESISTANCE_COEFF * VEHICLE_MASS * GRAVITY_ACCEL;
  double drag_force = AERO_DRAG_COEFF * twist_.linear.x * twist_.linear.x;
  double drag_torque = drag_force * WHEEL_RADIUS; // 将空气阻力实现为扭矩干扰

  if (twist_.linear.x > 0.0) {
    setAllWheelTorque(-rolling_resistance_torque);
    setAllWheelTorque(-drag_torque);
  } else {
    setAllWheelTorque(rolling_resistance_torque);
    setAllWheelTorque(drag_torque);
  }
}

// 设置所有车轮扭矩的函数
void CarInterfacePlugin::setAllWheelTorque(double torque) {
  wheel_rl_joint_->SetForce(0, 0.25 * torque);
  wheel_rr_joint_->SetForce(0, 0.25 * torque);
  wheel_fl_joint_->SetForce(0, 0.25 * torque);
  wheel_fr_joint_->SetForce(0, 0.25 * torque);
}

// 设置后轮扭矩的函数
void CarInterfacePlugin::setRearWheelTorque(double torque) {
  wheel_rl_joint_->SetForce(0, 0.5 * torque);
  wheel_rr_joint_->SetForce(0, 0.5 * torque);
}

// 停止车轮转动的函数
void CarInterfacePlugin::stopWheels() {
  wheel_fl_joint_->SetForce(0, -1000.0 * wheel_fl_joint_->GetVelocity(0));
  wheel_fr_joint_->SetForce(0, -1000.0 * wheel_fr_joint_->GetVelocity(0));
  wheel_rl_joint_->SetForce(0, -1000.0 * wheel_rl_joint_->GetVelocity(0));
  wheel_rr_joint_->SetForce(0, -1000.0 * wheel_rr_joint_->GetVelocity(0));
}

// 接收转向命令的回调函数
void CarInterfacePlugin::recvSteeringCmd(const std_msgs::Float64ConstPtr& msg) {
  if (!std::isfinite(msg->data)) {
    target_angle_ = 0.0;
    return;
  }

  target_angle_ = msg->data / CAR_STEERING_RATIO;
  if (target_angle_ > CAR_MAX_STEER_ANGLE) {
    target_angle_ = CAR_MAX_STEER_ANGLE;
  } else if (target_angle_ < -CAR_MAX_STEER_ANGLE) {
    target_angle_ = -CAR_MAX_STEER_ANGLE;
  }
}

// 接收制动命令的回调函数
void CarInterfacePlugin::recvBrakeCmd(const std_msgs::Float64ConstPtr& msg) {
  brake_cmd_ = msg->data;
  if (brake_cmd_ < 0) {
    brake_cmd_ = 0;
  } else if (brake_cmd_ > MAX_BRAKE_TORQUE) {
    brake_cmd_ = MAX_BRAKE_TORQUE;
  }
  brake_stamp_ = ros::Time::now();
}

// 接收油门命令的回调函数
void CarInterfacePlugin::recvThrottleCmd(const std_msgs::Float64ConstPtr& msg) {
  throttle_cmd_ = msg->data;
  if (throttle_cmd_ < 0.0) {
    throttle_cmd_ = 0.0;
  } else if (throttle_cmd_ > 1.0) {
    throttle_cmd_ = 1.0;
  }
  throttle_stamp_ = ros::Time::now();
}

// 接收挡位命令的回调函数
void CarInterfacePlugin::recvGearCmd(const std_msgs::UInt8ConstPtr& msg) {
  if (msg->data > REVERSE) {
    ROS_WARN("Invalid gear command received [%u]", msg->data);
  } else {
    gear_cmd_ = msg->data;
  }
}

// 反馈定时器回调函数，用于定期发布反馈信息
void CarInterfacePlugin::feedbackTimerCallback(const ros::TimerEvent& event) {
  geometry_msgs::TwistStamped twist_msg;
  twist_msg.header.frame_id = frame_id_;
  twist_msg.header.stamp = event.current_real;
  twist_msg.twist = twist_;
  pub_twist_.publish(twist_msg);

  std_msgs::UInt8 gear_state_msg;
  gear_state_msg.data = gear_cmd_;
  pub_gear_state_.publish(gear_state_msg);

  nav_msgs::Odometry odom_msg;
  odom_msg.header.frame_id = frame_id_;
  odom_msg.header.stamp = event.current_real;
  odom_msg.twist.twist = twist_;
  odom_msg.pose.pose.position.x = world_pose_.Pos().X();
  odom_msg.pose.pose.position.y = world_pose_.Pos().Y();
  odom_msg.pose.pose.position.z = world_pose_.Pos().Z();
  odom_msg.pose.pose.orientation.x = world_pose_.Rot().X();
  odom_msg.pose.pose.orientation.y = world_pose_.Rot().Y();
  odom_msg.pose.pose.orientation.z = world_pose_.Rot().Z();
  odom_msg.pose.pose.orientation.w = world_pose_.Rot().W();
  pub_odom_.publish(odom_msg);

  std_msgs::Float64 steering;
  steering.data = CAR_STEERING_RATIO * current_steering_angle_;
  pub_steering_.publish(steering);
}

// TF定时器回调函数，用于定期发布变换信息
void CarInterfacePlugin::tfTimerCallback(const ros::TimerEvent& event) {
  // 如果时间戳与上次相同，则不发布TF，以防止TF_REPEATED_DATA警告
  if ((event.current_real - event.last_real).toSec() < 1e-6) {
    return;
  }
  
  geometry_msgs::TransformStamped t;
  t.header.frame_id = "world";
  t.child_frame_id = frame_id_;
  t.header.stamp = event.current_real;
#if GAZEBO_MAJOR_VERSION >= 9
  t.transform.translation.x = world_pose_.Pos().X();
  t.transform.translation.y = world_pose_.Pos().Y();
  t.transform.translation.z = world_pose_.Pos().Z();
  t.transform.rotation.w = world_pose_.Rot().W();
  t.transform.rotation.x = world_pose_.Rot().X();
  t.transform.rotation.y = world_pose_.Rot().Y();
  t.transform.rotation.z = world_pose_.Rot().Z();
#else
  t.transform.translation.x = world_pose_.pos.x;
  t.transform.translation.y = world_pose_.pos.y;
  t.transform.translation.z = world_pose_.pos.z;
  t.transform.rotation.w = world_pose_.rot.w;
  t.transform.rotation.x = world_pose_.rot.x;
  t.transform.rotation.y = world_pose_.rot.y;
  t.transform.rotation.z = world_pose_.rot.z;
#endif
  br_.sendTransform(t);
}

// 重置插件状态的函数
void CarInterfacePlugin::Reset() {
}

// 关闭ROS节点并释放内存
CarInterfacePlugin::~CarInterfacePlugin() {
  n_->shutdown();
  delete n_;
}


}

第二种是借助已经定义好的插件类型来编译

<?xml version="1.0" ?>

<robot name="$(arg roboname)" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="PI" value="3.1415926835897931"/>

  <!-- base -->
  <xacro:property name="base_length" value="2.695"/>
  <xacro:property name="base_width" value="0.68"/>
  <xacro:property name="base_height" value="1.370"/>
  <xacro:property name="base_collision_height" value="0.875"/>
  <xacro:property name="base_mass" value="956"/>
  <xacro:property name="base_mass_ixx" value="343"/>
  <xacro:property name="base_mass_iyy" value="728"/>
  <xacro:property name="base_mass_izz" value="772"/>

  <!-- rear tyre -->
  <xacro:property name="rear_tyre_x" value="0.945"/>
  <xacro:property name="rear_tyre_y" value="0.642"/>
  <xacro:property name="rear_tyre_r" value="0.3"/>
  <xacro:property name="rear_tyre_length" value="0.165"/>
  <xacro:property name="rear_tyre_mass" value="20"/>
  <xacro:property name="rear_tyre_mass_ixx" value="0.5"/>
  <xacro:property name="rear_tyre_mass_iyy" value="0.9"/>
  <xacro:property name="rear_tyre_mass_izz" value="0.5"/> 

  <!-- front tyre -->
  <xacro:property name="front_tyre_x" value="0.923"/>
  <xacro:property name="front_tyre_y" value="0.642"/>
  <xacro:property name="front_tyre_r" value="0.3"/>
  <xacro:property name="front_tyre_length" value="0.165"/>
  <xacro:property name="front_tyre_mass" value="20"/>
  <xacro:property name="front_tyre_mass_ixx" value="0.5"/>
  <xacro:property name="front_tyre_mass_iyy" value="0.9"/>
  <xacro:property name="front_tyre_mass_izz" value="0.5"/>  

  <!-- steering -->
  <xacro:property name="str_angle" value="0.6"/>    
  <xacro:property name="str_length" value="0.01"/>
  <xacro:property name="str_radius" value="0.1"/>
  <xacro:property name="str_mass" value="5"/>
  <xacro:property name="str_mass_ixx" value="0.012"/>
  <xacro:property name="str_mass_iyy" value="0.025"/>
  <xacro:property name="str_mass_izz" value="0.012"/> 

  <!--Car Body-->
  <link name="base_link">
    <collision>
      <origin xyz="0 0 ${base_collision_height}" rpy="0 0 0"/>  
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0.05 0 ${base_collision_height}" rpy="0 0 ${PI/2}"/>  
      <geometry>
        <mesh filename="package://car_model/meshes/vehicle_body.dae"/>
        <!-- <box size="${base_length} ${base_width} ${base_height}"/> -->
      </geometry>
    </visual>
  </link>

  <joint name="inertial_joint" type="fixed">
    <parent link="base_link"/>
    <child link="main_mass"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>  

  <link name="main_mass" type="fixed">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${base_mass}"/>
      <inertia
        ixx="${base_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${base_mass_iyy}"  iyz="0.000000"
        izz="${base_mass_izz}"/>
    </inertial>
  </link>


  <!--Rear Right Wheel-->
  <joint name="rear_right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="rear_right_wheel_link"/>
    <origin xyz="${-rear_tyre_x} ${-rear_tyre_y} ${rear_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="100000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>

  <link name="rear_right_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 -${PI/2}"/>
      <geometry>
        <mesh filename="package://car_model/meshes/wheel.dae"/>
        <!-- <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/> -->
      </geometry>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${rear_tyre_mass}"/>
      <inertia
        ixx="${rear_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${rear_tyre_mass_iyy}"  iyz="0.000000"
        izz="${rear_tyre_mass_izz}"/>
    </inertial>
  </link>

  <!--Rear Left Wheel-->
  <joint name="rear_left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="rear_left_wheel_link"/>
    <origin xyz="${-rear_tyre_x} ${rear_tyre_y} ${rear_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="100000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>

  <link name="rear_left_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 ${PI/2}"/>
      <geometry>
        <mesh filename="package://car_model/meshes/wheel.dae"/>
        <cylinder length="${rear_tyre_length}" radius="${rear_tyre_r}"/>
      </geometry>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${rear_tyre_mass}"/>
      <inertia
        ixx="${rear_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${rear_tyre_mass_iyy}"  iyz="0.000000"
        izz="${rear_tyre_mass_izz}"/>
    </inertial>
  </link> 

  <!--Front Right Steering-->
  <joint name="front_right_steering_joint" type="revolute">
    <parent link="base_link"/>
    <child link="front_right_steering_link"/>
    <origin xyz="${front_tyre_x} ${-front_tyre_y} ${front_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-str_angle}" upper="${str_angle}" effort="1000000.0" velocity="10000.0"/>
    <dynamics damping="10.0" friction="0.0"/>
  </joint>

  <link name="front_right_steering_link">
    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${str_length}" radius="${str_radius}"/>
      </geometry>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${str_mass}"/>
      <inertia
      ixx="${str_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
      iyy="${str_mass_iyy}"  iyz="0.000000"
      izz="${str_mass_izz}"/>
    </inertial>
  </link>


  <!--Front Right Wheel-->
  <joint name="front_right_wheel_joint" type="continuous">
    <parent link="front_right_steering_link"/>
    <child link="front_right_wheel_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="1000000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>

  <link name="front_right_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 -${PI/2}"/>
      <geometry>
        <mesh filename="package://car_model/meshes/wheel.dae"/>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry>  
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${front_tyre_mass}"/>
      <inertia
        ixx="${front_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${front_tyre_mass_iyy}"  iyz="0.000000"
        izz="${front_tyre_mass_izz}"/>
    </inertial>
  </link>

  <!--Front Left Steering-->
  <joint name="front_left_steering_joint" type="revolute">
    <parent link="base_link"/>
    <child link="front_left_steering_link"/>
    <origin xyz="${front_tyre_x} ${front_tyre_y} ${front_tyre_r}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-str_angle}" upper="${str_angle}" effort="1000000.0" velocity="10000.0"/>
    <dynamics damping="10.0" friction="0.0"/>
  </joint>

  <link name="front_left_steering_link">

    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${str_length}" radius="${str_radius}"/>
      </geometry>
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${str_mass}"/>
      <inertia
      ixx="${str_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
      iyy="${str_mass_iyy}"  iyz="0.000000"
      izz="${str_mass_izz}"/>
    </inertial>
  </link>


  <!--Front Left Wheel-->
  <joint name="front_left_wheel_joint" type="continuous">
    <parent link="front_left_steering_link"/>
    <child link="front_left_wheel_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="0.1"/>
    <limit effort="1000000" velocity="10000" />
    <joint_properties damping="0.0" friction="0.0" />
  </joint>

  <link name="front_left_wheel_link">
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
      <geometry>
        <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 ${PI/2}"/>
      <geometry>
        <mesh filename="package://car_model/meshes/wheel.dae"/>
        <!-- <cylinder length="${front_tyre_length}" radius="${front_tyre_r}"/> -->
      </geometry>  
    </visual>

    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${front_tyre_mass}"/>
      <inertia
        ixx="${front_tyre_mass_ixx}"  ixy="0.000000"  ixz="0.000000"
        iyy="${front_tyre_mass_iyy}"  iyz="0.000000"
        izz="${front_tyre_mass_izz}"/>
    </inertial>
  </link>  



  <!-- motors and transmissions for the two rear wheels -->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rear_right_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>  

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rear_left_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>  

  <!-- EPS and transmissions for the front steering -->
  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="front_right_steering_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="eps_right">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
      <motorTorqueConstant>1000000</motorTorqueConstant>
    </actuator>
  </transmission>

  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="front_left_steering_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="eps_left">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
      <motorTorqueConstant>1000000</motorTorqueConstant>
    </actuator>
  </transmission>



  <!-- Friction Parametres -->

  <gazebo reference="rear_right_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>
    <minDepth>0.01</minDepth>
    <maxVel>100</maxVel>  
  </gazebo>  

  <gazebo reference="rear_left_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>     
    <minDepth>0.01</minDepth>
    <maxVel>100</maxVel>      
  </gazebo>   

  <gazebo reference="front_right_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>
    <minDepth>0.01</minDepth>
    <maxVel>100</maxVel>           
  </gazebo> 

  <gazebo reference="front_left_wheel_link">
    <mu1>10000000</mu1>
    <mu2>10000000</mu2>
    <kp>10000000</kp>
    <kd>1</kd>  
    <minDepth>0.01</minDepth>
    <maxVel>100</maxVel>          
  </gazebo>    

  <!-- Gazebo Plugins -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/$(arg roboname)</robotNamespace>
      <robotParam>robot_description</robotParam>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <jointName>rear_left_wheel_joint, rear_right_wheel_joint, front_left_steering_joint, front_right_steering_joint, front_right_wheel_joint, front_left_wheel_joint</jointName>
      <updateRate>50.0</updateRate>
      <robotNamespace>/$(arg roboname)</robotNamespace>
      <alwaysOn>true</alwaysOn>
    </plugin>
  </gazebo>

</robot>

这里的transmission定义好过后,通过python文件来启动对几个关节的控制:

#!/usr/bin/env python

import rospy
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import math

class CmdVel2Gazebo:

    def __init__(self):
        rospy.init_node('cmdvel2gazebo', anonymous=True)
        
        rospy.Subscriber('/smart/cmd_vel', Twist, self.callback, queue_size=1)

        self.pub_steerL = rospy.Publisher('/smart/front_left_steering_position_controller/command', Float64, queue_size=1)
        self.pub_steerR = rospy.Publisher('/smart/front_right_steering_position_controller/command', Float64, queue_size=1)
        self.pub_rearL = rospy.Publisher('/smart/rear_left_velocity_controller/command', Float64, queue_size=1)
        self.pub_rearR = rospy.Publisher('/smart/rear_right_velocity_controller/command', Float64, queue_size=1)

        # initial velocity and tire angle are 0
        self.x = 0
        self.z = 0

        # car Wheelbase (in m)
        self.L = 1.868

        # car Tread
        self.T_front = 1.284
        self.T_rear = 1.284 #1.386

        # how many seconds delay for the dead man's switch
        self.timeout=rospy.Duration.from_sec(0.2);
        self.lastMsg=rospy.Time.now()

        # maximum steer angle of the "inside" tire
        self.maxsteerInside=0.6;

        # turning radius for maximum steer angle just with the inside tire
        # tan(maxsteerInside) = wheelbase/radius --> solve for max radius at this angle
        rMax = self.L/math.tan(self.maxsteerInside);

        # radius of inside tire is rMax, so radius of the ideal middle tire (rIdeal) is rMax+treadwidth/2
        rIdeal = rMax+(self.T_front/2.0)

        # maximum steering angle for ideal middle tire
        # tan(angle) = wheelbase/radius
        self.maxsteer=math.atan2(self.L,rIdeal)

        # loop
        rate = rospy.Rate(10) # run at 10Hz
        while not rospy.is_shutdown():
            self.publish()
            rate.sleep()
        

    def callback(self,data):
        # w = v / r
        self.x = data.linear.x / 0.3
        # constrain the ideal steering angle such that the ackermann steering is maxed out
        self.z = max(-self.maxsteer,min(self.maxsteer,data.angular.z))
        self.lastMsg = rospy.Time.now()

    def publish(self):
        # now that these values are published, we
        # reset the velocity, so that if we don't hear new
        # ones for the next timestep that we time out; note
        # that the tire angle will not change
        # NOTE: we only set self.x to be 0 after 200ms of timeout
        delta_last_msg_time = rospy.Time.now() - self.lastMsg
        msgs_too_old = delta_last_msg_time > self.timeout
        if msgs_too_old:
            self.x = 0
            msgRear = Float64()
            msgRear.data = self.x
            self.pub_rearL.publish(msgRear)
            self.pub_rearR.publish(msgRear)
            msgSteer = Float64()
            msgSteer.data = 0
            self.pub_steerL.publish(msgSteer)
            self.pub_steerR.publish(msgSteer)

            return

        # The self.z is the delta angle in radians of the imaginary front wheel of ackerman model.
        if self.z != 0:
            T_rear = self.T_rear
            T_front = self.T_front
            L=self.L
            # self.v is the linear *velocity*
            r = L/math.fabs(math.tan(self.z))

            rL_rear = r-(math.copysign(1,self.z)*(T_rear/2.0))
            rR_rear = r+(math.copysign(1,self.z)*(T_rear/2.0))
            rL_front = r-(math.copysign(1,self.z)*(T_front/2.0))
            rR_front = r+(math.copysign(1,self.z)*(T_front/2.0))
            msgRearR = Float64()
            # the right tire will go a little faster when we turn left (positive angle)
            # amount is proportional to the radius of the outside/ideal
            msgRearR.data = self.x*rR_rear/r;
            msgRearL = Float64()
            # the left tire will go a little slower when we turn left (positive angle)
            # amount is proportional to the radius of the inside/ideal
            msgRearL.data = self.x*rL_rear/r;

            self.pub_rearL.publish(msgRearL)
            self.pub_rearR.publish(msgRearR)

            msgSteerL = Float64()
            msgSteerR = Float64()
            # the left tire's angle is solved directly from geometry
            msgSteerL.data = math.atan2(L,rL_front)*math.copysign(1,self.z)
            self.pub_steerL.publish(msgSteerL)
    
            # the right tire's angle is solved directly from geometry
            msgSteerR.data = math.atan2(L,rR_front)*math.copysign(1,self.z)
            self.pub_steerR.publish(msgSteerR)
        else:
            # if we aren't turning
            msgRear = Float64()
            msgRear.data = self.x;
            self.pub_rearL.publish(msgRear)
            # msgRear.data = 0;
            self.pub_rearR.publish(msgRear)

            msgSteer = Float64()
            msgSteer.data = self.z

            self.pub_steerL.publish(msgSteer)
            self.pub_steerR.publish(msgSteer)


if __name__ == '__main__':
    try:
        CmdVel2Gazebo()
    except rospy.ROSInterruptException:
        pass

转动惯量的计算公式:
在这里插入图片描述

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

白云千载尽

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值