FFTAI / Wiki-GRx-Gazebo

GNU General Public License v3.0
20 stars 4 forks source link

rl_sim节点的订阅问题 #2

Open Wwwwjunjie opened 1 month ago

Wwwwjunjie commented 1 month ago

在rl_sim.cpp的构造函数中订阅了/cmd_vel话题

    // subscriber
    this->cmd_vel_subscriber = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);
    this->model_state_subscriber = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
    this->joint_state_subscriber = nh.subscribe<sensor_msgs::JointState>(this->ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);

其回调如下

void RL_Sim::CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
    this->cmd_vel = *msg;
}

但在RL_Sim::RunModel()模型运行函数中,来自cmd_vel的指令参数在项目中被注释,这意味着该订阅没有意义。

  // this->obs.commands = torch::tensor({{this->cmd_vel.linear.x, this->cmd_vel.linear.y, this->cmd_vel.angular.z}});
        this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
fan-ziqi commented 1 month ago

是的,设计之初考虑了使用ros topic发送速度指令,也就是cmd_vel,可以实现手柄控制 同时也兼容键盘的控制指令,也就是this->control 两者选其一即可