Skip to content
Snippets Groups Projects
Commit b0d4a41f authored by Johannes Mey's avatar Johannes Mey
Browse files

improved structure and performance

parent 22b3039c
Branches
No related tags found
No related merge requests found
......@@ -74,8 +74,7 @@ void TimedPlanner::loadCircularTrajectory(double radius, geometry_msgs::Point or
}
}
bool TimedPlanner::loadWaypoints() {
bool TimedPlanner::updateWaypoints() {
if (nextTrajectory.is_initialized()) {
currentTrajectory = nextTrajectory.get();
nextTrajectory.reset();
......@@ -110,33 +109,28 @@ std::vector<moveit_msgs::RobotTrajectory> TimedPlanner::split(moveit::planning_i
}
void TimedPlanner::doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
loadWaypoints();
void TimedPlanner::doMotion() {
updateWaypoints();
do {
for (auto &waypoint : currentTrajectory.waypoints) {
moveit::planning_interface::MoveGroupInterface::Plan plan;
if (TrajectoryUtil::computePathToPose(group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
if (nextTrajectory.is_initialized()) {
return;
}
for (auto trajectory : split(group, plan)) {
if (TrajectoryUtil::computePathToPose(*group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) {
for (auto trajectory : split(*group, plan)) {
if (nextTrajectory.is_initialized()) {
ROS_WARN_STREAM("terminating current trajectory");
return;
}
ROS_INFO_STREAM(
"Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory);
moveit_msgs::MoveItErrorCodes errorCode = group->execute(trajectory);
if (errorCode.val != errorCode.SUCCESS) {
ROS_ERROR_STREAM("Error Code from executing segment: " << errorCode);
}
}
} else {
ROS_ERROR_STREAM(
"Unable to compute " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
ROS_ERROR_STREAM("Unable to find " << waypoint.mode << " path to the next waypoint." << std::endl << waypoint);
}
}
} while (currentTrajectory.loop);
......@@ -162,11 +156,10 @@ void TimedPlanner::newMotionSpeedFactorCallback(const std_msgs::Float64ConstPtr
}
TimedPlanner::TimedPlanner(moveit::planning_interface::MoveGroupInterface &group, const double defaultVelocity,
std::string defaultPlanningMode) : default_velocity(
defaultVelocity), default_planning_mode(std::move(defaultPlanningMode)) {
ros::NodeHandle node_handle("panda_mqtt_connector");
TimedPlanner::TimedPlanner(ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group,
const double defaultVelocity, std::string defaultPlanningMode) :
node_handle(&node_handle), group(&group), default_velocity(defaultVelocity),
default_planning_mode(std::move(defaultPlanningMode)), motionSpeedFactor(defaultVelocity) {
std::string defaultTrajectory = "<no value provided>";
node_handle.getParam("default_trajectory", defaultTrajectory);
......@@ -218,7 +211,7 @@ int main(int argc, char **argv) {
// setup this ros-node
ros::init(argc, argv, "timed_planner");
ros::NodeHandle node_handle("panda_mqtt_connector");
ros::AsyncSpinner spinner(1);
ros::AsyncSpinner spinner(2);
spinner.start();
// wait for robot init of robot_state_initializer
......@@ -229,15 +222,6 @@ int main(int argc, char **argv) {
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
TimedPlanner planner(group, .5, TrajectoryUtil::FLUID_PATH);
ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
&planner);
ros::Subscriber velocitySubscriber = node_handle.subscribe("max_velocity", 1000,
&TimedPlanner::newMotionSpeedFactorCallback,
&planner);
// init motionSpeedFactor
double factor = -1;
if (node_handle.getParam("robot_speed_factor", factor)) {
......@@ -254,11 +238,18 @@ int main(int argc, char **argv) {
constructPlate(collision_objects, 5.0, 5.0);
planning_scene_interface.applyCollisionObjects(collision_objects);
// everything is set up, we can start the planner now
TimedPlanner planner(node_handle, group, .5, TrajectoryUtil::FLUID_PATH);
ros::Subscriber sub = node_handle.subscribe("trajectory_update", 1000, &TimedPlanner::newTrajectoryCallback,
&planner);
ros::Subscriber velocitySubscriber = node_handle.subscribe("max_velocity", 1000,
&TimedPlanner::newMotionSpeedFactorCallback,
&planner);
while (ros::ok()) {
// execute a trajectory
planner.doMotion(node_handle, group);
planner.doMotion();
ros::Rate(100).sleep();
ros::spinOnce();
}
......
......@@ -23,9 +23,9 @@ public:
static std::vector<moveit_msgs::RobotTrajectory>
split(MoveGroupInterface &group, const MoveGroupInterface::Plan &plan);
void doMotion(const ros::NodeHandle &node_handle, MoveGroupInterface &group);
void doMotion();
TimedPlanner(MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
TimedPlanner(ros::NodeHandle &node_handle, MoveGroupInterface &group, double defaultVelocity, std::string defaultPlanningMode);
void newTrajectoryCallback(const panda_mqtt_connector::Trajectory::ConstPtr &msg);
void newMotionSpeedFactorCallback(const std_msgs::Float64ConstPtr &msg);
......@@ -33,10 +33,13 @@ public:
private:
double motionSpeedFactor;
std::shared_ptr<ros::NodeHandle> node_handle;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> group;
panda_mqtt_connector::Trajectory currentTrajectory;
boost::optional<panda_mqtt_connector::Trajectory> nextTrajectory;
bool loadWaypoints();
bool updateWaypoints();
void loadSquareTrajectory();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment