mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
e4a503fe5f
* Find matching hardware_interface using the required type The name of the controller was used in order to find and start the matching hardware interface. In consequence this meant that one could only define one controller for each hardware interface. Now, the controller's required type of hardware interface is used to find and start the matching hardware interface. * separate read & update in controller consume is defined as read+update, but update does not include read in ros_control terminology. * Handle latency in pipeline loop The controllers need to update at a rate of *at least* 125Hz, but the wait_dequeue_timed call could in theory slow the loop down to 62.5Hz. The old ur_modern_driver worked around this problem by sending goals at 4*125Hz. This patch exploits the onTimeout method of a consumer to update with the specified frequency of the control loop, even if no new state message arrived after the previous command. * publish wrench w.r.t. tcp frame The messages had an empty frame_id before and could not be displayed in RViz * support ros_control in indigo
92 lines
2.5 KiB
C++
92 lines
2.5 KiB
C++
#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_modern_driver/log.h"
|
|
#include "ur_modern_driver/ros/hardware_interface.h"
|
|
#include "ur_modern_driver/ros/service_stopper.h"
|
|
#include "ur_modern_driver/ur/commander.h"
|
|
#include "ur_modern_driver/ur/consumer.h"
|
|
#include "ur_modern_driver/ur/rt_state.h"
|
|
|
|
class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
|
|
{
|
|
private:
|
|
ros::NodeHandle nh_;
|
|
ros::Time lastUpdate_;
|
|
controller_manager::ControllerManager controller_;
|
|
|
|
// 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);
|
|
};
|