Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-09 17:40:47 +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

40
CMakeLists.txt
View File

@@ -29,7 +29,7 @@ catkin_package(
INCLUDE_DIRS
include
LIBRARIES
ur_rtde_hardware_interface
ur_rtde_driver
CATKIN_DEPENDS
actionlib
control_msgs
@@ -60,33 +60,16 @@ add_compile_options(-Wall)
add_compile_options(-Wextra)
add_compile_options(-Wno-unused-parameter)
# support indigo's ros_control - This can be removed upon EOL indigo
if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0")
add_definitions(-DUR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
endif()
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# Hardware Interface
add_library(ur_rtde_hardware_interface
src/ros/controller.cpp
src/ros/hardware_interface.cpp)
target_link_libraries(ur_rtde_hardware_interface ${catkin_LIBRARIES})
set(${PROJECT_NAME}_SOURCES
add_library(ur_rtde_driver
src/comm/stream.cpp
src/comm/tcp_socket.cpp
src/ros/action_server.cpp
src/ros/lowbandwidth_trajectory_follower.cpp
src/ros/mb_publisher.cpp
src/ros/rt_publisher.cpp
src/ros/service_stopper.cpp
src/ros/trajectory_follower.cpp
src/ros/urscript_handler.cpp
src/ur/commander.cpp
src/ur/master_board.cpp
src/ur/messages.cpp
@@ -94,24 +77,19 @@ set(${PROJECT_NAME}_SOURCES
src/ur/rt_state.cpp
src/ur/server.cpp
)
add_executable(ur_rtde_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp)
target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(ur_rtde_driver
ur_rtde_hardware_interface
${catkin_LIBRARIES}
)
install(DIRECTORY config launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(TARGETS ur_rtde_driver ur_rtde_hardware_interface
install(TARGETS ur_rtde_driver
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY config launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
@@ -125,5 +103,5 @@ if (CATKIN_ENABLE_TESTING)
tests/ur/rt_state.cpp)
catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp)
target_link_libraries(ur_rtde_driver_test ur_rtde_hardware_interface ${catkin_LIBRARIES})
target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES})
endif()

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

372
src/ros/action_server.cpp
View File

@@ -1,372 +0,0 @@
/*
* Copyright 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.
*/
#include "ur_rtde_driver/ros/action_server.h"
#include <cmath>
namespace ur_rtde_driver
{
ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names,
double max_velocity)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names)
, joint_set_(joint_names.begin(), joint_names.end())
, max_velocity_(max_velocity)
, interrupt_traj_(false)
, has_goal_(false)
, running_(false)
, follower_(follower)
, state_(RobotState::Error)
{
}
void ActionServer::start()
{
if (running_)
return;
LOG_INFO("Starting ActionServer");
running_ = true;
tj_thread_ = std::thread(&ActionServer::trajectoryThread, this);
as_.start();
}
void ActionServer::onRobotStateChange(RobotState state)
{
state_ = state;
// don't interrupt if everything is fine
if (state == RobotState::Running)
return;
// don't retry interrupts
if (interrupt_traj_ || !has_goal_)
return;
// on successful lock we're not executing a goal so don't interrupt
if (tj_mutex_.try_lock())
{
tj_mutex_.unlock();
return;
}
interrupt_traj_ = true;
// wait for goal to be interrupted and automagically unlock when going out of scope
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
res.error_code = -100;
res.error_string = "Robot safety stop";
curr_gh_.setAborted(res, res.error_string);
}
bool ActionServer::updateState(RTShared& data)
{
q_actual_ = data.q_actual;
qd_actual_ = data.qd_actual;
return true;
}
bool ActionServer::consume(RTState_V1_6__7& state)
{
return updateState(state);
}
bool ActionServer::consume(RTState_V1_8& state)
{
return updateState(state);
}
bool ActionServer::consume(RTState_V3_0__1& state)
{
return updateState(state);
}
bool ActionServer::consume(RTState_V3_2__3& state)
{
return updateState(state);
}
void ActionServer::onGoal(GoalHandle gh)
{
Result res;
res.error_code = -100;
LOG_INFO("Received new goal");
if (!validate(gh, res) || !try_execute(gh, res))
{
LOG_WARN("Goal error: %s", res.error_string.c_str());
gh.setRejected(res, res.error_string);
}
}
void ActionServer::onCancel(GoalHandle gh)
{
interrupt_traj_ = true;
// wait for goal to be interrupted
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
res.error_code = -100;
res.error_string = "Goal cancelled by client";
gh.setCanceled(res);
}
bool ActionServer::validate(GoalHandle& gh, Result& res)
{
return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res);
}
bool ActionServer::validateState(GoalHandle& gh, Result& res)
{
switch (state_)
{
case RobotState::EmergencyStopped:
res.error_string = "Robot is emergency stopped";
return false;
case RobotState::ProtectiveStopped:
res.error_string = "Robot is protective stopped";
return false;
case RobotState::Error:
res.error_string = "Robot is not ready, check robot_mode";
return false;
case RobotState::Running:
return true;
default:
res.error_string = "Undefined state";
return false;
}
}
bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
{
auto goal = gh.getGoal();
auto const& joints = goal->trajectory.joint_names;
std::set<std::string> goal_joints(joints.begin(), joints.end());
if (goal_joints == joint_set_)
return true;
res.error_code = Result::INVALID_JOINTS;
res.error_string = "Invalid joint names for goal\n";
res.error_string += "Expected: ";
std::for_each(goal_joints.begin(), goal_joints.end(),
[&res](std::string joint) { res.error_string += joint + ", "; });
res.error_string += "\nFound: ";
std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint) { res.error_string += joint + ", "; });
return false;
}
bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
{
auto goal = gh.getGoal();
res.error_code = Result::INVALID_GOAL;
// must at least have one point
if (goal->trajectory.points.size() < 1)
return false;
for (auto const& point : goal->trajectory.points)
{
if (point.velocities.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of velocities";
return false;
}
if (point.positions.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of positions";
return false;
}
for (auto const& velocity : point.velocities)
{
if (!std::isfinite(velocity))
{
res.error_string = "Received a goal with infinities or NaNs in velocity";
return false;
}
if (std::fabs(velocity) > max_velocity_)
{
res.error_string =
"Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_);
return false;
}
}
for (auto const& position : point.positions)
{
if (!std::isfinite(position))
{
res.error_string = "Received a goal with infinities or NaNs in positions";
return false;
}
}
}
// todo validate start position?
return true;
}
inline std::chrono::microseconds convert(const ros::Duration& dur)
{
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
std::chrono::nanoseconds(dur.nsec));
}
bool ActionServer::try_execute(GoalHandle& gh, Result& res)
{
if (!running_)
{
res.error_string = "Internal error";
return false;
}
if (!tj_mutex_.try_lock())
{
interrupt_traj_ = true;
res.error_string = "Received another trajectory";
curr_gh_.setAborted(res, res.error_string);
tj_mutex_.lock();
// todo: make configurable
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
// locked here
curr_gh_ = gh;
interrupt_traj_ = false;
has_goal_ = true;
tj_mutex_.unlock();
tj_cv_.notify_one();
return true;
}
std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
{
std::vector<size_t> indecies;
for (auto const& aj : joint_names_)
{
size_t j = 0;
for (auto const& gj : goal_joints)
{
if (aj == gj)
break;
j++;
}
indecies.push_back(j);
}
return indecies;
}
void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory thread started");
while (running_)
{
std::unique_lock<std::mutex> lk(tj_mutex_);
if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
continue;
LOG_INFO("Trajectory received and accepted");
curr_gh_.setAccepted();
auto goal = curr_gh_.getGoal();
std::vector<TrajectoryPoint> trajectory;
trajectory.reserve(goal->trajectory.points.size() + 1);
// joint names of the goal might have a different ordering compared
// to what URScript expects so need to map between the two
auto mapping = reorderMap(goal->trajectory.joint_names);
LOG_INFO("Translating trajectory");
auto const& fp = goal->trajectory.points[0];
auto fpt = convert(fp.time_from_start);
// make sure we have a proper t0 position
if (fpt > std::chrono::microseconds(0))
{
LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position");
trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
}
for (auto const& point : goal->trajectory.points)
{
std::array<double, 6> pos, vel;
for (size_t i = 0; i < 6; i++)
{
size_t idx = mapping[i];
pos[idx] = point.positions[i];
vel[idx] = point.velocities[i];
}
auto t = convert(point.time_from_start);
trajectory.push_back(TrajectoryPoint(pos, vel, t));
}
double t =
std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
.count();
LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
Result res;
LOG_INFO("Attempting to start follower %p", &follower_);
if (follower_.start())
{
if (follower_.execute(trajectory, interrupt_traj_))
{
// interrupted goals must be handled by interrupt trigger
if (!interrupt_traj_)
{
LOG_INFO("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
curr_gh_.setSucceeded(res);
}
else
LOG_INFO("Trajectory interrupted");
}
else
{
LOG_INFO("Trajectory failed");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
curr_gh_.setAborted(res, res.error_string);
}
follower_.stop();
}
else
{
LOG_ERROR("Failed to start trajectory follower!");
res.error_code = -100;
res.error_string = "Robot connection could not be established";
curr_gh_.setAborted(res, res.error_string);
}
has_goal_ = false;
lk.unlock();
}
}
} // namespace ur_rtde_driver

