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

Prepared msg publisher for joint_offset

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-10 15:30:13 +02:00
parent 1b4ef808ee
commit e71848f1f7
2 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/ur_ros_wrapper.cpp
View File

@@ -47,11 +47,12 @@ protected:
std::thread* rt_publish_thread_;
double io_flag_delay_;
double max_velocity_;
std::vector<double> joint_offsets_;
public:
RosWrapper(std::string host) :
as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host), io_flag_delay_(
0.05) {
0.05), joint_offsets_(6, 0.0) {
std::string joint_prefix = "";
std::vector<std::string> joint_names;
@@ -91,6 +92,7 @@ public:
ROS_INFO("Bounds for set_payload service calls: [%f, %f]",
min_payload, max_payload);
}
robot_.start();
//register the goal and feedback callbacks
@@ -137,21 +139,21 @@ private:
"Received a goal with incorrect joint names: "
+ outp_joint_names;
as_.setAborted(result_, result_.error_string);
ROS_ERROR("%s",result_.error_string.c_str());
ROS_ERROR("%s", result_.error_string.c_str());
}
if (!has_velocities()) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities";
as_.setAborted(result_, result_.error_string);
ROS_ERROR("%s",result_.error_string.c_str());
ROS_ERROR("%s", result_.error_string.c_str());
}
if (!traj_is_finite()) {
result_.error_string = "Received a goal with infinites or NaNs";
result_.error_code = result_.INVALID_GOAL;
as_.setAborted(result_, result_.error_string);
ROS_ERROR("%s",result_.error_string.c_str());
ROS_ERROR("%s", result_.error_string.c_str());
}
if (!has_limited_velocities()) {
@@ -159,7 +161,7 @@ private:
result_.error_string =
"Received a goal with velocities that are higher than %f", max_velocity_;
as_.setAborted(result_, result_.error_string);
ROS_ERROR("%s",result_.error_string.c_str());
ROS_ERROR("%s", result_.error_string.c_str());
}
reorder_traj_joints(goal_.trajectory);
@@ -330,6 +332,9 @@ private:
joint_msg.header.stamp = ros::Time::now();
joint_msg.position =
robot_.rt_interface_->robot_state_->getQActual();
for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
joint_msg.position[i] += joint_offsets_[i];
}
joint_msg.velocity =
robot_.rt_interface_->robot_state_->getQdActual();
joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();