Skip to content

Commit

Permalink
add namespace for data_ros_utils
Browse files Browse the repository at this point in the history
  • Loading branch information
@ljarin
ljarin committed Jun 3, 2022
1 parent 7357511 commit 07fd9e2
Show file tree
Hide file tree
Showing 4 changed files with 63 additions and 47 deletions.
8 changes: 4 additions & 4 deletions autonomy_core/map_plan/action_planner/src/global_plan_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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_);
}
Expand All @@ -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_);
}
Expand Down
22 changes: 11 additions & 11 deletions autonomy_core/map_plan/action_planner/src/local_plan_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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);

Expand Down
61 changes: 37 additions & 24 deletions autonomy_core/map_plan/mapper/src/local_global_mapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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 {
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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);

Expand All @@ -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();
Expand All @@ -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(
Expand Down
19 changes: 11 additions & 8 deletions autonomy_core/state_machine/state_machine_core/src/local_global_replan_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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
// ########################################################################################################
Expand Down Expand Up @@ -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_);

Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 07fd9e2

Please sign in to comment.