@@ -325,10 +325,10 @@ void LocalPlanServer::process_goal() {
325325 MPL::Waypoint3D start, goal;
326326 // instead of using current odometry as start, we use the given start position
327327 // for consistency between old and new trajectories in replan process
328- start.pos = kr_planning_rviz_plugins ::pose_to_eigen (goal_->p_init );
329- start.vel = kr_planning_rviz_plugins ::twist_to_eigen (goal_->v_init );
330- start.acc = kr_planning_rviz_plugins ::twist_to_eigen (goal_->a_init );
331- start.jrk = kr_planning_rviz_plugins ::twist_to_eigen (goal_->j_init );
328+ start.pos = kr ::pose_to_eigen (goal_->p_init );
329+ start.vel = kr ::twist_to_eigen (goal_->v_init );
330+ start.acc = kr ::twist_to_eigen (goal_->a_init );
331+ start.jrk = kr ::twist_to_eigen (goal_->j_init );
332332
333333 // Important: define use position, velocity, acceleration or jerk as control
334334 // inputs, note that the lowest order "false" term will be used as control
@@ -342,10 +342,10 @@ void LocalPlanServer::process_goal() {
342342 // in trajectory_tracker)
343343 start.use_yaw = false ;
344344
345- goal.pos = kr_planning_rviz_plugins ::pose_to_eigen (goal_->p_final );
346- goal.vel = kr_planning_rviz_plugins ::twist_to_eigen (goal_->v_final );
347- goal.acc = kr_planning_rviz_plugins ::twist_to_eigen (goal_->a_final );
348- goal.jrk = kr_planning_rviz_plugins ::twist_to_eigen (goal_->j_final );
345+ goal.pos = kr ::pose_to_eigen (goal_->p_final );
346+ goal.vel = kr ::twist_to_eigen (goal_->v_final );
347+ goal.acc = kr ::twist_to_eigen (goal_->a_final );
348+ goal.jrk = kr ::twist_to_eigen (goal_->j_final );
349349 goal.use_yaw = start.use_yaw ;
350350 goal.use_pos = start.use_pos ;
351351 goal.use_vel = start.use_vel ;
@@ -440,7 +440,7 @@ bool LocalPlanServer::local_plan_process(
440440 vec_Vec3f sg;
441441 sg.push_back (start.pos );
442442 sg.push_back (goal.pos );
443- kr_planning_msgs::Path sg_msg = kr_planning_rviz_plugins ::path_to_ros (sg);
443+ kr_planning_msgs::Path sg_msg = kr ::path_to_ros (sg);
444444 std::string map_frame; // set frame id
445445 map_frame = map.header .frame_id ;
446446 sg_msg.header .frame_id = map_frame;
@@ -458,7 +458,7 @@ bool LocalPlanServer::local_plan_process(
458458 }
459459
460460 // for visualization: publish expanded nodes as a point cloud
461- sensor_msgs::PointCloud expanded_ps = kr_planning_rviz_plugins ::vec_to_cloud (
461+ sensor_msgs::PointCloud expanded_ps = kr ::vec_to_cloud (
462462 mp_planner_util_->getExpandedNodes ());
463463 expanded_ps.header .frame_id = map_frame;
464464 expanded_cloud_pub.publish (expanded_ps);
0 commit comments