Skip to content

Commit b65ab65

Browse files
ljarinljarin
committed
address linting
1 parent 68058eb commit b65ab65

File tree

5 files changed

+74
-64
lines changed

5 files changed

+74
-64
lines changed

‎autonomy_core/map_plan/action_planner/src/data_conversions.cpp‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,15 @@
1-
#include "data_conversions.h"
1+
#include <data_conversions.h>
22

3-
void setMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
3+
#include <algorithm>
4+
5+
void setMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
46
const kr_planning_msgs::VoxelMap& msg) {
57
Vec3f ori(msg.origin.x, msg.origin.y, msg.origin.z);
68
Vec3i dim(msg.dim.x, msg.dim.y, msg.dim.z);
79
map_util->setMap(ori, dim, msg.data, msg.resolution);
810
}
911

10-
void getMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
12+
void getMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
1113
kr_planning_msgs::VoxelMap& map) {
1214
Vec3f ori = map_util->getOrigin();
1315
Vec3i dim = map_util->getDim();
@@ -25,14 +27,14 @@ void getMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
2527
map.data = map_util->getMap();
2628
}
2729

28-
void setMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
30+
void setMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
2931
const kr_planning_msgs::VoxelMap& msg) {
3032
Vec3f ori(msg.origin.x, msg.origin.y, msg.origin.z);
3133
Vec3i dim(msg.dim.x, msg.dim.y, msg.dim.z);
3234
map_util->setMap(ori, dim, msg.data, msg.resolution);
3335
}
3436

35-
void getMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
37+
void getMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
3638
kr_planning_msgs::VoxelMap& map) {
3739
Vec3f ori = map_util->getOrigin();
3840
Vec3i dim = map_util->getDim();
@@ -50,14 +52,14 @@ void getMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
5052
map.data = map_util->getMap();
5153
}
5254

53-
void setMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
55+
void setMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
5456
const kr_planning_msgs::VoxelMap& msg) {
5557
Vec2f ori(msg.origin.x, msg.origin.y);
5658
Vec2i dim(msg.dim.x, msg.dim.y);
5759
map_util->setMap(ori, dim, msg.data, msg.resolution);
5860
}
5961

60-
void getMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
62+
void getMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
6163
kr_planning_msgs::VoxelMap& map) {
6264
Vec2f ori = map_util->getOrigin();
6365
Vec2i dim = map_util->getDim();
@@ -76,7 +78,8 @@ void getMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
7678
}
7779

7880
kr_planning_msgs::VoxelMap sliceMap(const kr_planning_msgs::VoxelMap& map,
79-
double h, double hh) {
81+
double h,
82+
double hh) {
8083
// slice a 3D voxel map
8184
double res = map.resolution;
8285
int hhi = hh / res;

‎autonomy_core/map_plan/action_planner/src/data_conversions.h‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,28 +1,31 @@
11
#pragma once
22

33
#include <jps/map_util.h>
4-
#include <mpl_collision/map_util.h>
54
#include <kr_planning_msgs/VoxelMap.h>
5+
#include <mpl_collision/map_util.h>
6+
#include <memory>
7+
68

7-
void setMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
9+
void setMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
810
const kr_planning_msgs::VoxelMap& msg);
911

10-
void getMap(std::shared_ptr<MPL::VoxelMapUtil>& map_util,
12+
void getMap(const std::shared_ptr<MPL::VoxelMapUtil>& map_util,
1113
kr_planning_msgs::VoxelMap& map);
1214

13-
void setMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
15+
void setMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
1416
const kr_planning_msgs::VoxelMap& msg);
1517

16-
void getMap(std::shared_ptr<JPS::VoxelMapUtil>& map_util,
18+
void getMap(const std::shared_ptr<JPS::VoxelMapUtil>& map_util,
1719
kr_planning_msgs::VoxelMap& map);
1820

19-
void setMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
21+
void setMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
2022
const kr_planning_msgs::VoxelMap& msg);
2123

