Skip to content

Commit

Permalink
Use same base class for ROS2Controllers and MoveItControllers (widget…
Browse files Browse the repository at this point in the history
…/config/setup_step)
  • Loading branch information
DLu committed Jun 7, 2022
1 parent bc09c66 commit d64892e
Show file tree
Hide file tree
Showing 20 changed files with 1,127 additions and 651 deletions.
10 changes: 9 additions & 1 deletion moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,12 @@ add_library(${PROJECT_NAME}
src/controller_edit_widget.cpp
src/controllers.cpp
src/controllers_widget.cpp
src/controller_config.cpp
src/controllers_config.cpp
src/modified_urdf_config.cpp
src/moveit_controllers_config.cpp
src/moveit_controllers_widget.cpp
src/ros2_controllers_config.cpp
src/ros2_controllers_widget.cpp
src/urdf_modifications.cpp
src/urdf_modifications_widget.cpp
${MOC_FILES}
Expand Down Expand Up @@ -63,6 +67,10 @@ install(FILES moveit_setup_framework_plugins.xml
install(DIRECTORY include/
DESTINATION include
)

install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
ament_export_include_directories(include)

ament_export_libraries(${PROJECT_NAME})
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,13 +56,13 @@ class ControllerEditWidget : public QWidget
// ******************************************************************************************

/// Constructor
ControllerEditWidget(QWidget* parent, Controllers& setup_step);
ControllerEditWidget(QWidget* parent);

/// Set the previous data
void setSelected(const std::string& controller_name);
void setSelected(const std::string& controller_name, const ControllerInfo* info);

/// Populate the combo dropdown box with controllers types
void loadControllersTypesComboBox();
void loadControllersTypesComboBox(const std::vector<std::string>& controller_types);

/// Hide delete controller button
void hideDelete();
Expand Down Expand Up @@ -136,8 +136,6 @@ private Q_SLOTS:

// For loading default types combo box just once
bool has_loaded_ = false;

Controllers& setup_step_;
};
} // namespace controllers
} // namespace moveit_setup
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@

#include <moveit_setup_framework/setup_step.hpp>
#include <moveit_setup_framework/data/srdf_config.hpp>
#include <moveit_setup_controllers/controller_config.hpp>
#include <moveit_setup_controllers/controllers_config.hpp>

namespace moveit_setup
{
Expand All @@ -46,12 +46,13 @@ namespace controllers
class Controllers : public SetupStep
{
public:
std::string getName() const override
{
return "Controllers";
}
virtual std::string getInstructions() const = 0;

virtual std::string getButtonText() const = 0;

virtual std::vector<std::string> getAvailableTypes() const = 0;

void onInit() override;
virtual std::string getDefaultType() const = 0;

bool isReady() const override
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,104 +55,27 @@ struct ControllerInfo
std::vector<std::string> joints_; // joints controlled by this controller
};

static const std::string CONTROLLERS_YAML = "config/ros_controllers.yaml";

/**
* @brief All the controller configurations
*/
class ControllersConfig : public SetupConfig
{
public:
void loadPrevious(const std::filesystem::path& package_path, const YAML::Node& node) override;

bool isConfigured() const override
{
return !ros_controllers_config_.empty();
return !controllers_.empty();
}

/**
* \brief Gets ros_controllers_config_ vector
* \return pointer to ros_controllers_config_
* \brief Gets controllers_ vector
*/
std::vector<ControllerInfo>& getControllers()
{
return ros_controllers_config_;
return controllers_;
}

class GeneratedControllersConfig : public YamlGeneratedFile
{
public:
GeneratedControllersConfig(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
ControllersConfig& parent)
: YamlGeneratedFile(package_path, last_gen_time), parent_(parent)
{
}

bool hasChanges() const override
{
return parent_.changed_ || parent_.hasChangedGroups();
}

std::filesystem::path getRelativePath() const override
{
return CONTROLLERS_YAML;
}

std::string getDescription() const override
{
return "Creates configurations for ros_controllers.";
}

bool writeYaml(YAML::Emitter& emitter) override;

protected:
ControllersConfig& parent_;
};

class GeneratedContollerLaunch : public TemplatedGeneratedFile
{
public:
GeneratedContollerLaunch(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
ControllersConfig& parent)
: TemplatedGeneratedFile(package_path, last_gen_time), parent_(parent)
{
}

bool hasChanges() const override
{
return parent_.changed_ || parent_.hasChangedGroups();
}

std::filesystem::path getRelativePath() const override
{
return "launch/ros_controllers.launch";
}

std::filesystem::path getTemplatePath() const override
{
return getSharePath("moveit_setup_controllers") / "templates" / getRelativePath();
}

std::string getDescription() const override
{
return "ros_controllers launch file";
}

protected:
ControllersConfig& parent_;
};

void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
std::vector<GeneratedFilePtr>& files) override
{
files.push_back(std::make_shared<GeneratedControllersConfig>(package_path, last_gen_time, *this));
files.push_back(std::make_shared<GeneratedContollerLaunch>(package_path, last_gen_time, *this));
}

void collectVariables(std::vector<TemplateVariable>& variables) override;

/**
* \brief Adds a controller to ros_controllers_config_ vector
* \brief Adds a controller to controllers_ vector
* \param name Name of the controller
* \param type type of the controller
* \param joint_names vector of the joint names
Expand All @@ -161,7 +84,7 @@ class ControllersConfig : public SetupConfig
bool addController(const std::string& name, const std::string& type, const std::vector<std::string>& joint_names);

/**
* \brief Adds a controller to ros_controllers_config_ vector
* \brief Adds a controller to controllers_ vector
* \param new_controller a new Controller to add
* \return true if inserted correctly
*/
Expand Down Expand Up @@ -190,15 +113,8 @@ class ControllersConfig : public SetupConfig
}

