mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
114 lines
3.3 KiB
C++
114 lines
3.3 KiB
C++
/*
|
|
* Copyright 2017, 2018 Simon Rasmussen (refactor)
|
|
*
|
|
* Copyright 2015, 2016 Thomas Timm Andersen (original version)
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#pragma once
|
|
#include <controller_manager/controller_manager.h>
|
|
#include <hardware_interface/force_torque_sensor_interface.h>
|
|
#include <hardware_interface/joint_command_interface.h>
|
|
#include <hardware_interface/joint_state_interface.h>
|
|
#include <hardware_interface/robot_hw.h>
|
|
#include <ros/ros.h>
|
|
#include <atomic>
|
|
#include "ur_rtde_driver/log.h"
|
|
#include "ur_rtde_driver/ros/hardware_interface.h"
|
|
#include "ur_rtde_driver/ros/service_stopper.h"
|
|
#include "ur_rtde_driver/ur/commander.h"
|
|
#include "ur_rtde_driver/ur/consumer.h"
|
|
#include "ur_rtde_driver/ur/rt_state.h"
|
|
|
|
namespace ur_rtde_driver
|
|
{
|
|
class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
|
|
{
|
|
private:
|
|
ros::NodeHandle nh_;
|
|
ros::Time lastUpdate_;
|
|
controller_manager::ControllerManager controller_;
|
|
bool robot_state_received_;
|
|
|
|
// state interfaces
|
|
JointInterface joint_interface_;
|
|
WrenchInterface wrench_interface_;
|
|
// controller interfaces
|
|
PositionInterface position_interface_;
|
|
VelocityInterface velocity_interface_;
|
|
|
|
// currently activated controller
|
|
HardwareInterface* active_interface_;
|
|
// map of switchable controllers
|
|
std::map<std::string, HardwareInterface*> available_interfaces_;
|
|
|
|
std::atomic<bool> service_enabled_;
|
|
std::atomic<uint32_t> service_cooldown_;
|
|
|
|
// helper functions to map interfaces
|
|
template <typename T>
|
|
void registerInterface(T* interface)
|
|
{
|
|
RobotHW::registerInterface<typename T::parent_type>(interface);
|
|
}
|
|
template <typename T>
|
|
void registerControllereInterface(T* interface)
|
|
{
|
|
registerInterface(interface);
|
|
available_interfaces_[T::INTERFACE_NAME] = interface;
|
|
}
|
|
|
|
void read(RTShared& state);
|
|
bool update();
|
|
bool write();
|
|
void reset();
|
|
|
|
public:
|
|
ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
|
|
double max_vel_change, std::string tcp_link);
|
|
virtual ~ROSController()
|
|
{
|
|
}
|
|
// from RobotHW
|
|
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
|
|
const std::list<hardware_interface::ControllerInfo>& stop_list);
|
|
// from URRTPacketConsumer
|
|
virtual void setupConsumer();
|
|
virtual bool consume(RTState_V1_6__7& state)
|
|
{
|
|
read(state);
|
|
return update();
|
|
}
|
|
virtual bool consume(RTState_V1_8& state)
|
|
{
|
|
read(state);
|
|
return update();
|
|
}
|
|
virtual bool consume(RTState_V3_0__1& state)
|
|
{
|
|
read(state);
|
|
return update();
|
|
}
|
|
virtual bool consume(RTState_V3_2__3& state)
|
|
{
|
|
read(state);
|
|
return update();
|
|
}
|
|
|
|
virtual void onTimeout();
|
|
|
|
virtual void onRobotStateChange(RobotState state);
|
|
};
|
|
} // namespace ur_rtde_driver
|