152
src/ros/controller.cpp
View File

@@ -1,152 +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.
*/
#include "ur_rtde_driver/ros/controller.h"
namespace ur_rtde_driver
{
ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
std::vector<std::string>& joint_names, double max_vel_change, std::string tcp_link)
: controller_(this, nh_)
, robot_state_received_(false)
, joint_interface_(joint_names)
, wrench_interface_(tcp_link)
, position_interface_(follower, joint_interface_, joint_names)
, velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
{
registerInterface(&joint_interface_);
registerInterface(&wrench_interface_);
registerControllereInterface(&position_interface_);
registerControllereInterface(&velocity_interface_);
}
void ROSController::setupConsumer()
{
lastUpdate_ = ros::Time::now();
}
void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
LOG_INFO("Switching hardware interface");
if (active_interface_ != nullptr && stop_list.size() > 0)
{
LOG_INFO("Stopping active interface");
active_interface_->stop();
active_interface_ = nullptr;
}
for (auto const& ci : start_list)
{
std::string requested_interface("");
#if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
requested_interface = ci.hardware_interface;
#else
if (!ci.claimed_resources.empty())
requested_interface = ci.claimed_resources[0].hardware_interface;
#endif
auto ait = available_interfaces_.find(requested_interface);
if (ait == available_interfaces_.end())
continue;
auto new_interface = ait->second;
LOG_INFO("Starting %s", ci.name.c_str());
active_interface_ = new_interface;
new_interface->start();
return;
}
if (start_list.size() > 0)
LOG_WARN("Failed to start interface!");
}
bool ROSController::write()
{
if (active_interface_ == nullptr)
return true;
return active_interface_->write();
}
void ROSController::reset()
{
if (active_interface_ == nullptr)
return;
active_interface_->reset();
}
void ROSController::read(RTShared& packet)
{
joint_interface_.update(packet);
wrench_interface_.update(packet);
robot_state_received_ = true;
}
bool ROSController::update()
{
// don't run controllers if we haven't received robot state yet
if (!robot_state_received_)
return true;
auto time = ros::Time::now();
auto diff = time - lastUpdate_;
lastUpdate_ = time;
controller_.update(time, diff, !service_enabled_);
// emergency stop and such should not kill the pipeline
// but still prevent writes
if (!service_enabled_)
{
reset();
return true;
}
// allow the controller to update x times before allowing writes again
if (service_cooldown_ > 0)
{
service_cooldown_ -= 1;
return true;
}
return write();
}
void ROSController::onTimeout()
{
update();
}
void ROSController::onRobotStateChange(RobotState state)
{
bool next = (state == RobotState::Running);
if (next == service_enabled_)
return;
service_enabled_ = next;
service_cooldown_ = 125;
}
} // namespace ur_rtde_driver

