Skip to content

Commit

Permalink
cleanup, revert dynamic reconfigure uncomment, fix parameter transfer…
Browse files Browse the repository at this point in the history 
… to controller_mgr_node

clang format fixes #3

compile and clang tidy fix

clang format fixes #4
  • Loading branch information
@ddengster
ddengster authored and AndyZe committed Sep 6, 2020
1 parent 9e95f14 commit 6788e42
Show file tree
Hide file tree
Showing 7 changed files with 47 additions and 119 deletions.
3 changes: 0 additions & 3 deletions moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,6 @@ class MoveGroupKinematicsService : public MoveGroupCapability
{
public:
MoveGroupKinematicsService();
~MoveGroupKinematicsService()
{
}

void initialize() override;

Expand Down
8 changes: 4 additions & 4 deletions moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,11 +271,11 @@ void MoveGroupMoveAction::setMoveState(MoveGroupState state, const std::shared_p
{
move_state_ = state;

if (goal.get())
if (goal)
{
auto move_feedback_ = std::make_shared<MGAction::Feedback>();
move_feedback_->state = stateToStr(state);
goal->publish_feedback(move_feedback_);
auto move_feedback = std::make_shared<MGAction::Feedback>();
move_feedback->state = stateToStr(state);
goal->publish_feedback(move_feedback);
}
}
} // namespace move_group
Expand Down
6 changes: 3 additions & 3 deletions moveit_ros/move_group/src/move_group.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,7 @@ T getParameterFromRemoteNode(const rclcpp::Node::SharedPtr& node, const std::str
if (!rclcpp::ok())
{
RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting.");
return 0;
return T();
}
RCLCPP_INFO(LOGGER, "service not available, waiting again...");
}
Expand Down Expand Up @@ -271,8 +271,8 @@ int main(int argc, char** argv)
planning_scene_monitor->publishDebugInformation(debug);

mge.status();
auto controller_mgr_node_ = mge.getContext()->trajectory_execution_manager_->getControllerManagerNode();
executor.add_node(controller_mgr_node_);
auto controller_mgr_node = mge.getContext()->trajectory_execution_manager_->getControllerManagerNode();
executor.add_node(controller_mgr_node);
executor.add_node(monitor_node);
executor.add_node(nh);
executor.spin();
Expand Down
4 changes: 2 additions & 2 deletions ...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -547,8 +547,8 @@ class PlanningSceneMonitor : private boost::noncopyable
void getPlanningSceneServiceCallback(moveit_msgs::srv::GetPlanningScene::Request::SharedPtr req,
moveit_msgs::srv::GetPlanningScene::Response::SharedPtr res);

void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates,
bool publish_transform_updates, bool publish_planning_scene);
void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates,
bool publish_planning_scene);

// Lock for state_update_pending_ and dt_state_update_
std::mutex state_pending_mutex_;
Expand Down
68 changes: 6 additions & 62 deletions moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,60 +58,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.planning_sce

