ROS从入门到精通8-2：构造动态多行人可视化仿真环境

0 专栏介绍

本专栏旨在通过对ROS的系统学习，掌握ROS底层基本分布式原理，并具有机器人建模和应用ROS进行实际项目的开发和调试的工程能力。

详情：《ROS从入门到精通》

---

1 社会性导航

最初，移动机器人的应用仅限于制造业。但如今，随着人工智能技术的发展，它在娱乐、医疗、采矿、救援、教育、军事、空间、农业等领域被广泛应用。人们希望机器人能更多地参与到人们的生活中，提高它们的社会服务能力，称这类机器人会社会机器人(social robot)。

social robot的应用场景也非常广泛，包括但不限于：

• 教育：可以作为学习辅助工具，帮助儿童和成人学习语言、数学、科学等知识；
• 医疗：可以用于医疗辅助，如陪伴孤独老人、帮助病人进行康复训练等
• 家庭服务：可以作为家庭服务机器人，帮助家庭进行家务、安全监控等任务
• 商业：可以用于商业服务，如客户服务、导购助手等
• 娱乐：可以用于娱乐领域，如陪伴、游戏等

在社会机器人的应用中，有一类问题至关重要，称为社会性导航(social navigation)，具体地，它指机器人在移动过程中需要与周围的人类进行交互，并且需要考虑到人类的社交行为和规范。在这种情况下，机器人需要能够理解人类的意图和行为，预测他们的行动和意图，以便与他们协同工作，共同完成任务。

在social navigation问题中，机器人需要考虑到人类的社交规范和习惯，如礼貌、尊重和合作等。机器人需要与人类进行有效的沟通，并根据人类的反馈来调整自己的行为。此外，机器人还需要考虑到人类的情感状态，并采取相应的行动来缓解紧张和不适感。同时，为了确保机器人能够与人类进行有效的social navigation，需要在机器人的程序中嵌入相应的社交规范和策略，如避让规则、社交礼仪和沟通技巧等，以确保机器人的行为合理且能够与人类进行良好的互动。

2 动态多行人环境

在社会性导航问题的研究中，一个基础性的问题是如何搭建实验仿真环境？在ROS从入门到精通2-5：Gazebo插件制作案例(带碰撞属性的多行人场景)中，我们介绍了如何通过Gazebo插件将行人引入三维世界场景，同时让他们遵守行人规则(由SFM模型保证)

但是，在这个环境中，我们无法知道行人的速度、位姿信息，也无法在Rviz上可视化行人，这给实际应用带来很多困难。而现有的一些库对机器人Rviz骨架建立了良好的模型，例如pedsim_ros，但它们没有提供三维世界的仿真。

为此，本文将已有的Rviz模型和Gazebo模型相结合，实现可知行人位姿信息的全套可视化仿真环境。该软件包可用于机器人导航实验，用于模拟在实际场景中难以获取的拥挤场景。而且，单个行人的行走基于社交力模型，可实现实时处理非常大的人群。

3 代码实现

为了实现需求，我们需要实现两个插件：Rviz可视化插件和Gazebo可视化插件

3.1 Gazebo可视化插件

核心原理如下所示，就是订阅Gazebo行人模型的位姿状态并发布出去，行人模型请参考ROS从入门到精通2-5：Gazebo插件制作案例(带碰撞属性的多行人场景)，发布的消息包含行人的id、位置、速度

for (unsigned int i = 0; i < world_->ModelCount(); ++i)
  {
    physics::ModelPtr model = world_->ModelByIndex(i);
    if (((int)model->GetType() == (int)actor_->GetType()))
    {
      human_id++;
      pedsim_msgs::TrackedPerson person;
      person.track_id = human_id;
      person.is_occluded = false;
      person.detection_id = human_id;

      ros::ServiceClient state_client = node_->serviceClient<gazebo_sfm_plugin::ped_state>(model->GetName() + "_state");
      gazebo_sfm_plugin::ped_state model_state;
      model_state.request.name = model->GetName();
      state_client.call(model_state);

      geometry_msgs::PoseWithCovariance pose_with_cov;
      pose_with_cov.pose.position.x = model_state.response.px;
      pose_with_cov.pose.position.y = model_state.response.py;
      pose_with_cov.pose.position.z = model_state.response.pz;
      tf2::Quaternion q;
      q.setRPY(0, 0, model_state.response.theta - 1.57);
      tf2::convert(q, pose_with_cov.pose.orientation);
      person.pose = pose_with_cov;

      geometry_msgs::TwistWithCovariance twist_with_cov;
      twist_with_cov.twist.linear.x = model_state.response.vx;
      twist_with_cov.twist.linear.y = model_state.response.vy;
      person.twist = twist_with_cov;

      tracked_people.tracks.push_back(person);
    }
  }
  ped_visual_pub_.publish(tracked_people);

3.2 Rviz可视化插件

Rviz可视化插件采用开源模型spencer_tracking_rviz_plugin，它内置了行人骨架，可视化效果非常棒！这个开源仓库的实现比较复杂，关键的部分在于

trackedPersonVisual->isDeleted = false;
trackedPersonVisual->numCyclesNotSeen = 0;

Ogre::SceneNode* currentSceneNode = trackedPersonVisual->sceneNode.get();

//
// Person visualization
//

// Create new visual for the person itself, if needed
boost::shared_ptr<PersonVisual>& personVisual = trackedPersonVisual->personVisual;
createPersonVisualIfRequired(currentSceneNode, personVisual);

const double personHeight = personVisual ? personVisual->getHeight() : 0;
const double halfPersonHeight = personHeight / 2.0;

//
// Position of entire track
//

const Ogre::Matrix3 covXYZinTargetFrame = covarianceXYZIntoTargetFrame(trackedPersonIt->pose);
setPoseOrientation(currentSceneNode, trackedPersonIt->pose, covXYZinTargetFrame, personHeight);

必须注意的是，在Ubuntu20环境中，这个插件不能直接运行，需要将src/visuals/mesh_node.h和src/visuals/person_visual.cpp中的

Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create(ss.str(), "rviz");

修改为

Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);

3.3 环境效果

本文完整工程代码请通过下方名片联系博主获取

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系