109
src/ros/hardware_interface.cpp
View File

@@ -1,109 +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.
*/
#include "ur_rtde_driver/ros/hardware_interface.h"
#include "ur_rtde_driver/log.h"
namespace ur_rtde_driver
{
const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface";
JointInterface::JointInterface(std::vector<std::string>& joint_names)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i]));
}
}
void JointInterface::update(RTShared& packet)
{
positions_ = packet.q_actual;
velocities_ = packet.qd_actual;
efforts_ = packet.i_actual;
}
const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface";
WrenchInterface::WrenchInterface(std::string tcp_link)
{
registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3));
}
void WrenchInterface::update(RTShared& packet)
{
tcp_ = packet.tcp_force;
}
const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface";
VelocityInterface::VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface,
std::vector<std::string>& joint_names, double max_vel_change)
: commander_(commander), max_vel_change_(max_vel_change), prev_velocity_cmd_({ 0, 0, 0, 0, 0, 0 })
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i]));
}
}
bool VelocityInterface::write()
{
for (size_t i = 0; i < 6; i++)
{
// clamp value to ±max_vel_change
double prev = prev_velocity_cmd_[i];
double lo = prev - max_vel_change_;
double hi = prev + max_vel_change_;
prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi));
}
return commander_.speedj(prev_velocity_cmd_, max_vel_change_);
}
void VelocityInterface::reset()
{
for (auto& val : prev_velocity_cmd_)
{
val = 0;
}
}
const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface";
PositionInterface::PositionInterface(TrajectoryFollower& follower,
hardware_interface::JointStateInterface& js_interface,
std::vector<std::string>& joint_names)
: follower_(follower)
{
for (size_t i = 0; i < 6; i++)
{
registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i]));
}
}
bool PositionInterface::write()
{
return follower_.execute(position_cmd_);
}
void PositionInterface::start()
{
follower_.start();
}
void PositionInterface::stop()
{
follower_.stop();
}
} // namespace ur_rtde_driver

407
src/ros/lowbandwidth_trajectory_follower.cpp
View File