namespace planning_scene_monitor
{
// This namespace is used for the dynamic_reconfigure
// using namespace moveit_ros_planning;

// class PlanningSceneMonitor::DynamicReconfigureImpl
// {
// public:
// DynamicReconfigureImpl(PlanningSceneMonitor* owner)
// : owner_(owner) /*, dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))*/
// {
// // TODO (anasarrak): re-add when starting with the parameters for ros2
// // dynamic_reconfigure_server_.setCallback(
// // boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
// }
//
// private:
// // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
// // instances in a process
// // TODO (anasarrak): Update advertise for ros2
// // static std::string decideNamespace(const std::string& name)
// // {
// // std::string ns = "~/" + name;
// // std::replace(ns.begin(), ns.end(), ' ', '_');
// // std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
// // if (ros::service::exists(ns + "/set_parameters", false))
// // {
// // unsigned int c = 1;
// // while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
// // c++;
// // ns += boost::lexical_cast<std::string>(c);
// // }
// // return ns;
// // }
// // TODO(anasarrak): uncomment this once the config for ROS2 is generated
// // void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level)
// // {
// // PlanningSceneMonitor::SceneUpdateType event = PlanningSceneMonitor::UPDATE_NONE;
// // if (config.publish_geometry_updates)
// // event = (PlanningSceneMonitor::SceneUpdateType)((int)event | (int)PlanningSceneMonitor::UPDATE_GEOMETRY);
// // if (config.publish_state_updates)
// // event = (PlanningSceneMonitor::SceneUpdateType)((int)event | (int)PlanningSceneMonitor::UPDATE_STATE);
// // if (config.publish_transforms_updates)
// // event = (PlanningSceneMonitor::SceneUpdateType)((int)event | (int)PlanningSceneMonitor::UPDATE_TRANSFORMS);
// // if (config.publish_planning_scene)
// // {
// // owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
// // owner_->startPublishingPlanningScene(event);
// // }
// // else
// // owner_->stopPublishingPlanningScene();
// // }
//
// // PlanningSceneMonitor* owner_;
// // dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
// };

const std::string PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
const std::string PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object";
Expand Down Expand Up @@ -283,15 +229,13 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
bool publish_state_updates = node_->get_parameter("planning_scene_monitor.publish_state_updates").as_bool();
bool publish_transform_updates = node_->get_parameter("planning_scene_monitor.publish_transforms_updates").as_bool();

updatePublishSettings(publish_geom_updates, publish_state_updates,
publish_transform_updates, publish_planning_scene);
updatePublishSettings(publish_geom_updates, publish_state_updates, publish_transform_updates, publish_planning_scene);

auto psm_parameter_set_callback = [this](std::vector<rclcpp::Parameter> parameters)
{
auto psm_parameter_set_callback = [this](std::vector<rclcpp::Parameter> parameters) {
auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
bool publish_planning_scene = false, publish_geometry_updates = false,
publish_state_updates = false, publish_transform_updates = false;
bool publish_planning_scene = false, publish_geometry_updates = false, publish_state_updates = false,
publish_transform_updates = false;
for (auto parameter : parameters)
{
rclcpp::ParameterType parameter_type = parameter.get_type();
Expand All @@ -313,8 +257,8 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
result.successful = false;
}
if (result.successful)
updatePublishSettings(publish_geometry_updates, publish_state_updates,
publish_transform_updates, publish_planning_scene);
updatePublishSettings(publish_geometry_updates, publish_state_updates, publish_transform_updates,
publish_planning_scene);
return result;
};
callback_handler_ = node_->add_on_set_parameters_callback(psm_parameter_set_callback);
Expand Down
3 changes: 0 additions & 3 deletions ...cution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -354,9 +354,6 @@ class TrajectoryExecutionManager

bool verbose_;

class DynamicReconfigureImpl;
DynamicReconfigureImpl* reconfigure_impl_;

bool execution_duration_monitoring_;
// 'global' values
double allowed_execution_duration_scaling_;
Expand Down
74 changes: 32 additions & 42 deletions moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,32 +56,32 @@ static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =

// using namespace moveit_ros_planning; // Used for dynamic_reconfigure

class TrajectoryExecutionManager::DynamicReconfigureImpl
{
public:
DynamicReconfigureImpl(TrajectoryExecutionManager* owner)
: owner_(owner) /*, dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))*/
{
// TODO: generate a similar thing for ros2 using the parameters
// dynamic_reconfigure_server_.setCallback(
// boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
}

private:
// TODO: generate a similar thing for ros2 using the parameters
// void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level)
// {
// owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
// owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
// owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
// owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
// owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
// owner_->setWaitForTrajectoryCompletion(config.wait_for_trajectory_completion);
// }

TrajectoryExecutionManager* owner_;
// dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
};
// class TrajectoryExecutionManager::DynamicReconfigureImpl
// {
// public:
// DynamicReconfigureImpl(TrajectoryExecutionManager* owner)
// : owner_(owner) /*, dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))*/
// {
// // TODO: generate a similar thing for ros2 using the parameters
// // dynamic_reconfigure_server_.setCallback(
// // boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
// }

// private:
// // TODO: generate a similar thing for ros2 using the parameters
// // void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level)
// // {
// // owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
// // owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
// // owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
// // owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
// // owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
// // owner_->setWaitForTrajectoryCompletion(config.wait_for_trajectory_completion);
// // }

// TrajectoryExecutionManager* owner_;
// // dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
// };

TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& robot_model,
Expand All @@ -106,12 +106,10 @@ TrajectoryExecutionManager::~TrajectoryExecutionManager()
{
run_continuous_execution_thread_ = false;
stopExecution(true);
delete reconfigure_impl_;
}

void TrajectoryExecutionManager::initialize()
{
reconfigure_impl_ = nullptr;
verbose_ = false;
execution_complete_ = true;
stop_continuous_execution_ = false;
Expand Down Expand Up @@ -166,24 +164,17 @@ void TrajectoryExecutionManager::initialize()
if (!controller.empty())
try
{
//@note: because of things being singlethreaded, we make a node named "moveit_simple_controller_manager"
// then copy parameters from the move_group node and then add it to the multithreadedexecutor
// alternatives:
// node_->create_sub_node doesnt work
// rename the node under ros_controllers.yaml to moveit_simple_controller_manager
// We make a node called moveit_simple_controller_manager so it's able to
// receive callbacks on another thread. We then copy parameters from the move_group node
// and then add it to the multithreadedexecutor
rclcpp::NodeOptions opt;
opt.allow_undeclared_parameters(true);
opt.automatically_declare_parameters_from_overrides(true);
controller_mgr_node_.reset(new rclcpp::Node("moveit_simple_controller_manager", opt));

// the alternative is to create a node with name "moveit_simple_controller_manager"
// auto allparams = node_->get_node_parameters_interface()->get_parameter_overrides();
// for (auto param : allparams)
// {
// RCLCPP_INFO(LOGGER, "%s", param.first.c_str());

// controller_mgr_node_->set_parameter(rclcpp::Parameter(param.first, param.second));
// }
auto all_params = node_->get_node_parameters_interface()->get_parameter_overrides();
for (auto param : all_params)
controller_mgr_node_->set_parameter(rclcpp::Parameter(param.first, param.second));

controller_manager_ = controller_manager_loader_->createUniqueInstance(controller);
controller_manager_->initialize(controller_mgr_node_);
Expand All @@ -199,7 +190,6 @@ void TrajectoryExecutionManager::initialize()
event_topic_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
EXECUTION_EVENT_TOPIC, 100, std::bind(&TrajectoryExecutionManager::receiveEvent, this, std::placeholders::_1));

reconfigure_impl_ = new DynamicReconfigureImpl(this);
controller_mgr_node_->get_parameter("trajectory_execution.allowed_execution_duration_scaling",
allowed_execution_duration_scaling_);
controller_mgr_node_->get_parameter("trajectory_execution.allowed_goal_duration_margin",
Expand Down

0 comments on commit 6788e42

Please sign in to comment.