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
/*while (timestamps.back() < inp_timestamps.back()) {
while (timestamps.back() < inp_timestamps.back()) {
timestamps.push_back(timestamps.back() + 0.008);
}
timestamps.pop_back();
*/
unsigned int j = 0;
for (unsigned int i = 0; i < timestamps.size(); i++) {
while (inp_timestamps[j] <= timestamps[i]) {
j += 1;
}
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_velocities[j - 1], inp_velocities[j]));
}

9
src/ur_ros_wrapper.cpp
View File

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