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

Bugfix in calculating servoj commands

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-09 11:15:14 +02:00
parent 28e033f5b3
commit cb38c1f028
2 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/ur_driver.cpp
View File

@@ -55,18 +55,18 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
} }
} }
//make sure we come to a smooth stop //make sure we come to a smooth stop
/*while (timestamps.back() < inp_timestamps.back()) { while (timestamps.back() < inp_timestamps.back()) {
timestamps.push_back(timestamps.back() + 0.008); timestamps.push_back(timestamps.back() + 0.008);
} }
timestamps.pop_back(); timestamps.pop_back();
*/
unsigned int j = 0; unsigned int j = 0;
for (unsigned int i = 0; i < timestamps.size(); i++) { for (unsigned int i = 0; i < timestamps.size(); i++) {
while (inp_timestamps[j] <= timestamps[i]) { while (inp_timestamps[j] <= timestamps[i]) {
j += 1; j += 1;
} }
positions.push_back( positions.push_back(
UrDriver::interp_cubic(timestamps[i], inp_timestamps[j], UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j-1], inp_timestamps[j] - inp_timestamps[j-1],
inp_positions[j - 1], inp_positions[j], inp_positions[j - 1], inp_positions[j],
inp_velocities[j - 1], inp_velocities[j])); inp_velocities[j - 1], inp_velocities[j]));
} }

9
src/ur_ros_wrapper.cpp
View File

@@ -30,15 +30,14 @@ class URTrajectoryFollower {
protected: protected:
ros::NodeHandle nh_; ros::NodeHandle nh_;
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
std::string action_name_;
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal_; actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal_;
control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryFeedback feedback_;
control_msgs::FollowJointTrajectoryResult result_; control_msgs::FollowJointTrajectoryResult result_;
ros::Subscriber sub_; ros::Subscriber sub_;
UrDriver* robot_; UrDriver* robot_;
public: public:
URTrajectoryFollower(UrDriver* robot, std::string name) : URTrajectoryFollower(UrDriver* robot) :
as_(nh_, "follow_joint_trajectory", false), action_name_(name) { as_(nh_, "follow_joint_trajectory", false) {
robot_ = robot; robot_ = robot;
//register the goal and feeback callbacks //register the goal and feeback callbacks
as_.registerGoalCallback( as_.registerGoalCallback(
@@ -88,7 +87,7 @@ public:
} }
void preemptCB() { void preemptCB() {
ROS_INFO("%s: Preempted", action_name_.c_str()); ROS_INFO("on_cancel");
// set the action state to preempted // set the action state to preempted
robot_->stopTraj(); robot_->stopTraj();
as_.setPreempted(); as_.setPreempted();
@@ -216,7 +215,7 @@ int main(int argc, char **argv) {
robot.start(); robot.start();
std::thread rt_publish_thread(publishRTMsg, robot); std::thread rt_publish_thread(publishRTMsg, robot);
URTrajectoryFollower action_server(&robot, ros::this_node::getName()); URTrajectoryFollower action_server(&robot);
ros::spin(); ros::spin();
robot.halt(); robot.halt();