add namespace for data_ros_utils

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

@@ -262,8 +262,8 @@ 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,7 +283,7 @@ 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)

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);