cartographer_ros node （五）：服务处理

最新推荐文章于 2024-03-17 01:48:15 发布

Teamo1996

最新推荐文章于 2024-03-17 01:48:15 发布

阅读量436

点赞数

文章标签：自动驾驶

本文链接：https://blog.csdn.net/weixin_39061796/article/details/119111325

版权

本次主要讲解node.cc文件中的相关服务的回调函数，主要包括以下函数

子图获取服务

	/**
 * @brief 获取对应id轨迹的 索引为submap_index 的submap
 *
 * @param[in] request                     获取submap的请求
 * @param[out] response                   服务的回应
 * @return true:                          ROS的service只能返回true, 返回false程序会中断
 */
bool Node::HandleSubmapQuery(
    ::cartographer_ros_msgs::SubmapQuery::Request& request,
    ::cartographer_ros_msgs::SubmapQuery::Response& response) {
  absl::MutexLock lock(&mutex_);
  map_builder_bridge_.HandleSubmapQuery(request, response);
  return true;
}

其中的request和response的定义如下

int32 trajectory_id									// 轨迹id
int32 submap_index									// 子图索引
---
cartographer_ros_msgs/StatusResponse status			// 服务响应状态
int32 submap_version								// 子图版本
cartographer_ros_msgs/SubmapTexture[] textures		// 子图纹理

其中的子图纹理消息的定义如下

uint8[] cells										// 栅格数据
int32 width											// 切片子图宽度
int32 height										// 切片子图高度
float64 resolution									// 切片子图分辨率
geometry_msgs/Pose slice_pose						// 切片子图位姿

轨迹获取服务

	/**
 * @brief 获取对应id的轨迹
 *
 * @param[in] request
 * @param[out] response
 * @return true: ROS的service只能返回true, 返回false程序会中断
 */
bool Node::HandleTrajectoryQuery(
    ::cartographer_ros_msgs::TrajectoryQuery::Request& request,
    ::cartographer_ros_msgs::TrajectoryQuery::Response& response) {
  absl::MutexLock lock(&mutex_);

  // 检查对应id的轨迹是否存在, 如果存在判断一下该id轨迹的状态
  response.status = TrajectoryStateToStatus(
      request.trajectory_id,
      {TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
       TrajectoryState::FROZEN} /* valid states */);
  if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
    LOG(ERROR) << "Can't query trajectory from pose graph: "
               << response.status.message;
    return true;
  }
  // 获取轨迹
  map_builder_bridge_.HandleTrajectoryQuery(request, response);
  return true;
}

其中的request和response的定义如下

int32 trajectory_id									// 轨迹id
---
cartographer_ros_msgs/StatusResponse status			// 服务响应状态
geometry_msgs/PoseStamped[] trajectory				// 轨迹位姿数组

添加一条新轨迹服务

	/**
 * @brief 通过服务来开始一条新的轨迹
 *
 * @param[in] request
 * 配置文件的目录与名字, 是否使用初始位姿, 初始位姿以及其是相对于哪条轨迹的id, 
 * @param[out] response 返回轨迹的状态与id
 * @return true: ROS的service只能返回true, 返回false程序会中断
 */
bool Node::HandleStartTrajectory(
    ::cartographer_ros_msgs::StartTrajectory::Request& request,
    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
  TrajectoryOptions trajectory_options;
  // 获取配置文件内容
  std::tie(std::ignore, trajectory_options) = LoadOptions(
      request.configuration_directory, request.configuration_basename);

  // 如果给定了一个初始位姿
  if (request.use_initial_pose) {

    // 获取初始位姿并判断有效性
    const auto pose = ToRigid3d(request.initial_pose);
    if (!pose.IsValid()) {
      response.status.message =
          "Invalid pose argument. Orientation quaternion must be normalized.";
      LOG(ERROR) << response.status.message;
      response.status.code =
          cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
      return true;
    }

    // 检查 initial_pose 对应的轨迹id是否存在
    response.status = TrajectoryStateToStatus(
        request.relative_to_trajectory_id,
        {TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
         TrajectoryState::FINISHED} /* valid states */);
    if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
      LOG(ERROR) << "Can't start a trajectory with initial pose: "
                 << response.status.message;
      return true;
    }

    ::cartographer::mapping::proto::InitialTrajectoryPose initial_trajectory_pose;
    initial_trajectory_pose.set_to_trajectory_id(
        request.relative_to_trajectory_id);
        
    // 将pose转成proto格式,放进initial_trajectory_pose
    *initial_trajectory_pose.mutable_relative_pose() =
        cartographer::transform::ToProto(pose);
    initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal(
        ::cartographer_ros::FromRos(ros::Time(0))));

    // 将初始位姿信息加入到trajectory_options中
    *trajectory_options.trajectory_builder_options
         .mutable_initial_trajectory_pose() = initial_trajectory_pose;
  }

  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  if (!ValidateTrajectoryOptions(trajectory_options)) {
    response.status.message = "Invalid trajectory options.";
    LOG(ERROR) << response.status.message;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
  }
  // 检查topic名字是否被其他轨迹使用
  else if (!ValidateTopicNames(trajectory_options)) {
    response.status.message = "Topics are already used by another trajectory.";
    LOG(ERROR) << response.status.message;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
  }
  // 检查通过, 添加一个新的轨迹
  else {
    response.status.message = "Success.";
    response.trajectory_id = AddTrajectory(trajectory_options);
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
  }
  return true;
}

