add namespace for data_ros_utils

Author: ljarin

autonomy_core/map_plan/action_planner/src/global_plan_server.cpp

@@ -257,8 +257,8 @@ void GlobalPlanServer::process_goal() {
   }
 
   // record current odometry as start
-  start.pos = pose_to_eigen(odom_msg_->pose.pose);
-  goal.pos = pose_to_eigen(goal_->p_final);
+  start.pos = kr_planning_rviz_plugins::pose_to_eigen(odom_msg_->pose.pose);
+  goal.pos = kr_planning_rviz_plugins::pose_to_eigen(goal_->p_final);
 
   // call the planner
 
@@ -395,7 +395,7 @@ bool GlobalPlanServer::global_plan_process(
       }
 
       // publish global_path_msg_
-      global_path_msg_ = path_to_ros(global_path);
+      global_path_msg_ = kr_planning_rviz_plugins::path_to_ros(global_path);
       global_path_msg_.header.frame_id = map_frame;
       path_pub_.publish(global_path_msg_);
     }
@@ -414,7 +414,7 @@ bool GlobalPlanServer::global_plan_process(
       }
 
       // publish
-      global_path_msg_ = path_to_ros(global_path);
+      global_path_msg_ = kr_planning_rviz_plugins::path_to_ros(global_path);
       global_path_msg_.header.frame_id = map_frame;
       path_pub_.publish(global_path_msg_);
     }

autonomy_core/map_plan/action_planner/src/local_plan_server.cpp

