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

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

3
include/ur_modern_driver/ur_driver.h
View File

@@ -28,7 +28,8 @@ public:
UrRealtimeCommunication* rt_interface_; UrRealtimeCommunication* rt_interface_;
UrDriver(std::condition_variable& msg_cond, std::string host, UrDriver(std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload= 0., double max_payload=1.); unsigned int safety_count_max = 12, double max_time_step = 0.08,
double min_payload = 0., double max_payload = 1.);
void start(); void start();
void halt(); void halt();

15
src/ur_ros_wrapper.cpp
View File

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