gazebo中添加动态行人方法对比插件代码注释

pedsim_ros, Plugin, addwaypoint方法对比

pedsim之前有写:pedsim_ros

add_waypoint与Plugin的参考:gazebo tutorial

Pedsim_rosadd_waypointPlugin
模型model:box/personactoractor
轨迹方式set_pose,离散仿人,自然仿人,自然
设置行人数量方便不方便不方便
行人间避让可以不可以不可以
处理障碍需要手动添加,无反馈读取模型中障碍,但是目前存在一些问题
设置行动区间waypoint形式走,提前给定提前给定在程序中有相关代码,可在到达目标点后随机产生给定区间内的新目标

Plugin

插件代码如下:

/*
 * Copyright (C) 2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * 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.
 *
*/

#include <functional>

#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "plugins/ActorPlugin.hh"

using namespace gazebo;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin)

#define WALKING_ANIMATION "walking"

/
ActorPlugin::ActorPlugin()
{
}

// 加载参数
void ActorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  this->sdf = _sdf;
  this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
  this->world = this->actor->GetWorld();

  this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
          std::bind(&ActorPlugin::OnUpdate, this, std::placeholders::_1)));

  this->Reset();

  // Read in the target weight
  if (_sdf->HasElement("target_weight"))
    this->targetWeight = _sdf->Get<double>("target_weight");
  else
    this->targetWeight = 1.15;

  // Read in the obstacle weight
  if (_sdf->HasElement("obstacle_weight"))
    this->obstacleWeight = _sdf->Get<double>("obstacle_weight");
  else
    this->obstacleWeight = 1.5;

  // Read in the animation factor (applied in the OnUpdate function).
  if (_sdf->HasElement("animation_factor"))
    this->animationFactor = _sdf->Get<double>("animation_factor");
  else
    this->animationFactor = 4.5;

  // Add our own name to models we should ignore when avoiding obstacles.
  this->ignoreModels.push_back(this->actor->GetName());

  // Read in the other obstacles to ignore
  if (_sdf->HasElement("ignore_obstacles"))
  {
    sdf::ElementPtr modelElem =
      _sdf->GetElement("ignore_obstacles")->GetElement("model");
    while (modelElem)
    {
      this->ignoreModels.push_back(modelElem->Get<std::string>());
      modelElem = modelElem->GetNextElement("model");
    }
  }
}

//设置skin,walk的方式,跟addwaypoint中 animation类似。
void ActorPlugin::Reset()
{
  this->velocity = 0.8;
  this->lastUpdate = 0;

  if (this->sdf && this->sdf->HasElement("target"))
    this->target = this->sdf->Get<ignition::math::Vector3d>("target");
  else
    this->target = ignition::math::Vector3d(0, -5, 1.2138);

  auto skelAnims = this->actor->SkeletonAnimations();
  if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
  {
    gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
  }
  else
  {
    // Create custom trajectory
    this->trajectoryInfo.reset(new physics::TrajectoryInfo());
    this->trajectoryInfo->type = WALKING_ANIMATION;
    this->trajectoryInfo->duration = 1.0;

    this->actor->SetCustomTrajectory(this->trajectoryInfo);
  }
}

// 选择新的目标点。
void ActorPlugin::ChooseNewTarget()
{
  ignition::math::Vector3d newTarget(this->target);
  while ((newTarget - this->target).Length() < 2.0)
  {
    newTarget.X(ignition::math::Rand::DblUniform(-3, 3.5));
    newTarget.Y(ignition::math::Rand::DblUniform(-10, 2));

    for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
    {
      double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
          - newTarget).Length();
      if (dist < 2.0) // 距离小于2才行,但是我觉得没必要。
      {
        newTarget = this->target;
        break;
      }
    }
  }
  this->target = newTarget;
}

//处理障碍,ignoreModels是读取.world文件中的ignor_obstacle中的model参数。
//要是自己建立的应该只有墙壁忽略但实际上并不能忽略,所以只需要忽略ground_plane就行了。
void ActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
{
  for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
  {
    physics::ModelPtr model = this->world->ModelByIndex(i);
    if (std::find(this->ignoreModels.begin(), this->ignoreModels.end(),
          model->GetName()) == this->ignoreModels.end())
    {
      ignition::math::Vector3d offset = model->WorldPose().Pos() -
        this->actor->WorldPose().Pos();
      double modelDist = offset.Length();
      if (modelDist < 4.0)
      {
        double invModelDist = this->obstacleWeight / modelDist;
        offset.Normalize();
        offset *= invModelDist;
        _pos -= offset;
      }
    }
  }
}

