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

Action server improvements

Browse Source
This commit is contained in:
Simon Rasmussen
2017-05-19 01:20:30 +02:00
parent c59bfc78cc
commit 8d845c6f38
2 changed files with 110 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
include/ur_modern_driver/ros/action_server.h
View File

@@ -1,40 +1,40 @@
#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 <ros/ros.h>
#include <actionlib/server/action_server.h>
#include <actionlib/server/server_goal_handle.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/master_board.h"
#include "ur_modern_driver/ur/state.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
class ActionServer : public Service //,public URRTPacketConsumer
class ActionServer : public Service, public IConsumer<RTPacket>
{
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_;
RobotState state_;
RobotState state_;
std::array<double, 6> q_actual_, qd_actual_;
GoalHandle curr_gh_;
std::atomic<bool> interrupt_traj_;
std::atomic<bool> has_goal_, running_;
@@ -48,7 +48,7 @@ private:
void onCancel(GoalHandle gh);
bool validate(GoalHandle& gh, Result& res);
bool validateState(GoalHandle& gh, Result& res);
bool validateState(GoalHandle& gh, Result& res);
bool validateJoints(GoalHandle& gh, Result& res);
bool validateTrajectory(GoalHandle& gh, Result& res);
@@ -58,11 +58,16 @@ private:
std::vector<size_t> reorderMap(std::vector<std::string> goal_joints);
void trajectoryThread();
bool updateState(RTShared& data);
public:
ActionServer(TrajectoryFollower& 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);
};