rpg_ig_active源码阅读(四)

2023-07-26 16:34:34

今天看的是flying_gazebo_stereo_cam的code_base文件夹下的部分和ros_nodes文件夹下的部分

pcl_rerouter.cpp

namespace ros_tools:

PclRerouter:
- PclRerouter 构造函数

  PclRerouter::PclRerouter( ros::NodeHandle nh, std::string in_name, std::string out_name  )
  : nh_(nh)
  , forward_one_(false)
  , has_published_one_(false)
  , one_to_srv_(false)
  {
    // 话题
    pcl_subscriber_ = nh_.subscribe( in_name, 1, &PclRerouter::pclCallback, this );
    pcl_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(out_name, 1);
    // server
    pcl_service_caller_ = nh_.serviceClient<ig_active_reconstruction_msgs::PclInput>(out_name);
  }

rerouteOneToTopic 发布topic
rerouteOneToSrv 发布server
pclCallback 回调函数根据forward_one_选择发布topic还是server

controller.cpp

namespace flying_gazebo_stereo_cam:

Controller:
- Controller 构造函数传入相机模型名称并初始化是否已移动和是否一直发布以及相机frame到图像frame的一个四元数

  Controller::Controller(std::string cam_model_name)
  : cam_model_name_(cam_model_name)
  , has_moved_(false)
  , keepPublishing_(false)
  , cam_to_image_(0.5,0.5,-0.5,0.5)
  {
  }

其他的函数

bool Controller::moveTo( movements::Pose new_pose )
给定新的位姿后直接将当前位姿设为新的位姿角度要乘以一个四元数，并发布一个service，名字是/gazebo/set_model_state去call那个server 返回是否call成功
movements::Pose Controller::currentPose()
通过call /gazebo/set_model_state 返回当前位置
startTfPublisher 发布camera 的frame到世界坐标系的frame的转换
stopTfPublisher 停止发布
keepPublishing 一直发布

robot_communication_interface.cpp

namespace flying_gazebo_stereo_cam:
只有CommunicationInterface这一个类继承的是 ig_active_reconstruction::robot::CommunicationInterface
有两个公有的成员变量

typedef ig_active_reconstruction::views::View View;
typedef ig_active_reconstruction::robot::MovementCost MovementCost;

和两个私有成员变量

std::shared_ptr<Controller> cam_controller_; //! For movements etc.
ros_tools::PclRerouter pcl_rerouter_;

构造函数初始化controller 和 pcl_rerouter

  CommunicationInterface::CommunicationInterface( ros::NodeHandle nh, std::shared_ptr<Controller> controller, std::string in_name, std::string out_name )
  : cam_controller_(controller)
  , pcl_rerouter_(nh,in_name,out_name)
  {
  }

其他函数有:

virtual View CommunicationInterface::getCurrentView()
返回当前相机的位姿cam_controller_->currentPose();
virtual ReceptionInfo retrieveData();
命令机器人去检索数据返回的数据类型是ReceptionInfo 成功为0 失败为1
-virtual MovementCost movementCost( View& target_view )
当前位姿到target_view的距离

auto distance = current.position - target_pos;
cost.cost = distance.norm();

virtual MovementCost movementCost( View& start_view, View& target_view, bool fill_additional_information );
emm根本没用到fill_additional_information

    auto distance = start_view.pose().position - target_view.pose().position;
    cost.cost = distance.norm();

virtual bool moveTo( View& target_view );
调用cam_controller_->moveTo(target_view.pose());

robot_interface.cpp

是一个gazebo里相机的ros节点实现，节点的名称是robot_interface

参数

  std::string model_name, camera_frame_name, world_frame_name, sensor_in_topic, sensor_out_name;
  ros_tools::getExpParam(model_name,"model_name");
  ros_tools::getExpParam(camera_frame_name,"camera_frame_name");
  ros_tools::getExpParam(world_frame_name,"world_frame_name");
  ros_tools::getExpParam(sensor_in_topic,"sensor_in_topic");
  ros_tools::getExpParam(sensor_out_name,"sensor_out_name");

model_name
相机的frame
世界frame
点云输入topic名称
点云输出topic名称

controller

  std::shared_ptr<Controller> controller = std::make_shared<Controller>(model_name);
  //! publish tf
  controller->startTfPublisher(camera_frame_name,world_frame_name);

关于智能指针的用法，是C++11标准里的，可以参考
常见用法是

std::shared_ptr<Test> p = std::make_shared<Test>();
std::shared_ptr<Test> p(new Test);

其中make_shared的优先级高于new

interface

  boost::shared_ptr<CommunicationInterface> robot_interface =
      boost::make_shared<CommunicationInterface>(nh,controller,sensor_in_topic,sensor_out_name);

communication interface

ig_active_reconstruction::robot::RosServerCI comm_unit(nh,robot_interface);

把通讯接口给ROS
这里还需要看目前没看到

码农公寓