rpg_ig_active源码阅读(四)

今天看的是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
这里还需要看 目前没看到

launch

目前开了两个节点

• robot_interface
  模型 frame各种赋值
• rviz

还有一个launch simple_viewspace_module.launch
主要作用是加载预先设定的viewpoint 包含xyz和四元数