@@ -1,407 +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.
*/
#include "ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h"
#include <endian.h>
#include <ros/ros.h>
#include <cmath>
namespace ur_rtde_driver
{
static const std::array<double, 6> EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}");
static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}");
static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}");
static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}");
static const std::string REVERSE_IP("{{REVERSE_IP}}");
static const std::string REVERSE_PORT("{{REVERSE_PORT}}");
static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}");
static const std::string POSITION_PROGRAM = R"(
def driveRobotLowBandwidthTrajectory():
global JOINT_NUM = 6
global TIME_INTERVAL = {{TIME_INTERVAL}}
global SERVOJ_TIME = {{SERVOJ_TIME}}
global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}}
global MAX_WAITING_TIME = {{MAX_WAITING_TIME}}
global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global SERVOJ_GAIN = {{SERVOJ_GAIN}}
global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}}
global CONNECTION_NAME = "reverse_connection"
global REVERSE_IP = "{{REVERSE_IP}}"
global REVERSE_PORT = {{REVERSE_PORT}}
global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}}
global g_position_previous = EMPTY_VALUES
global g_position_target = EMPTY_VALUES
global g_position_next = EMPTY_VALUES
global g_velocity_previous = EMPTY_VALUES
global g_velocity_target = EMPTY_VALUES
global g_velocity_next = EMPTY_VALUES
global g_time_previous = 0.0
global g_time_target = 0.0
global g_time_next = 0.0
global g_num_previous = -1
global g_num_target = -1
global g_num_next = -1
global g_received_waypoints_number = -1
global g_requested_waypoints_number = -1
global g_total_elapsed_time = 0
global g_stopping = False
def send_message(message):
socket_send_string(message, CONNECTION_NAME)
socket_send_byte(10, CONNECTION_NAME)
end
def is_waypoint_sentinel(waypoint):
local l_previous_index = 2
while l_previous_index < 1 + JOINT_NUM * 2 + 2:
if waypoint[l_previous_index] != 0.0:
return False
end
l_previous_index = l_previous_index + 1
end
return True
end
def is_final_position_reached(position):
local l_current_position = get_actual_joint_positions()
local l_index = 0
while l_index < JOINT_NUM:
if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE:
return False
end
l_index = l_index + 1
end
return True
end
def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel):
local a = start_pos
local b = l_start_vel
local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2)
local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3)
return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3)
end
def add_next_waypoint(waypoint):
enter_critical
g_position_previous = g_position_target
g_velocity_previous = g_velocity_target
g_time_previous = g_time_target
g_num_previous = g_num_target
g_position_target = g_position_next
g_velocity_target = g_velocity_next
g_time_target = g_time_next
g_num_target = g_num_next
g_num_next = waypoint[1]
g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]]
g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]]
g_time_next = waypoint[14]
g_received_waypoints_number = g_num_next
exit_critical
end
thread controllingThread():
local l_received_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopped = False
local l_current_position = get_actual_joint_positions()
enter_critical
g_requested_waypoints_number = 2
exit_critical
while True:
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
exit_critical
local l_max_waiting_time_left = MAX_WAITING_TIME
while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0:
servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
l_received_waypoints_number = g_received_waypoints_number
exit_critical
l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING
end
if l_max_waiting_time_left <= 0:
textmsg("Closing the connection on waiting too long.")
socket_close(CONNECTION_NAME)
halt
end
enter_critical
local l_start_pos = g_position_previous
local l_start_vel = g_velocity_previous
local l_start_time = g_time_previous
local l_start_num= g_num_previous
local l_end_pos = g_position_target
local l_end_vel = g_velocity_target
local l_end_time = g_time_target
local l_end_num = g_num_target
local l_total_elapsed_time = g_total_elapsed_time
local l_stopping_after_next_interpolation = g_stopping
g_requested_waypoints_number = g_requested_waypoints_number + 1
exit_critical
l_current_position = l_start_pos
local l_total_segment_time = l_end_time - l_start_time
while l_total_elapsed_time <= l_end_time:
local l_segment_elapsed_time = l_total_elapsed_time - l_start_time
j = 0
while j < JOINT_NUM:
l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j])
j = j + 1
end
servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL
l_total_elapsed_time = g_total_elapsed_time
exit_critical
end
if l_stopping_after_next_interpolation:
while not is_final_position_reached(l_end_pos):
textmsg("Catching up on final position not reached first time.")
servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
end
textmsg("Finishing the controlling thread. Final position reached.")
break
end
end
end
thread sendingThread():
local controlling_thread = run controllingThread()
local l_sent_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopping = False
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_stopping = g_stopping
exit_critical
while not l_stopping:
while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping:
sleep(SERVOJ_TIME_WAITING)
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_stopping = g_stopping
exit_critical
end
if l_stopping:
break
end
send_message(l_sent_waypoints_number + 1)
l_sent_waypoints_number = l_sent_waypoints_number + 1
end
join controlling_thread
end
thread receivingThread():
local sending_thread = run sendingThread()
while True:
waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME)
if waypoint_received[0] == 0:
textmsg("Not received trajectory for the last 2 seconds. Quitting")
enter_critical
g_stopping = True
exit_critical
break
elif waypoint_received[0] != JOINT_NUM * 2 + 2:
textmsg("Received wrong number of floats in trajectory. This is certainly not OK.")
textmsg(waypoint_received[0])
enter_critical
g_stopping = True
exit_critical
break
elif is_waypoint_sentinel(waypoint_received):
add_next_waypoint(waypoint_received)
enter_critical
g_stopping = True
g_received_waypoints_number = g_received_waypoints_number + 1
exit_critical
break
end
add_next_waypoint(waypoint_received)
end
join sending_thread
end
socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME)
receiving_thread = run receivingThread()
join receiving_thread
socket_close(CONNECTION_NAME)
textmsg("Exiting the program")
end
)";
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander& commander, std::string& reverse_ip,
int reverse_port, bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, time_interval_(0.008)
, servoj_time_(0.008)
, servoj_time_waiting_(0.001)
, max_waiting_time_(2.0)
, servoj_gain_(300.0)
, servoj_lookahead_time_(0.03)
, max_joint_difference_(0.01)
{
ros::param::get("~time_interval", time_interval_);
ros::param::get("~servoj_time", servoj_time_);
ros::param::get("~servoj_time_waiting", servoj_time_waiting_);
ros::param::get("~max_waiting_time", max_waiting_time_);
ros::param::get("~servoj_gain", servoj_gain_);
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~max_joint_difference", max_joint_difference_);
std::string res(POSITION_PROGRAM);
std::ostringstream out;
if (!version_3)
{
LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
std::exit(-1);
}
res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_));
res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_));
res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_));
res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_));
res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_));
res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_));
res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip);
res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port));
res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_));
program_ = res;
if (!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
LOG_INFO("Low Bandwidth Trajectory Follower is initialized!");
}
bool LowBandwidthTrajectoryFollower::start()
{
LOG_INFO("Starting LowBandwidthTrajectoryFollower");
if (running_)
return true; // not sure
LOG_INFO("Uploading trajectory program to robot");
if (!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_DEBUG("Awaiting incoming robot connection");
if (!server_.accept())
{
LOG_ERROR("Failed to accept incoming robot connection");
return false;
}
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6>& positions,
const std::array<double, 6>& velocities, double sample_number,
double time_in_seconds)
{
if (!running_)
return false;
std::ostringstream out;
out << "(";
out << sample_number << ",";
for (auto const& pos : positions)
{
out << pos << ",";
}
for (auto const& vel : velocities)
{
out << vel << ",";
}
out << time_in_seconds << ")\r\n";
// I know it's ugly but it's the most efficient and fastest way
// We have only ASCII characters and we can cast char -> uint_8
const std::string tmp = out.str();
const char* formatted_message = tmp.c_str();
const uint8_t* buf = (uint8_t*)formatted_message;
size_t written;
LOG_DEBUG("Sending message %s", formatted_message);
return server_.write(buf, strlen(formatted_message) + 1, written);
}
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt)
{
if (!running_)
return false;
bool finished = false;
char* line[MAX_SERVER_BUF_LEN];
bool res = true;
while (!finished && !interrupt)
{
if (!server_.readLine((char*)line, MAX_SERVER_BUF_LEN))
{
LOG_DEBUG("Connection closed. Finishing!");
finished = true;
break;
}
unsigned int message_num = atoi((const char*)line);
LOG_DEBUG("Received request %i", message_num);
if (message_num < trajectory.size())
{
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num,
trajectory[message_num].time_from_start.count() / 1e6);
}
else
{
res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
}
if (!res)
{
finished = true;
}
}
return res;
}
void LowBandwidthTrajectoryFollower::stop()
{
if (!running_)
return;
server_.disconnectClient();
running_ = false;
}
} // namespace ur_rtde_driver