其中的request和response的定义如下

string configuration_directory							// 配置文件目录
string configuration_basename							// 配置文件基础名称
bool use_initial_pose									// 是否使用初始位姿
geometry_msgs/Pose initial_pose							// 初始位姿
int32 relative_to_trajectory_id							// 初始位姿定义相对的轨迹
---
cartographer_ros_msgs/StatusResponse status				// 服务响应状态
int32 trajectory_id										// 新轨迹的id

结束轨迹服务

	/**
 * @brief 结束一条轨迹
 *
 * @param[in] request 轨迹的id
 * @param[out]] response 返回StatusResponse格式的处理状态
 * @return true
 */
bool Node::HandleFinishTrajectory(
    ::cartographer_ros_msgs::FinishTrajectory::Request& request,
    ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
  absl::MutexLock lock(&mutex_);
  response.status = FinishTrajectoryUnderLock(request.trajectory_id);
  return true;
}

其中的request和response的定义如下

int32 trajectory_id										// 轨迹id
---
cartographer_ros_msgs/StatusResponse status				// 服务响应状态

序列化状态服务

	/**
 * @brief
 * 当前状态序列化为proto流文件.如果将'include_unfinished_submaps'设置为true, 
 * 则未完成的子图（即尚未接收到所有测距仪数据插入的子图）将包含在序列化状态中.
 *
 * @param[in] request 要生成的文件名,以及是否包含未完成的submap
 * @param[out] response
 * @return true
 */
bool Node::HandleWriteState(
    ::cartographer_ros_msgs::WriteState::Request& request,
    ::cartographer_ros_msgs::WriteState::Response& response) {
  absl::MutexLock lock(&mutex_);
  // 直接调用cartographer的map_builder_的SerializeStateToFile()函数进行文件的保存
  if (map_builder_bridge_.SerializeState(request.filename,
                                         request.include_unfinished_submaps)) {
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
    response.status.message =
        absl::StrCat("State written to '", request.filename, "'.");
  } else {
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message =
        absl::StrCat("Failed to write '", request.filename, "'.");
  }
  return true;
}

其中的request和response的定义如下

string filename										// 序列化文件的名称
bool include_unfinished_submaps						// 是否包含未完成的子图
---
cartographer_ros_msgs/StatusResponse status			// 服务响应状态

获取所有轨迹状态服务

	/**
 * @brief 获取所有轨迹的状态
 *
 * @param[in] request 无
 * @param[out] response 返回所有轨迹的状态
 * @return true
 */
bool Node::HandleGetTrajectoryStates(
    ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
    ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
  // enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };
  using TrajectoryState =
      ::cartographer::mapping::PoseGraphInterface::TrajectoryState;

  absl::MutexLock lock(&mutex_);
  response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
  response.trajectory_states.header.stamp = ros::Time::now();
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
    // 轨迹的id
    response.trajectory_states.trajectory_id.push_back(entry.first);
    // 每个轨迹对应一个TrajectoryStates
    switch (entry.second) {
      case TrajectoryState::ACTIVE:
        response.trajectory_states.trajectory_state.push_back(
            ::cartographer_ros_msgs::TrajectoryStates::ACTIVE);
        break;
      case TrajectoryState::FINISHED:
        response.trajectory_states.trajectory_state.push_back(
            ::cartographer_ros_msgs::TrajectoryStates::FINISHED);
        break;
      case TrajectoryState::FROZEN:
        response.trajectory_states.trajectory_state.push_back(
            ::cartographer_ros_msgs::TrajectoryStates::FROZEN);
        break;
      case TrajectoryState::DELETED:
        response.trajectory_states.trajectory_state.push_back(
            ::cartographer_ros_msgs::TrajectoryStates::DELETED);
        break;
    }
  }
  return true;
}

注意map_builder_bridge_.GetTrajectoryStates()的返回值为std::map<int, cartographer::mapping::PoseGraphInterface::TrajectoryState>，map的键是轨迹id，值是轨迹状态

其中的request和response的定义如下

	---
cartographer_ros_msgs/StatusResponse status					// 服务响应状态
cartographer_ros_msgs/TrajectoryStates trajectory_states	// 轨迹状态

其中的cartographer_ros_msgs/TrajectoryStates的定义如下

uint8 ACTIVE = 0
uint8 FINISHED = 1
uint8 FROZEN = 2
uint8 DELETED = 3

std_msgs/Header header										// 消息头，主要是时间戳
int32[] trajectory_id										// 轨迹id数组
uint8[] trajectory_state									// 与轨迹id一一对应的轨迹状态数组