22-
void getMap(std::shared_ptr<JPS::OccMapUtil>& map_util,
24+
void getMap(const std::shared_ptr<JPS::OccMapUtil>& map_util,
2325
kr_planning_msgs::VoxelMap& map);
2426

2527
// NOTE: This function is the same as getInflatedOccMap function in
2628
// voxel_mapper.cpp, should merge them.
2729
kr_planning_msgs::VoxelMap sliceMap(const kr_planning_msgs::VoxelMap& map,
28-
double h, double hh = 0);
30+
double h,
31+
double hh = 0);

‎autonomy_core/map_plan/action_planner/src/global_plan_server.cpp‎

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
#include <action_planner/ActionPlannerConfig.h>
22
#include <actionlib/server/simple_action_server.h>
3+
#include <data_conversions.h> // setMap, getMap, etc
34
#include <eigen_conversions/eigen_msg.h>
45
#include <jps/jps_planner.h> // jps related
56
#include <jps/map_util.h> // jps related
67
#include <kr_planning_msgs/PlanTwoPointAction.h>
78
#include <kr_planning_msgs/VoxelMap.h>
89
#include <kr_planning_rviz_plugins/data_ros_utils.h>
910
#include <nav_msgs/Odometry.h> // odometry
11+
#include <primitive_ros_utils.h>
1012
#include <ros/ros.h>
1113
#include <traj_opt_ros/ros_bridge.h>
1214

@@ -15,9 +17,6 @@
1517
#include <boost/thread/mutex.hpp>
1618
#include <boost/thread/thread.hpp>
1719

18-
#include "data_conversions.h" // setMap, getMap, etc
19-
#include "primitive_ros_utils.h"
20-
2120
using boost::irange;
2221