135
src/ros/mb_publisher.cpp
View File

@@ -1,135 +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.
*/
#include "ur_rtde_driver/ros/mb_publisher.h"
namespace ur_rtde_driver
{
inline void appendAnalog(std::vector<ur_msgs::Analog>& vec, double val, uint8_t pin)
{
ur_msgs::Analog ana;
ana.pin = pin;
ana.state = val;
vec.push_back(ana);
}
void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data)
{
appendAnalog(io_msg.analog_in_states, data.analog_input0, 0);
appendAnalog(io_msg.analog_in_states, data.analog_input1, 1);
appendAnalog(io_msg.analog_out_states, data.analog_output0, 0);
appendAnalog(io_msg.analog_out_states, data.analog_output1, 1);
io_pub_.publish(io_msg);
}
void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const
{
// note that this is true as soon as the drives are powered,
// even if the breakes are still closed
// which is in slight contrast to the comments in the
// message definition
status.drives_powered.val = data.robot_power_on;
status.e_stopped.val = data.emergency_stopped;
// I found no way to reliably get information if the robot is moving
// data.programm_running would be true when using this driver to move the robot
// but it would also be true when another programm is running that might not
// in fact contain any movement commands
status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
// the error code, if any, is not transmitted by this protocol
// it can and should be fetched seperately
status.error_code = 0;
// note that e-stop is handled by a seperate variable
status.in_error.val = data.protective_stopped;
status_pub_.publish(status);
}
void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
{
industrial_msgs::RobotStatus msg;
if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE)
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
else
msg.mode.val = industrial_msgs::RobotMode::AUTO;
// todo: verify that this correct, there is also ROBOT_READY_MODE
msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) ? industrial_msgs::TriState::ON :
industrial_msgs::TriState::OFF;
publishRobotStatus(msg, data);
}
void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
{
industrial_msgs::RobotStatus msg;
msg.motion_possible.val =
(data.robot_mode == robot_mode_V3_X::RUNNING) ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
if (data.control_mode == robot_control_mode_V3_X::TEACH)
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
else
msg.mode.val = industrial_msgs::RobotMode::AUTO;
publishRobotStatus(msg, data);
}
bool MBPublisher::consume(MasterBoardData_V1_X& data)
{
ur_msgs::IOStates io_msg;
appendDigital(io_msg.digital_in_states, data.digital_input_bits);
appendDigital(io_msg.digital_out_states, data.digital_output_bits);
publish(io_msg, data);
return true;
}
bool MBPublisher::consume(MasterBoardData_V3_0__1& data)
{
ur_msgs::IOStates io_msg;
appendDigital(io_msg.digital_in_states, data.digital_input_bits);
appendDigital(io_msg.digital_out_states, data.digital_output_bits);
publish(io_msg, data);
return true;
}
bool MBPublisher::consume(MasterBoardData_V3_2& data)
{
consume(static_cast<MasterBoardData_V3_0__1&>(data));
return true;
}
bool MBPublisher::consume(RobotModeData_V1_X& data)
{
publishRobotStatus(data);
return true;
}
bool MBPublisher::consume(RobotModeData_V3_0__1& data)
{
publishRobotStatus(data);
return true;
}
bool MBPublisher::consume(RobotModeData_V3_2& data)
{
publishRobotStatus(data);
return true;
}
} // namespace ur_rtde_driver

