mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
removed unused files
This commit is contained in:
Felix Mauch
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user