@@ -325,10 +325,10 @@ void LocalPlanServer::process_goal() {
   MPL::Waypoint3D start, goal;
   // instead of using current odometry as start, we use the given start position
   // for consistency between old and new trajectories in replan process
-  start.pos = pose_to_eigen(goal_->p_init);
-  start.vel = twist_to_eigen(goal_->v_init);
-  start.acc = twist_to_eigen(goal_->a_init);
-  start.jrk = twist_to_eigen(goal_->j_init);
+  start.pos = kr_planning_rviz_plugins::pose_to_eigen(goal_->p_init);
+  start.vel = kr_planning_rviz_plugins::twist_to_eigen(goal_->v_init);
+  start.acc = kr_planning_rviz_plugins::twist_to_eigen(goal_->a_init);
+  start.jrk = kr_planning_rviz_plugins::twist_to_eigen(goal_->j_init);
 
   // Important: define use position, velocity, acceleration or jerk as control
   // inputs, note that the lowest order "false" term will be used as control
@@ -342,10 +342,10 @@ void LocalPlanServer::process_goal() {
   // in trajectory_tracker)
   start.use_yaw = false;
 
-  goal.pos = pose_to_eigen(goal_->p_final);
-  goal.vel = twist_to_eigen(goal_->v_final);
-  goal.acc = twist_to_eigen(goal_->a_final);
-  goal.jrk = twist_to_eigen(goal_->j_final);
+  goal.pos = kr_planning_rviz_plugins::pose_to_eigen(goal_->p_final);
+  goal.vel = kr_planning_rviz_plugins::twist_to_eigen(goal_->v_final);
+  goal.acc = kr_planning_rviz_plugins::twist_to_eigen(goal_->a_final);
+  goal.jrk = kr_planning_rviz_plugins::twist_to_eigen(goal_->j_final);
   goal.use_yaw = start.use_yaw;
   goal.use_pos = start.use_pos;
   goal.use_vel = start.use_vel;
@@ -440,7 +440,7 @@ bool LocalPlanServer::local_plan_process(
   vec_Vec3f sg;
   sg.push_back(start.pos);
   sg.push_back(goal.pos);
-  kr_planning_msgs::Path sg_msg = path_to_ros(sg);
+  kr_planning_msgs::Path sg_msg = kr_planning_rviz_plugins::path_to_ros(sg);
   std::string map_frame;  // set frame id
   map_frame = map.header.frame_id;
   sg_msg.header.frame_id = map_frame;
@@ -458,8 +458,8 @@ bool LocalPlanServer::local_plan_process(
   }
 
   // for visualization: publish expanded nodes as a point cloud
-  sensor_msgs::PointCloud expanded_ps =
-      vec_to_cloud(mp_planner_util_->getExpandedNodes());
+  sensor_msgs::PointCloud expanded_ps = kr_planning_rviz_plugins::vec_to_cloud(
+      mp_planner_util_->getExpandedNodes());
   expanded_ps.header.frame_id = map_frame;
   expanded_cloud_pub.publish(expanded_ps);
 

autonomy_core/map_plan/mapper/src/local_global_mapper.cpp

@@ -4,8 +4,8 @@ LocalGlobalMapperNode::LocalGlobalMapperNode(const ros::NodeHandle& nh)
     : nh_(nh) {
   initParams();
 
-  cloud_sub = nh_.subscribe(cloud_name_, 1,
-                            &LocalGlobalMapperNode::cloudCallback, this);
+  cloud_sub = nh_.subscribe(
+      cloud_name_, 1, &LocalGlobalMapperNode::cloudCallback, this);
 
   global_map_pub =
       nh_.advertise<kr_planning_msgs::VoxelMap>("global_voxel_map", 1, true);
@@ -116,9 +116,12 @@ void LocalGlobalMapperNode::globalMapInit() {
   const double global_res = global_map_info_.resolution;
   int8_t global_val_default = 0;
   // Initialize the mapper
-  global_voxel_mapper_.reset(new mapper::VoxelMapper(
-      global_origin, global_dim_d, global_res, global_val_default,
-      global_decay_times_to_empty_));
+  global_voxel_mapper_.reset(
+      new mapper::VoxelMapper(global_origin,
+                              global_dim_d,
+                              global_res,
+                              global_val_default,
+                              global_decay_times_to_empty_));
 
   // build the array for map inflation
   global_infla_array_.clear();
@@ -153,9 +156,12 @@ void LocalGlobalMapperNode::storageMapInit() {
   const double res = storage_map_info_.resolution;
   int8_t storage_val_default = 0;
   // Initialize the mapper
-  storage_voxel_mapper_.reset(new mapper::VoxelMapper(
-      storage_origin, storage_dim_d, res, storage_val_default,
-      local_decay_times_to_empty_));
+  storage_voxel_mapper_.reset(
+      new mapper::VoxelMapper(storage_origin,
+                              storage_dim_d,
+                              res,
+                              storage_val_default,
+                              local_decay_times_to_empty_));
 }
 
 void LocalGlobalMapperNode::localInflaInit() {
@@ -213,15 +219,17 @@ void LocalGlobalMapperNode::getLidarPoses(
     // for real robot, the point cloud frame_id may not exist in the tf tree,
     // manually defining it here.
     // TODO(xu): make this automatic
-    auto tf_map_lidar = tf_listener.LookupTransform(map_frame_, lidar_frame_,
-                                                    cloud_header.stamp);
-    auto tf_odom_lidar = tf_listener.LookupTransform(odom_frame_, lidar_frame_,
-                                                     cloud_header.stamp);
+    auto tf_map_lidar = tf_listener.LookupTransform(
+        map_frame_, lidar_frame_, cloud_header.stamp);
+    auto tf_odom_lidar = tf_listener.LookupTransform(
+        odom_frame_, lidar_frame_, cloud_header.stamp);
     if ((!tf_map_lidar) || (!tf_odom_lidar)) {
       ROS_WARN(
           "[Mapper real-robot:] Failed to get transform (either from %s to %s; "
           "or from %s to %s)",
-          lidar_frame_.c_str(), map_frame_.c_str(), lidar_frame_.c_str(),
+          lidar_frame_.c_str(),
+          map_frame_.c_str(),
+          lidar_frame_.c_str(),
           odom_frame_.c_str());
       return;
     } else {
@@ -237,13 +245,15 @@ void LocalGlobalMapperNode::getLidarPoses(
       ROS_WARN(
           "[Mapper simulation:] Failed to get transform map to lidar (from %s "
           "to %s)",
-          cloud_header.frame_id.c_str(), map_frame_.c_str());
+          cloud_header.frame_id.c_str(),
+          map_frame_.c_str());
       return;
     } else if (!tf_odom_lidar) {
       ROS_WARN(
           "[Mapper simulation:] Failed to get transform odom to lidar (from %s "
           "to %s)",
-          cloud_header.frame_id.c_str(), odom_frame_.c_str());
+          cloud_header.frame_id.c_str(),
+          odom_frame_.c_str());
       return;
     } else {
       *pose_map_lidar_ptr = *tf_map_lidar;
@@ -262,8 +272,10 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) {
   geometry_msgs::Pose pose_odom_lidar;
   getLidarPoses(cloud.header, &pose_map_lidar, &pose_odom_lidar);
 
-  const Eigen::Affine3d T_map_lidar = toTF(pose_map_lidar);
-  const Eigen::Affine3d T_odom_lidar = toTF(pose_odom_lidar);
+  const Eigen::Affine3d T_map_lidar =
+      kr_planning_rviz_plugins::toTF(pose_map_lidar);
+  const Eigen::Affine3d T_odom_lidar =
+      kr_planning_rviz_plugins::toTF(pose_odom_lidar);
 
   // This is the lidar position in the odom frame, used for raytracing &
   // cropping local map
@@ -283,12 +295,13 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) {
   double min_range = 0.75;  // points within this distance will be discarded
   double min_range_squared;
   min_range_squared = min_range * min_range;
-  const auto pts = cloud_to_vec_filter(cloud, min_range_squared);
+  const auto pts =
+      kr_planning_rviz_plugins::cloud_to_vec_filter(cloud, min_range_squared);
 
   timer.start();
   // local raytracing using lidar position in the map frame (not odom frame)
-  storage_voxel_mapper_->addCloud(pts, T_map_lidar, local_infla_array_, false,
-                                  local_max_raycast_);
+  storage_voxel_mapper_->addCloud(
+      pts, T_map_lidar, local_infla_array_, false, local_max_raycast_);
   ROS_DEBUG("[storage map addCloud]: %f",
             static_cast<double>(timer.elapsed().wall) / 1e6);
 
@@ -311,8 +324,8 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) {
   ++counter_;
   if (counter_ % update_interval_ == 0) {
     timer.start();
-    global_voxel_mapper_->addCloud(pts, T_map_lidar, global_infla_array_, false,
-                                   global_max_raycast_);
+    global_voxel_mapper_->addCloud(
+        pts, T_map_lidar, global_infla_array_, false, global_max_raycast_);
     ROS_DEBUG("[global map addCloud]: %f",
               static_cast<double>(timer.elapsed().wall) / 1e6);
     timer.start();
@@ -334,8 +347,8 @@ void LocalGlobalMapperNode::processCloud(const sensor_msgs::PointCloud& cloud) {
     global_map_pub.publish(global_map);
   }
 
-  ROS_DEBUG_THROTTLE(1, "[Mapper]: Got cloud, number of points: [%zu]",
-                     cloud.points.size());
+  ROS_DEBUG_THROTTLE(
+      1, "[Mapper]: Got cloud, number of points: [%zu]", cloud.points.size());
 }
 
 void LocalGlobalMapperNode::cloudCallback(

autonomy_core/state_machine/state_machine_core/src/local_global_replan_server.cpp

@@ -5,7 +5,8 @@
  */
 void RePlanner::GlobalPathCb(const kr_planning_msgs::Path& path) {
   global_path_.clear();
-  global_path_ = ros_to_path(path);  // extract the global path information
+  global_path_ = kr_planning_rviz_plugins::ros_to_path(
+      path);  // extract the global path information
 }
 
 void RePlanner::EpochCb(const std_msgs::Int64& msg) {
@@ -282,7 +283,7 @@ void RePlanner::setup_replanner() {
   }
   auto global_result = global_plan_client_->getResult();
   if (global_result->success) {
-    global_path_ = ros_to_path(
+    global_path_ = kr_planning_rviz_plugins::ros_to_path(
         global_result->path);  // extract the global path information
     ROS_WARN("initial global plan succeeded!");
   } else {
@@ -459,7 +460,7 @@ bool RePlanner::PlanTrajectory(int horizon) {
                                 &local_tpgoal.a_init,
                                 &local_tpgoal.j_init);
   Vec3f start_pos;
-  start_pos = pose_to_eigen(local_tpgoal.p_init);
+  start_pos = kr_planning_rviz_plugins::pose_to_eigen(local_tpgoal.p_init);
 
   // Replan step 1: Global plan
   // ########################################################################################################
@@ -770,7 +771,8 @@ vec_Vec3f RePlanner::PathCropIntersect(const vec_Vec3f& path) {
   }
 
   // publish for visualization
-  kr_planning_msgs::Path local_path_msg_ = path_to_ros(cropped_path);
+  kr_planning_msgs::Path local_path_msg_ =
+      kr_planning_rviz_plugins::path_to_ros(cropped_path);
   local_path_msg_.header.frame_id = map_frame_;
   cropped_path_pub_.publish(local_path_msg_);
 
@@ -815,7 +817,7 @@ vec_Vec3f RePlanner::TransformGlobalPath(const vec_Vec3f& path_original) {
   map_to_odom.orientation.z = transformStamped.transform.rotation.z;
 
   // TF transform from the map frame to odom frame
-  auto map_to_odom_tf = toTF(map_to_odom);
+  auto map_to_odom_tf = kr_planning_rviz_plugins::toTF(map_to_odom);
   Vec3f waypoint_wrt_map;
 
   vec_Vec3f path_wrt_map;
@@ -826,7 +828,8 @@ vec_Vec3f RePlanner::TransformGlobalPath(const vec_Vec3f& path_original) {
   }
 
   // publish transformed global path for visualization
-  kr_planning_msgs::Path path_wrt_map_msg = path_to_ros(path_wrt_map);
+  kr_planning_msgs::Path path_wrt_map_msg =
+      kr_planning_rviz_plugins::path_to_ros(path_wrt_map);
   path_wrt_map_msg.header.frame_id = map_frame_;
   global_path_wrt_map_pub_.publish(path_wrt_map_msg);
   return path_wrt_map;
@@ -1033,8 +1036,8 @@ RePlanner::RePlanner() : nh_("~") {
   cropped_path_pub_ =
       priv_nh.advertise<kr_planning_msgs::Path>("cropped_local_path", 1, true);
 
-  global_path_wrt_map_pub_ = priv_nh.advertise<kr_planning_msgs::Path>(
-      "global_path_wrt_map", 1, true);
+  global_path_wrt_map_pub_ =
+      priv_nh.advertise<kr_planning_msgs::Path>("global_path_wrt_map", 1, true);
 
   priv_nh.param("max_horizon", max_horizon_, 5);
   priv_nh.param("crop_radius", crop_radius_, 10.0);