139
src/ros/rt_publisher.cpp
View File

@@ -1,139 +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.
*/
#include "ur_rtde_driver/ros/rt_publisher.h"
namespace ur_rtde_driver
{
bool RTPublisher::publishJoints(RTShared& packet, Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name.assign(joint_names_.begin(), joint_names_.end());
joint_msg.position.assign(packet.q_actual.begin(), packet.q_actual.end());
joint_msg.velocity.assign(packet.qd_actual.begin(), packet.qd_actual.end());
joint_msg.effort.assign(packet.i_actual.begin(), packet.i_actual.end());
joint_pub_.publish(joint_msg);
return true;
}
bool RTPublisher::publishWrench(RTShared& packet, Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
wrench_msg.wrench.force.x = packet.tcp_force[0];
wrench_msg.wrench.force.y = packet.tcp_force[1];
wrench_msg.wrench.force.z = packet.tcp_force[2];
wrench_msg.wrench.torque.x = packet.tcp_force[3];
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
wrench_pub_.publish(wrench_msg);
return true;
}
bool RTPublisher::publishTool(RTShared& packet, Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
tool_twist.header.frame_id = base_frame_;
tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
tool_vel_pub_.publish(tool_twist);
return true;
}
bool RTPublisher::publishTransform(RTShared& packet, Time& t)
{
auto tv = packet.tool_vector_actual;
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
// Create quaternion
Quaternion quat;
double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
if (angle < 1e-16)
{
quat.setValue(0, 0, 0, 1);
}
else
{
quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
}
transform.setRotation(quat);
transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
return true;
}
bool RTPublisher::publishTemperature(RTShared& packet, Time& t)
{
size_t len = joint_names_.size();
for (size_t i = 0; i < len; i++)
{
sensor_msgs::Temperature msg;
msg.header.stamp = t;
msg.header.frame_id = joint_names_[i];
msg.temperature = packet.motor_temperatures[i];
joint_temperature_pub_.publish(msg);
}
return true;
}
bool RTPublisher::publish(RTShared& packet)
{
Time time = Time::now();
bool res = true;
if (!temp_only_)
{
res = publishJoints(packet, time) && publishWrench(packet, time);
}
return res && publishTool(packet, time) && publishTransform(packet, time) && publishTemperature(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state);
}
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
}
} // namespace ur_rtde_driver

264
src/ros/trajectory_follower.cpp
View File

@@ -1,264 +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.
*/
#include "ur_rtde_driver/ros/trajectory_follower.h"
#include <endian.h>
#include <ros/ros.h>
#include <cmath>
static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
static const std::string POSITION_PROGRAM = R"(
def driverProg():
MULT_jointstate = {{JOINT_STATE_REPLACE}}
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_q = q
exit_critical
end
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
q = cmd_servo_q
do_brake = False
if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
do_brake = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
if do_brake:
stopj(1.0)
sync()
elif state == SERVO_RUNNING:
servoj(q, {{SERVO_J_REPLACE}})
else:
sync()
end
end
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}})
thread_servo = run servoThread()
keepalive = 1
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1)
if params_mult[0] > 0:
q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
keepalive = params_mult[7]
set_servo_setpoint(q)
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
namespace ur_rtde_driver
{
TrajectoryFollower::TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port,
bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, servoj_time_(0.008)
, servoj_lookahead_time_(0.03)
, servoj_gain_(300.)
{
ros::param::get("~servoj_time", servoj_time_);
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~servoj_gain", servoj_gain_);
std::string res(POSITION_PROGRAM);
res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
if (version_3)
out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip);
res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
program_ = res;
if (!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
}
bool TrajectoryFollower::start()
{
if (running_)
return true; // not sure
LOG_INFO("Uploading trajectory program to robot");
if (!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_DEBUG("Awaiting incoming robot connection");
if (!server_.accept())
{
LOG_ERROR("Failed to accept incoming robot connection");
return false;
}
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
bool TrajectoryFollower::execute(std::array<double, 6>& positions, bool keep_alive)
{
if (!running_)
return false;
// LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4],
// positions[5]);
last_positions_ = positions;
uint8_t buf[sizeof(uint32_t) * 7];
uint8_t* idx = buf;
for (auto const& pos : positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE_);
val = htobe32(val);
idx += append(idx, val);
}
int32_t val = htobe32(static_cast<int32_t>(keep_alive));
append(idx, val);
size_t written;
return server_.write(buf, sizeof(buf), written);
}
double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel)
{
using std::pow;
double a = p0_pos;
double b = p0_vel;
double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2);
double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3);
return a + b * t + c * pow(t, 2) + d * pow(t, 3);
}
bool TrajectoryFollower::execute(std::array<double, 6>& positions)
{
return execute(positions, true);
}
bool TrajectoryFollower::execute(std::vector<TrajectoryPoint>& trajectory, std::atomic<bool>& interrupt)
{
if (!running_)
return false;
using namespace std::chrono;
typedef duration<double> double_seconds;
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto& last = trajectory[trajectory.size() - 1];
auto& prev = trajectory[0];
Time t0 = Clock::now();
Time latest = t0;
std::array<double, 6> positions;
for (auto const& point : trajectory)
{
// skip t0
if (&point == &prev)
continue;
if (interrupt)
break;
auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count();
// interpolation loop
while (!interrupt)
{
latest = Clock::now();
auto elapsed = latest - t0;
if (point.time_from_start <= elapsed)
break;
if (last.time_from_start <= elapsed)
return true;
double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
for (size_t j = 0; j < positions.size(); j++)
{
positions[j] =
interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]);
}
if (!execute(positions, true))
return false;
std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
}
prev = point;
}
// In theory it's possible the last position won't be sent by
// the interpolation loop above but rather some position between
// t[N-1] and t[N] where N is the number of trajectory points.
// To make sure this does not happen the last position is sent
return execute(last.positions, true);
}
void TrajectoryFollower::stop()
{
if (!running_)
return;
// std::array<double, 6> empty;
// execute(empty, false);
server_.disconnectClient();
running_ = false;
}
} // namespace ur_rtde_driver