2322
class GlobalPlanServer {
@@ -122,7 +121,8 @@ class GlobalPlanServer {
122121
/**
123122
* @brief Slice map util, only used if plan with a 2d jps planner
124123
*/
125-
kr_planning_msgs::VoxelMap SliceMap(double h, double hh,
124+
kr_planning_msgs::VoxelMap SliceMap(double h,
125+
double hh,
126126
const kr_planning_msgs::VoxelMap& map);
127127

128128
/**
@@ -155,8 +155,8 @@ GlobalPlanServer::GlobalPlanServer(const ros::NodeHandle& nh) : pnh_(nh) {
155155
global_map_cleared_pub_ = pnh_.advertise<kr_planning_msgs::VoxelMap>(
156156
"global_voxel_map_cleared", 1, true);
157157

158-
global_map_sub_ = pnh_.subscribe("global_voxel_map", 2,
159-
&GlobalPlanServer::globalMapCB, this);
158+
global_map_sub_ = pnh_.subscribe(
159+
"global_voxel_map", 2, &GlobalPlanServer::globalMapCB, this);
160160

161161
ros::NodeHandle traj_planner_nh(pnh_, "trajectory_planner");
162162
traj_planner_nh.param("use_3d_global", use_3d_global_, false);
@@ -174,7 +174,10 @@ GlobalPlanServer::GlobalPlanServer(const ros::NodeHandle& nh) : pnh_(nh) {
174174
global_as_->start();
175175

176176
// odom callback
177-
odom_sub_ = pnh_.subscribe("odom", 10, &GlobalPlanServer::odom_callback, this,
177+
odom_sub_ = pnh_.subscribe("odom",
178+
10,
179+
&GlobalPlanServer::odom_callback,
180+
this,
178181
ros::TransportHints().tcpNoDelay());
179182

180183
// Set map util for jps
@@ -348,7 +351,8 @@ void GlobalPlanServer::goalCB() {
348351
}
349352

350353
bool GlobalPlanServer::global_plan_process(
351-
const MPL::Waypoint3D& start, const MPL::Waypoint3D& goal,
354+
const MPL::Waypoint3D& start,
355+
const MPL::Waypoint3D& goal,
352356
const kr_planning_msgs::VoxelMap& global_map) {
353357
std::string map_frame;
354358
map_frame = global_map.header.frame_id;
@@ -396,8 +400,8 @@ bool GlobalPlanServer::global_plan_process(
396400
path_pub_.publish(global_path_msg_);
397401
}
398402
} else {
399-
if (!jps_util_->plan(start.pos.topRows(2), goal.pos.topRows(2), 1.0,
400-
true)) {
403+
if (!jps_util_->plan(
404+
start.pos.topRows(2), goal.pos.topRows(2), 1.0, true)) {
401405
// jps_util_->plan params: start, goal, eps, use_jps
402406
ROS_WARN("Fail to plan a 2d global path!");
403407
return false;

‎autonomy_core/map_plan/action_planner/src/local_plan_server.cpp‎

Lines changed: 7 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,13 @@
11
#include <action_planner/ActionPlannerConfig.h>
22
#include <actionlib/server/simple_action_server.h>
3+
#include <data_conversions.h> // setMap, getMap, etc
34
#include <eigen_conversions/eigen_msg.h>
5+
#include <kr_planning_msgs/PlanTwoPointAction.h>
6+
#include <kr_planning_rviz_plugins/data_ros_utils.h>
47
#include <mpl_basis/trajectory.h>
58
#include <mpl_collision/map_util.h>
69
#include <mpl_planner/map_planner.h>
7-
#include <kr_planning_msgs/PlanTwoPointAction.h>
8-
#include <kr_planning_rviz_plugins/data_ros_utils.h>
10+
#include <primitive_ros_utils.h>
911
#include <ros/console.h>
1012
#include <ros/ros.h>
1113
#include <traj_opt_ros/ros_bridge.h>
@@ -15,9 +17,6 @@
1517
#include <boost/thread/mutex.hpp>
1618
#include <boost/thread/thread.hpp>
1719

18-
#include "primitive_ros_utils.h"
19-
#include "data_conversions.h" // setMap, getMap, etc
20-
2120
using boost::irange;
2221

2322
// Local planning server for Sikang's motion primitive planner
@@ -260,7 +259,7 @@ void LocalPlanServer::process_result(const MPL::Trajectory3D& traj,
260259
geometry_msgs::Pose p_fin;
261260
geometry_msgs::Twist v_fin, a_fin, j_fin;
262261

263-
MPL::Waypoint3D pt_f = traj.evaluate(endt * double(i + 1));
262+
MPL::Waypoint3D pt_f = traj.evaluate(endt * static_cast<double>(i + 1));
264263
// check if evaluation is successful, if not, set result->success to be
265264
// false! (if failure case, a null Waypoint is returned)
266265
if ((pt_f.pos(0) == 0) && (pt_f.pos(1) == 0) && (pt_f.pos(2) == 0) &&
@@ -269,7 +268,7 @@ void LocalPlanServer::process_result(const MPL::Trajectory3D& traj,
269268
ROS_WARN_STREAM(
270269
"waypoint evaluation failed, set result->success to be false");
271270
ROS_WARN_STREAM("trajectory total time:" << traj.total_t_);
272-
ROS_WARN_STREAM("evaluating at:" << endt * double(i + 1));
271+
ROS_WARN_STREAM("evaluating at:" << endt * static_cast<double>(i + 1));
273272
}
274273

275274
p_fin.position.x = pt_f.pos(0), p_fin.position.y = pt_f.pos(1),
@@ -386,7 +385,7 @@ kr_planning_msgs::VoxelMap LocalPlanServer::clear_map_position(
386385
local_map_cleared = local_map_original;
387386

388387
kr_planning_msgs::VoxelMap voxel_map;
389-
388+
390389
// Replaced with corresponding parameter value from VoxelMsg.msg
391390
int8_t val_free = voxel_map.val_free;
392391
ROS_WARN_ONCE("Value free is set as %d", val_free);

0 commit comments

Comments
 (0)