protected:
/**
* Helper function for parsing ros_controllers.yaml file
* @param YAML::Node - individual controller to be parsed
* @return true if the file was read correctly
*/
bool parseController(const YAML::Node& controller);

/// Controllers config data
std::vector<ControllerInfo> ros_controllers_config_;
std::vector<ControllerInfo> controllers_;
bool changed_;
};
} // namespace controllers
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ class ControllersWidget : public SetupStepWidget

SetupStep& getSetupStep() override
{
return setup_step_;
return *setup_step_;
}

private Q_SLOTS:
Expand Down Expand Up @@ -118,7 +118,7 @@ private Q_SLOTS:
/// Called sleceted item changed
void itemSelectionChanged();

private:
protected:
// ******************************************************************************************
// Qt Components
// ******************************************************************************************
Expand All @@ -144,7 +144,7 @@ private Q_SLOTS:
/// Remember whethere we're editing a controller or adding a new one
bool adding_new_controller_;

Controllers setup_step_;
std::shared_ptr<Controllers> setup_step_;

/// Builds the main screen list widget
QWidget* createContentsWidget();
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2021, PickNik Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PickNik Robotics nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: David V. Lu!! */

#pragma once

#include <moveit_setup_controllers/controllers.hpp>
#include <moveit_setup_controllers/moveit_controllers_config.hpp>

namespace moveit_setup
{
namespace controllers
{
class MoveItControllers : public Controllers
{
public:
std::string getName() const override
{
return "MoveIt Controllers";
}

void onInit() override
{
config_data_->registerType("moveit_controllers", "moveit_setup::controllers::MoveItControllersConfig");
srdf_config_ = config_data_->get<SRDFConfig>("srdf");
controllers_config_ = config_data_->get<MoveItControllersConfig>("moveit_controllers");
}

std::string getInstructions() const override
{
return "Configure controllers to be used by MoveIt's controller manager(s) to operate the robot's physical "
"hardware";
}

std::string getButtonText() const override
{
return "Auto Add &FollowJointsTrajectory \n Controllers For Each Planning Group";
}

std::vector<std::string> getAvailableTypes() const override
{
return { "FollowJointTrajectory", "GripperCommand" };
}

std::string getDefaultType() const override
{
return "FollowJointTrajectory";
}
};
} // namespace controllers
} // namespace moveit_setup
Loading

0 comments on commit d64892e

Please sign in to comment.