65
src/ros/urscript_handler.cpp
View File

@@ -1,65 +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.
*/
#include "ur_rtde_driver/ros/urscript_handler.h"
#include "ur_rtde_driver/log.h"
namespace ur_rtde_driver
{
URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error)
{
LOG_INFO("Initializing ur_driver/URScript subscriber");
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
LOG_INFO("The ur_driver/URScript initialized");
}
void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg)
{
LOG_INFO("Message received");
std::string str(msg->data);
if (str.back() != '\n')
str.append("\n");
switch (state_)
{
case RobotState::Running:
if (!commander_.uploadProg(str))
{
LOG_ERROR("Program upload failed!");
}
break;
case RobotState::EmergencyStopped:
LOG_ERROR("Robot is emergency stopped");
break;
case RobotState::ProtectiveStopped:
LOG_ERROR("Robot is protective stopped");
break;
case RobotState::Error:
LOG_ERROR("Robot is not ready, check robot_mode");
break;
default:
LOG_ERROR("Robot is in undefined state");
break;
}
}
void URScriptHandler::onRobotStateChange(RobotState state)
{
state_ = state;
}
} // namespace ur_rtde_driver

250
src/ros_main.cpp
View File

@@ -1,250 +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.
*/
#include <ros/ros.h>
#include <chrono>
#include <cstdlib>
#include <string>
#include <thread>
#include "ur_rtde_driver/log.h"
#include "ur_rtde_driver/comm/parser.h"
#include "ur_rtde_driver/comm/pipeline.h"
#include "ur_rtde_driver/ros/action_server.h"
#include "ur_rtde_driver/ros/controller.h"
#include "ur_rtde_driver/ros/io_service.h"
#include "ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h"
#include "ur_rtde_driver/ros/mb_publisher.h"
#include "ur_rtde_driver/ros/rt_publisher.h"
#include "ur_rtde_driver/ros/service_stopper.h"
#include "ur_rtde_driver/ros/trajectory_follower.h"
#include "ur_rtde_driver/ros/urscript_handler.h"
#include "ur_rtde_driver/ur/commander.h"
#include "ur_rtde_driver/ur/factory.h"
#include "ur_rtde_driver/ur/messages.h"
#include "ur_rtde_driver/ur/producer.h"
#include "ur_rtde_driver/ur/rt_state.h"
#include "ur_rtde_driver/ur/state.h"
static const std::string IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port");
static const std::string ROS_CONTROL_ARG("~use_ros_control");
static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower");
static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
static const std::string TCP_LINK_ARG("~tcp_link");
static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
static const std::string SHUTDOWN_ON_DISCONNECT_ARG("~shutdown_on_disconnect");
static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
static const int UR_SECONDARY_PORT = 30002;
static const int UR_RT_PORT = 30003;
using namespace ur_rtde_driver;
struct ProgArgs
{
public:
std::string host;
std::string prefix;
std::string base_frame;
std::string tool_frame;
std::string tcp_link;
std::vector<std::string> joint_names;
double max_acceleration;
double max_velocity;
double max_vel_change;
int32_t reverse_port;
bool use_ros_control;
bool use_lowbandwidth_trajectory_follower;
bool shutdown_on_disconnect;
};
class IgnorePipelineStoppedNotifier : public comm::INotifier
{
public:
void started(std::string name)
{
LOG_INFO("Starting pipeline %s", name.c_str());
}
void stopped(std::string name)
{
LOG_INFO("Stopping pipeline %s", name.c_str());
}
};
class ShutdownOnPipelineStoppedNotifier : public comm::INotifier
{
public:
void started(std::string name)
{
LOG_INFO("Starting pipeline %s", name.c_str());
}
void stopped(std::string name)
{
LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
ros::shutdown();
exit(1);
}
};
bool parse_args(ProgArgs& args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host))
{
LOG_ERROR("robot_ip_address parameter must be set!");
return false;
}
ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0);
ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, false);
ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0");
ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS);
ros::param::param(SHUTDOWN_ON_DISCONNECT_ARG, args.shutdown_on_disconnect, true);
return true;
}
std::string getLocalIPAccessibleFromHost(std::string& host)
{
comm::URStream stream(host, UR_RT_PORT);
return stream.connect() ? stream.getIP() : std::string();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "ur_driver");
ProgArgs args;
if (!parse_args(args))
{
return EXIT_FAILURE;
}
// Add prefix to joint names
std::transform(args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
[&args](std::string name) { return args.prefix + name; });
std::string local_ip(getLocalIPAccessibleFromHost(args.host));
URFactory factory(args.host);
std::vector<Service*> services;
// RT packets
auto rt_parser = factory.getRTParser();
comm::URStream rt_stream(args.host, UR_RT_PORT);
URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
auto rt_commander = factory.getCommander(rt_stream);
std::vector<comm::IConsumer<RTPacket>*> rt_vec{ &rt_pub };
comm::INotifier* notifier(nullptr);
ROSController* controller(nullptr);
ActionServer* action_server(nullptr);
if (args.use_ros_control)
{
LOG_INFO("ROS control enabled");
TrajectoryFollower* traj_follower =
new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
rt_vec.push_back(controller);
services.push_back(controller);
}
else
{
LOG_INFO("ActionServer enabled");
ActionTrajectoryFollowerInterface* traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower)
{
LOG_INFO("Use low bandwidth trajectory follower");
traj_follower =
new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
}
else
{
LOG_INFO("Use standard trajectory follower");
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
}
action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
rt_vec.push_back(action_server);
services.push_back(action_server);
}
URScriptHandler urscript_handler(*rt_commander);
services.push_back(&urscript_handler);
if (args.shutdown_on_disconnect)
{
LOG_INFO("Notifier: Pipeline disconnect will shutdown the node");
notifier = new ShutdownOnPipelineStoppedNotifier();
}
else
{
LOG_INFO("Notifier: Pipeline disconnect will be ignored.");
notifier = new IgnorePipelineStoppedNotifier();
}
comm::MultiConsumer<RTPacket> rt_cons(rt_vec);
comm::Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
// Message packets
auto state_parser = factory.getStateParser();
comm::URStream state_stream(args.host, UR_SECONDARY_PORT);
URProducer<StatePacket> state_prod(state_stream, *state_parser);
MBPublisher state_pub;
ServiceStopper service_stopper(services);
std::vector<comm::IConsumer<StatePacket>*> state_vec{ &state_pub, &service_stopper };
comm::MultiConsumer<StatePacket> state_cons(state_vec);
comm::Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
LOG_INFO("Starting main loop");
rt_pl.run();
state_pl.run();
auto state_commander = factory.getCommander(state_stream);
IOService io_service(*state_commander);
if (action_server)
action_server->start();
ros::spin();
LOG_INFO("ROS stopping, shutting down pipelines");
rt_pl.stop();
state_pl.stop();
if (controller)
delete controller;
LOG_INFO("Pipelines shutdown complete");
return EXIT_SUCCESS;
}

