RosRobot.hpp

Go to the documentation of this file.

#ifndef AIKIDO_ROBOT_ROS_ROSROBOT_HPP_
#define AIKIDO_ROBOT_ROS_ROSROBOT_HPP_
 
#include <string>
 
#include <controller_manager_msgs/ListControllers.h>
#include <controller_manager_msgs/LoadController.h>
#include <controller_manager_msgs/SwitchController.h>
#include <dart/utils/urdf/DartLoader.hpp>
 
#include "aikido/control/ros/RosJointModeCommandClient.hpp"
#include "aikido/io/CatkinResourceRetriever.hpp"
#include "aikido/robot/Robot.hpp"
 
namespace aikido {
namespace robot {
namespace ros {
 
AIKIDO_DECLARE_POINTERS(RosRobot)
 
class RosRobot : public Robot
{
public:
  RosRobot(
      const dart::common::Uri& urdf,
      const dart::common::Uri& srdf,
      const std::string name,
      const bool addDefaultExecutors,
      const dart::common::ResourceRetrieverPtr& retriever
      = std::make_shared<aikido::io::CatkinResourceRetriever>(),
      const ::ros::NodeHandle& node = ::ros::NodeHandle(),
      const std::string modeControllerName = "");
 
  RosRobot(
      dart::dynamics::ReferentialSkeletonPtr refSkeleton,
      Robot* rootRobot,
      dart::collision::CollisionDetectorPtr collisionDetector,
      std::shared_ptr<dart::collision::BodyNodeCollisionFilter> collisionFilter,
      const std::string name,
      const ::ros::NodeHandle& node = ::ros::NodeHandle(),
      const std::string modeControllerName = "");
 
  virtual ~RosRobot() = default;
 
  void step(const std::chrono::system_clock::time_point& timepoint) override;
 
  RobotPtr registerSubRobot(
      dart::dynamics::ReferentialSkeletonPtr refSkeleton,
      const std::string& name,
      const std::string& modeControllerName = "");
 
  void deactivateExecutor() override;
 
  int registerExecutor(
      aikido::control::ExecutorPtr executor,
      std::string desiredName,
      std::string controllerName,
      hardware_interface::JointCommandModes controllerMode);
 
  int registerExecutor(
      aikido::control::ExecutorPtr executor,
      std::string desiredName,
      std::string controllerName);
 
  bool activateExecutor(const int id) override;
 
  // This un-hides Robot::activateExecutor(const aikido::control::ExecutorType
  // type) and Robot::activateExecutor(std::string id)
  using Robot::activateExecutor;
 
  void setRosControllerClients(const std::string modeControllerName);
 
protected:
  std::unordered_map<int, hardware_interface::JointCommandModes>
      mRosControllerModes;
  std::unordered_map<int, std::string> mRosControllerNames;
 
  // Ros node associated with this robot.
  ::ros::NodeHandle mNode;
 
  // Ros list controller service client.
  std::shared_ptr<::ros::ServiceClient> mRosListControllersServiceClient;
 
  // Ros load controller service client.
  std::shared_ptr<::ros::ServiceClient> mRosLoadControllerServiceClient;
 
  // Ros switch controller service client.
  std::shared_ptr<::ros::ServiceClient> mRosSwitchControllerServiceClient;
 
  // Ros joint mode command client.
  // Default is nullptr so mode switching is impossible.
  std::shared_ptr<aikido::control::ros::RosJointModeCommandClient>
      mRosJointModeCommandClient;
 
  bool startController(const std::string startControllerName);
 
  bool stopController(const std::string stopControllerName);
 
  bool switchControllers(
      const std::vector<std::string> startControllersNames,
      const std::vector<std::string> stopControllersNames);
 
  bool loadController(const std::string loadControllerName);
 
  bool switchControllerMode(
      const hardware_interface::JointCommandModes jointMode);
 
  bool isControllerActive(const std::string controllerName);
};
 
} // namespace ros
} // namespace robot
} // namespace aikido
 
#endif // AIKIDO_ROBOT_ROS_ROSROBOT_HPP_