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

Implemented communication on port 30001 and 30002 - untested

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-15 17:03:39 +02:00
parent b5a687a54a
commit cb18dd1d96
7 changed files with 26 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/robot_state_RT.cpp
View File

@@ -85,6 +85,12 @@ std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
return ret;
}
void RobotStateRT::setVersion(double ver) {
val_lock_.lock();
version_ = ver;
val_lock_.unlock();
}
double RobotStateRT::getVersion() {
double ret;
val_lock_.lock();

6
src/ur_driver.cpp
View File

@@ -11,13 +11,14 @@
#include "ur_modern_driver/ur_driver.h"
UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
unsigned int safety_count_max, double max_time_step, double min_payload,
double max_payload) :
maximum_time_step_(max_time_step), minimum_payload_(min_payload), maximum_payload_(
max_payload) {
rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
safety_count_max);
sec_interface_ = new UrCommunication(msg_cond, host);
}
@@ -96,10 +97,13 @@ void UrDriver::stopTraj() {
}
void UrDriver::start() {
sec_interface_->start();
rt_interface_->robot_state_->setVersion(sec_interface_->robot_state_->getVersion());
rt_interface_->start();
}
void UrDriver::halt() {
sec_interface_->halt();
rt_interface_->halt();
}

6
src/ur_realtime_communication.cpp
View File

@@ -43,8 +43,8 @@ void UrRealtimeCommunication::start() {
keepalive_ = true;
if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
< 0)
printf("Error connecting");
printf("connecting...\n");
printf("Error connecting to RT port 30003");
printf("Realtime port: Connecting...\n");
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
}
@@ -78,7 +78,7 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
void UrRealtimeCommunication::run() {
uint8_t buf[2048];
bzero(buf, 2048);
printf("Got connection\n");
printf("Realtime port: Got connection\n");
connected_ = true;
while (keepalive_) {
read(sockfd_, buf, 2048);

5
src/ur_ros_wrapper.cpp
View File

@@ -35,6 +35,7 @@
class RosWrapper {
protected:
UrDriver robot_;
std::condition_variable rt_msg_cond_;
std::condition_variable msg_cond_;
ros::NodeHandle nh_;
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
@@ -51,7 +52,7 @@ protected:
public:
RosWrapper(std::string host) :
as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host), io_flag_delay_(
as_(nh_, "follow_joint_trajectory", false), robot_(rt_msg_cond_, msg_cond_, host), io_flag_delay_(
0.05), joint_offsets_(6, 0.0) {
std::string joint_prefix = "";
@@ -327,7 +328,7 @@ private:
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getNewDataAvailable()) {
msg_cond_.wait(locker);
rt_msg_cond_.wait(locker);
}
joint_msg.header.stamp = ros::Time::now();
joint_msg.position =