168
test_move.py
View File

@@ -1,168 +0,0 @@
#!/usr/bin/env python
#
# 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.
import time
import roslib; roslib.load_manifest('ur_driver')
import rospy
import actionlib
from control_msgs.msg import *
from trajectory_msgs.msg import *
from sensor_msgs.msg import JointState
from math import pi
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
Q1 = [2.2,0,-1.57,0,0,0]
Q2 = [1.5,0,-1.57,0,0,0]
Q3 = [1.5,-0.2,-1.57,0,0,0]
client = None
def move1():
global joints_pos
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
g.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def move_disordered():
order = [4, 2, 3, 1, 5, 0]
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = [JOINT_NAMES[i] for i in order]
q1 = [Q1[i] for i in order]
q2 = [Q2[i] for i in order]
q3 = [Q3[i] for i in order]
try:
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
g.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def move_repeated():
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
d = 2.0
g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))]
for i in range(10):
g.trajectory.points.append(
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d)))
d += 1
g.trajectory.points.append(
JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d)))
d += 1
g.trajectory.points.append(
JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d)))
d += 2
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def move_interrupt():
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
g.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
time.sleep(3.0)
print "Interrupting"
joint_states = rospy.wait_for_message("joint_states", JointState)
joints_pos = joint_states.position
g.trajectory.points = [
JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def main():
global client
try:
rospy.init_node("test_move", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server..."
client.wait_for_server()
print "Connected to server"
parameters = rospy.get_param(None)
index = str(parameters).find('prefix')
if (index > 0):
prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))]
for i, name in enumerate(JOINT_NAMES):
JOINT_NAMES[i] = prefix + name
print "This program makes the robot move between the following three poses:"
print str([Q1[i]*180./pi for i in xrange(0,6)])
print str([Q2[i]*180./pi for i in xrange(0,6)])
print str([Q3[i]*180./pi for i in xrange(0,6)])
print "Please make sure that your robot can move freely between these poses before proceeding!"
inp = raw_input("Continue? y/n: ")[0]
if (inp == 'y'):
#move1()
move_repeated()
#move_disordered()
#move_interrupt()
else:
print "Halting program"
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
if __name__ == '__main__': main()