// 回调函数,
void ActorPlugin::OnUpdate(const common::UpdateInfo &_info)
{
  // Time delta
  double dt = (_info.simTime - this->lastUpdate).Double();

  ignition::math::Pose3d pose = this->actor->WorldPose();
  ignition::math::Vector3d pos = this->target - pose.Pos(); //位置(x,y,z)的差
  //当前actor的角度,0°是gazebo中的Y轴负方向
  ignition::math::Vector3d rpy = pose.Rot().Euler(); 

  double distance = pos.Length(); //距离

  // Choose a new target position if the actor has reached its current
  // target.
  if (distance < 0.3)
  {
    this->ChooseNewTarget();
    pos = this->target - pose.Pos();
  }

  // Normalize the direction vector, and apply the target weight
  pos = pos.Normalize() * this->targetWeight; //向量差单位化以后乘一个权重

  // Adjust the direction vector by avoiding obstacles
  this->HandleObstacles(pos);// 处理障碍,这个会在下边解释

  // Compute the yaw orientation 计算需要转过的角度,也会在下边解释
  ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
  yaw.Normalize(); //这个就是yaw本身

  // Rotate in place, instead of jumping.
  if (std::abs(yaw.Radian()) > IGN_DTOR(10))  // IGN_DTOR是将度数转换成弧度数
  {
    pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+
        yaw.Radian()*0.001); // 下一个时刻应该到的位置,距离越近就会越慢。
        // 始终是+是因为本身yaw会有负值。这样可以避免始终逆时针或者顺时针转。
  }
  else
  { //如果相差角度小于10,就行走,并且加上差的角度。
    pose.Pos() += pos * this->velocity * dt;
    pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
  }

  // Make sure the actor stays within bounds 确保在某个框内运动。
  pose.Pos().X(std::max(-3.0, std::min(3.5, pose.Pos().X())));
  pose.Pos().Y(std::max(-10.0, std::min(2.0, pose.Pos().Y())));
  pose.Pos().Z(1.2138);

  // Distance traveled is used to coordinate motion with the walking
  // animation动画。
  double distanceTraveled = (pose.Pos() -
      this->actor->WorldPose().Pos()).Length();

  this->actor->SetWorldPose(pose, false, false);
  this->actor->SetScriptTime(this->actor->ScriptTime() +
    (distanceTraveled * this->animationFactor));
  this->lastUpdate = _info.simTime;
}

其中处理障碍的原理如下:在这里插入图片描述
如果有不同理解的可以交流。右边是自己在测试过程中遇到的一个问题,在某一个地方一直晃动,移动很小。如果有什么解决方法可以解决也提前感谢。

yaw的计算方法,1.5707是由于gazebo中actor的模型的0°是gazebo中的-90°。
在这里插入图片描述
其他的在注释中都比较简单。

  • 6
    点赞
  • 34
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
Gazebo是一个用于仿真机器人和复杂系统的开源三维仿真环境。在Gazebo添加行人可以帮助我们模拟真实世界的人类行为和交互。 在Gazebo,我们可以通过一些步骤来添加行人。首先,我们需要创建一个行人模型。可以使用CAD软件或者从已有的行人选择一个合适的模型。接下来,我们需要定义行人的运动方式。这可以通过编写行人控制算法或者使用预定义的行人行走模式来实现。在定义好行人的模型和运动方式后,我们需要将其添加Gazebo的世界。 可以通过创建行人的SDF(Simulation Description Format)文件来添加行人。SDF文件是一种用于描述仿真世界的XML格式文件。在SDF文件,我们可以指定行人的名称、外观、运动方式、初始位置等属性。将行人的SDF文件保存到指定的目录后,我们可以通过调用Gazebo的API函数,在仿真环境加载并显示这个行人模型。 在Gazebo添加行人后,我们可以通过一些方法来控制和观察他们的行为。例如,我们可以通过键盘或者游戏手柄来控制行人的运动,使其在仿真环境行走、跑步或者停留。同时,我们还可以利用Gazebo提供的传感器模型,如摄像头或激光雷达,来观察行人的位置、速度等信息,并进行相关的分析和研究。 总之,通过在Gazebo添加行人,我们可以模拟真实世界行人行为和交互,从而更好地理解和研究人类活动。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值