Skip to content

Commit

Permalink
Revert "[ros2] Add Stage property to assign a list of controllers to …
Browse files Browse the repository at this point in the history
…use when executing the planned trajectory (#355)"

This reverts commit 94d6514.
  • Loading branch information
sjahr committed Nov 3, 2023
1 parent 94d6514 commit f58bb89
Show file tree
Hide file tree
Showing 9 changed files with 0 additions and 74 deletions.
1 change: 0 additions & 1 deletion capabilities/src/execute_task_solution_capability.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,6 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
}
exec_traj.trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
exec_traj.trajectory->setRobotTrajectoryMsg(state, sub_traj.trajectory);
exec_traj.controller_name = sub_traj.execution_info.controller_names;

/* TODO add action feedback and markers */
exec_traj.effect_on_success = [this, sub_traj,
Expand Down
9 changes: 0 additions & 9 deletions core/include/moveit/task_constructor/stage.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,6 @@

#pragma once

#include "trajectory_execution_info.h"
#include "utils.h"
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/storage.h>
Expand Down Expand Up @@ -202,14 +201,6 @@ class Stage
/// marker namespace of solution markers
const std::string& markerNS() { return properties().get<std::string>("marker_ns"); }

/// Set and get info to use when executing the stage's trajectory
void setTrajectoryExecutionInfo(TrajectoryExecutionInfo trajectory_execution_info) {
setProperty("trajectory_execution_info", trajectory_execution_info);
}
TrajectoryExecutionInfo trajectoryExecutionInfo() {
return properties().get<TrajectoryExecutionInfo>("trajectory_execution_info");
}

/// forwarding of properties between interface states
void forwardProperties(const InterfaceState& source, InterfaceState& dest);
std::set<std::string> forwardedProperties() const {
Expand Down
47 changes: 0 additions & 47 deletions core/include/moveit/task_constructor/trajectory_execution_info.h

This file was deleted.

5 changes: 0 additions & 5 deletions core/src/container.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -365,13 +365,8 @@ void ContainerBase::insert(Stage::pointer&& stage, int before) {
if (!stage)
throw std::runtime_error(name() + ": received invalid stage pointer");

if (stage->trajectoryExecutionInfo().controller_names.empty()) {
stage->setTrajectoryExecutionInfo(this->trajectoryExecutionInfo());
}

StagePrivate* impl = stage->pimpl();
impl->setParent(this);

ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true);
ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage));
impl->setParentPosition(it);
Expand Down
2 changes: 0 additions & 2 deletions core/src/stage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -308,8 +308,6 @@ Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
auto& p = properties();
p.declare<double>("timeout", "timeout per run (s)");
p.declare<std::string>("marker_ns", name(), "marker namespace");
p.declare<TrajectoryExecutionInfo>("trajectory_execution_info", TrajectoryExecutionInfo(),
"settings used when executing the trajectory");

p.declare<std::set<std::string>>("forwarded_properties", std::set<std::string>(),
"set of interface properties to forward");
Expand Down
4 changes: 0 additions & 4 deletions core/src/storage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -229,10 +229,6 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, I
moveit_task_constructor_msgs::msg::SubTrajectory& t = msg.sub_trajectory.back();
SolutionBase::fillInfo(t.info, introspection);

const auto trajectory_execution_info =
creator()->properties().get<TrajectoryExecutionInfo>("trajectory_execution_info");
t.execution_info = trajectory_execution_info;

if (trajectory())
trajectory()->getRobotTrajectoryMsg(t.trajectory);

Expand Down
1 change: 0 additions & 1 deletion msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@ set(msg_files
"msg/SubTrajectory.msg"
"msg/TaskDescription.msg"
"msg/TaskStatistics.msg"
"msg/TrajectoryExecutionInfo.msg"
)

set(srv_files
Expand Down
3 changes: 0 additions & 3 deletions msgs/msg/SubTrajectory.msg
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
# generic solution information
SolutionInfo info

# trajectory execution information, like controller configuration
TrajectoryExecutionInfo execution_info

# trajectory
moveit_msgs/RobotTrajectory trajectory

Expand Down
2 changes: 0 additions & 2 deletions msgs/msg/TrajectoryExecutionInfo.msg

This file was deleted.

0 comments on commit f58bb89

Please sign in to comment.