Skip to content

Commit 4142567

Browse files
vmayoralvmayoral
committed
Fixes to melodic-sync, adapt to ROS2 style
1 parent 371baab commit 4142567

5 files changed

Lines changed: 17 additions & 15 deletions

File tree

‎moveit_core/collision_distance_field/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
2121
urdf
2222
visualization_msgs
2323
tf2_eigen
24+
angles
2425
)
2526

2627
install(TARGETS ${MOVEIT_LIB_NAME}

‎moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -179,6 +179,6 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const rob
179179
double resolution);
180180

181181
void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame,
182-
visualization_msgs::MarkerArray& body_marker_array);
182+
visualization_msgs::msg::MarkerArray& body_marker_array);
183183
}
184184
#endif

‎moveit_core/collision_distance_field/src/collision_common_distance_field.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const rob
128128
}
129129

130130
void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& gsr, const std::string& reference_frame,
131-
visualization_msgs::MarkerArray& body_marker_array)
131+
visualization_msgs::msg::MarkerArray& body_marker_array)
132132
{
133133
// creating namespaces
134134
std::string robot_ns = gsr->dfce_->group_name_ + "_sphere_decomposition";

‎moveit_core/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
<buildtool_depend>ament_cmake</buildtool_depend>
2323

24+
<build_depend>angles</build_depend>
2425
<build_depend>assimp</build_depend>
2526
<build_depend>boost</build_depend>
2627
<build_depend>eigen</build_depend>
@@ -52,6 +53,7 @@
5253
<build_depend>xmlrpcpp</build_depend>
5354
<build_depend>rcutils</build_depend>
5455

56+
<exec_depend>angles</exec_depend>
5557
<exec_depend>assimp</exec_depend>
5658
<exec_depend>boost</exec_depend>
5759
<exec_depend>eigen</exec_depend>

‎moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp‎

Lines changed: 12 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -42,14 +42,13 @@
4242
#include <angles/angles.h>
4343
#include <cmath>
4444
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
45-
#include <ros/console.h>
4645
#include <vector>
47-
46+
#include "rclcpp/rclcpp.hpp"
4847
#include <iostream>
4948

5049
namespace trajectory_processing
5150
{
52-
const std::string LOGNAME = "trajectory_processing.time_optimal_trajectory_generation";
51+
rclcpp::Logger logger_trajectory_processing_optimal = rclcpp::get_logger("trajectory_processing.time_optimal_trajectory_generation");
5352
constexpr double EPS = 0.000001;
5453
class LinearPathSegment : public PathSegment
5554
{
@@ -553,7 +552,7 @@ bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double
553552
else if (path_vel < 0.0)
554553
{
555554
valid_ = false;
556-
ROS_ERROR_NAMED(LOGNAME, "Error while integrating forward: Negative path velocity");
555+
RCLCPP_ERROR(logger_trajectory_processing_optimal, "Error while integrating forward: Negative path velocity");
557556
return true;
558557
}
559558

@@ -650,7 +649,7 @@ void Trajectory::integrateBackward(std::list<TrajectoryStep>& start_trajectory,
650649
if (path_vel < 0.0)
651650
{
652651
valid_ = false;
653-
ROS_ERROR_NAMED(LOGNAME, "Error while integrating backward: Negative path velocity");
652+
RCLCPP_ERROR(logger_trajectory_processing_optimal, "Error while integrating backward: Negative path velocity");
654653
end_trajectory_ = trajectory;
655654
return;
656655
}
@@ -679,7 +678,7 @@ void Trajectory::integrateBackward(std::list<TrajectoryStep>& start_trajectory,
679678
}
680679

681680
valid_ = false;
682-
ROS_ERROR_NAMED(LOGNAME, "Error while integrating backward: Did not hit start trajectory");
681+
RCLCPP_ERROR(logger_trajectory_processing_optimal, "Error while integrating backward: Did not hit start trajectory");
683682
end_trajectory_ = trajectory;
684683
}
685684

@@ -880,7 +879,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
880879
const robot_model::JointModelGroup* group = trajectory.getGroup();
881880
if (!group)
882881
{
883-
ROS_ERROR_NAMED(LOGNAME, "It looks like the planner did not set the group the plan was computed for");
882+
RCLCPP_ERROR(logger_trajectory_processing_optimal, "It looks like the planner did not set the group the plan was computed for");
884883
return false;
885884
}
886885

@@ -892,12 +891,12 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
892891
}
893892
else if (max_velocity_scaling_factor == 0.0)
894893
{
895-
ROS_DEBUG_NAMED(LOGNAME, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
894+
RCLCPP_DEBUG(logger_trajectory_processing_optimal, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
896895
velocity_scaling_factor);
897896
}
898897
else
899898
{
900-
ROS_WARN_NAMED(LOGNAME, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
899+
RCLCPP_WARN(logger_trajectory_processing_optimal, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
901900
max_velocity_scaling_factor, velocity_scaling_factor);
902901
}
903902

@@ -908,12 +907,12 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
908907
}
909908
else if (max_acceleration_scaling_factor == 0.0)
910909
{
911-
ROS_DEBUG_NAMED(LOGNAME, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
910+
RCLCPP_DEBUG(logger_trajectory_processing_optimal, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
912911
acceleration_scaling_factor);
913912
}
914913
else
915914
{
916-
ROS_WARN_NAMED(LOGNAME, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
915+
RCLCPP_WARN(logger_trajectory_processing_optimal, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
917916
max_acceleration_scaling_factor, acceleration_scaling_factor);
918917
}
919918

@@ -974,7 +973,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
974973
// Return trajectory with only the first waypoint if there are not multiple diverse points
975974
if (points.size() == 1)
976975
{
977-
ROS_WARN_NAMED(LOGNAME, "Trajectory is not being parameterized since it only contains a single distinct waypoint.");
976+
RCLCPP_WARN(logger_trajectory_processing_optimal, "Trajectory is not being parameterized since it only contains a single distinct waypoint.");
978977
robot_state::RobotState waypoint = robot_state::RobotState(trajectory.getWayPoint(0));
979978
trajectory.clear();
980979
trajectory.addSuffixWayPoint(waypoint, 0.0);
@@ -985,7 +984,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
985984
Trajectory parameterized(Path(points, path_tolerance_), max_velocity, max_acceleration, 0.001);
986985
if (!parameterized.isValid())
987986
{
988-
ROS_ERROR_NAMED(LOGNAME, "Unable to parameterize trajectory.");
987+
RCLCPP_ERROR(logger_trajectory_processing_optimal, "Unable to parameterize trajectory.");
989988
return false;
990989
}
991990

0 commit comments

Comments
 (0)