Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

removed unused files

Browse Source
This commit is contained in:
Felix Mauch
2019-04-01 16:27:43 +02:00
parent 31746259cf
commit 44751cfb2a
20 changed files with 9 additions and 2810 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
include/ur_rtde_driver/ros/action_server.h
View File

@@ -1,96 +0,0 @@
/*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* 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 <actionlib/server/action_server.h>
#include <actionlib/server/server_goal_handle.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <ros/ros.h>
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <mutex>
#include <set>
#include <thread>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ros/service_stopper.h"
#include "ur_rtde_driver/ros/trajectory_follower.h"
#include "ur_rtde_driver/ur/consumer.h"
#include "ur_rtde_driver/ur/master_board.h"
#include "ur_rtde_driver/ur/state.h"
namespace ur_rtde_driver
{
class ActionServer : public Service, public URRTPacketConsumer
{
private:
typedef control_msgs::FollowJointTrajectoryAction Action;
typedef control_msgs::FollowJointTrajectoryResult Result;
typedef actionlib::ServerGoalHandle<Action> GoalHandle;
typedef actionlib::ActionServer<Action> Server;
ros::NodeHandle nh_;
Server as_;
std::vector<std::string> joint_names_;
std::set<std::string> joint_set_;
double max_velocity_;
GoalHandle curr_gh_;
std::atomic<bool> interrupt_traj_;
std::atomic<bool> has_goal_, running_;
std::mutex tj_mutex_;
std::condition_variable tj_cv_;
std::thread tj_thread_;
ActionTrajectoryFollowerInterface& follower_;
RobotState state_;
std::array<double, 6> q_actual_, qd_actual_;
void onGoal(GoalHandle gh);
void onCancel(GoalHandle gh);
bool validate(GoalHandle& gh, Result& res);
bool validateState(GoalHandle& gh, Result& res);
bool validateJoints(GoalHandle& gh, Result& res);
bool validateTrajectory(GoalHandle& gh, Result& res);
bool try_execute(GoalHandle& gh, Result& res);
void interruptGoal(GoalHandle& gh);
std::vector<size_t> reorderMap(std::vector<std::string> goal_joints);
void trajectoryThread();
bool updateState(RTShared& data);
public:
ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
void start();
virtual void onRobotStateChange(RobotState state);
virtual bool consume(RTState_V1_6__7& state);
virtual bool consume(RTState_V1_8& state);
virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state);
};
} // namespace ur_rtde_driver

55
include/ur_rtde_driver/ros/action_trajectory_follower_interface.h
View File

@@ -1,55 +0,0 @@
/*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* 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 <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <vector>
namespace ur_rtde_driver
{
struct TrajectoryPoint
{
std::array<double, 6> positions;
std::array<double, 6> velocities;
std::chrono::microseconds time_from_start;
TrajectoryPoint()
{
}
TrajectoryPoint(std::array<double, 6>& pos, std::array<double, 6>& vel, std::chrono::microseconds tfs)
: positions(pos), velocities(vel), time_from_start(tfs)
{
}
};
class ActionTrajectoryFollowerInterface
{
public:
virtual bool start() = 0;
virtual bool execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt) = 0;
virtual void stop() = 0;
virtual ~ActionTrajectoryFollowerInterface(){};
};
} // namespace ur_rtde_driver

113
include/ur_rtde_driver/ros/controller.h
View File

@@ -1,113 +0,0 @@
/*
* 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

104
include/ur_rtde_driver/ros/hardware_interface.h
View File

@@ -1,104 +0,0 @@
/*
* 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 <algorithm>
#include "ur_rtde_driver/ros/trajectory_follower.h"
#include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/rt_state.h"
namespace ur_rtde_driver
{
class HardwareInterface
{
public:
virtual bool write() = 0;
virtual void start()
{
}
virtual void stop()
{
}
virtual void reset()
{
}
};
using hardware_interface::JointHandle;
class JointInterface : public hardware_interface::JointStateInterface
{
private:
std::array<double, 6> velocities_, positions_, efforts_;
public:
JointInterface(std::vector<std::string>& joint_names);
void update(RTShared& packet);
typedef hardware_interface::JointStateInterface parent_type;
static const std::string INTERFACE_NAME;
};
class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface
{
std::array<double, 6> tcp_;
public:
WrenchInterface(std::string tcp_link);
void update(RTShared& packet);
typedef hardware_interface::ForceTorqueSensorInterface parent_type;
static const std::string INTERFACE_NAME;
};
class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface
{
private:
URCommander& commander_;
std::array<double, 6> velocity_cmd_, prev_velocity_cmd_;
double max_vel_change_;
public:
VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string>& joint_names, double max_vel_change);
virtual bool write();
virtual void reset();
typedef hardware_interface::VelocityJointInterface parent_type;
static const std::string INTERFACE_NAME;
};
class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
{
private:
TrajectoryFollower& follower_;
std::array<double, 6> position_cmd_;
public:
PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string>& joint_names);
virtual bool write();
virtual void start();
virtual void stop();
typedef hardware_interface::PositionJointInterface parent_type;
static const std::string INTERFACE_NAME;
};
} // namespace ur_rtde_driver

59
include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h
View File

@@ -1,59 +0,0 @@
/*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* 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 <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <cstring>
#include <string>
#include <thread>
#include <vector>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ros/action_trajectory_follower_interface.h"
#include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/server.h"
namespace ur_rtde_driver
{
class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;
std::array<double, 6> last_positions_;
URCommander& commander_;
URServer server_;
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_,
max_joint_difference_;
std::string program_;
bool execute(const std::array<double, 6>& positions, const std::array<double, 6>& velocities, double sample_number,
double time_in_seconds);
public:
LowBandwidthTrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, bool version_3);
bool start();
bool execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt);
void stop();
virtual ~LowBandwidthTrajectoryFollower(){};
};
} // namespace ur_rtde_driver

82
include/ur_rtde_driver/ros/mb_publisher.h
View File

@@ -1,82 +0,0 @@
/*
* 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 <industrial_msgs/RobotStatus.h>
#include <ros/ros.h>
#include <ur_msgs/Analog.h>
#include <ur_msgs/Digital.h>
#include <ur_msgs/IOStates.h>
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
using namespace ros;
class MBPublisher : public URStatePacketConsumer
{
private:
NodeHandle nh_;
Publisher io_pub_;
Publisher status_pub_;
template <size_t N>
inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
{
for (size_t i = 0; i < N; i++)
{
ur_msgs::Digital digi;
digi.pin = static_cast<uint8_t>(i);
digi.state = bits.test(i);
vec.push_back(digi);
}
}
void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const;
void publishRobotStatus(const RobotModeData_V1_X& data) const;
void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
public:
MBPublisher()
: io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
, status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1))
{
}
virtual bool consume(MasterBoardData_V1_X& data);
virtual bool consume(MasterBoardData_V3_0__1& data);
virtual bool consume(MasterBoardData_V3_2& data);
virtual bool consume(RobotModeData_V1_X& data);
virtual bool consume(RobotModeData_V3_0__1& data);
virtual bool consume(RobotModeData_V3_2& data);
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
};
} // namespace ur_rtde_driver

95
include/ur_rtde_driver/ros/rt_publisher.h
View File

@@ -1,95 +0,0 @@
/*
* 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 <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Temperature.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <cstdlib>
#include <vector>
#include "ur_rtde_driver/ur/consumer.h"
namespace ur_rtde_driver
{
using namespace ros;
using namespace tf;
const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
class RTPublisher : public URRTPacketConsumer
{
private:
NodeHandle nh_;
Publisher joint_pub_;
Publisher wrench_pub_;
Publisher tool_vel_pub_;
Publisher joint_temperature_pub_;
TransformBroadcaster transform_broadcaster_;
std::vector<std::string> joint_names_;
std::string base_frame_;
std::string tool_frame_;
bool temp_only_;
bool publishJoints(RTShared& packet, Time& t);
bool publishWrench(RTShared& packet, Time& t);
bool publishTool(RTShared& packet, Time& t);
bool publishTransform(RTShared& packet, Time& t);
bool publishTemperature(RTShared& packet, Time& t);
bool publish(RTShared& packet);
public:
RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false)
: joint_pub_(nh_.advertise<sensor_msgs::JointState>("joint_states", 1))
, wrench_pub_(nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1))
, tool_vel_pub_(nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1))
, joint_temperature_pub_(nh_.advertise<sensor_msgs::Temperature>("joint_temperature", 1))
, base_frame_(base_frame)
, tool_frame_(tool_frame)
, temp_only_(temp_only)
{
for (auto const& j : JOINTS)
{
joint_names_.push_back(joint_prefix + j);
}
}
virtual bool consume(RTState_V1_6__7& state);
virtual bool consume(RTState_V1_8& state);
virtual bool consume(RTState_V3_0__1& state);
virtual bool consume(RTState_V3_2__3& state);
virtual void setupConsumer()
{
}
virtual void teardownConsumer()
{
}
virtual void stopConsumer()
{
}
};
} // namespace ur_rtde_driver

70
include/ur_rtde_driver/ros/trajectory_follower.h
View File

@@ -1,70 +0,0 @@
/*
* Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower)
*
* 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 <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <cstring>
#include <string>
#include <thread>
#include <vector>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ros/action_trajectory_follower_interface.h"
#include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/server.h"
namespace ur_rtde_driver
{
class TrajectoryFollower : public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;
std::array<double, 6> last_positions_;
URCommander& commander_;
URServer server_;
double servoj_time_, servoj_lookahead_time_, servoj_gain_;
std::string program_;
template <typename T>
size_t append(uint8_t* buffer, T& val)
{
size_t s = sizeof(T);
std::memcpy(buffer, &val, s);
return s;
}
bool execute(std::array<double, 6>& positions, bool keep_alive);
double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
public:
TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, bool version_3);
bool start();
bool execute(std::array<double, 6>& positions);
bool execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt);
void stop();
virtual ~TrajectoryFollower(){};
};
} // namespace ur_rtde_driver

44
include/ur_rtde_driver/ros/urscript_handler.h
View File

@@ -1,44 +0,0 @@
/*
* 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 <ros/ros.h>
#include <string>
#include "std_msgs/String.h"
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/ros/service_stopper.h"
#include "ur_rtde_driver/ur/commander.h"
namespace ur_rtde_driver
{
class URScriptHandler : public Service
{
private:
ros::NodeHandle nh_;
URCommander& commander_;
ros::Subscriber urscript_sub_;
RobotState state_;
public:
URScriptHandler(URCommander& commander);
void urscriptInterface(const std_msgs::String::ConstPtr& msg);
void onRobotStateChange(RobotState state);
};
} // namespace ur_rtde_driver