Implemented ros-control

src/robot_state_RT.cpp

@@ -41,23 +41,33 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
 	v_robot_ = 0.0;
 	i_robot_ = 0.0;
 	v_actual_.assign(6, 0.0);
-	new_data_available_ = false;
+	data_published_ = false;
+	controller_updated_ = false;
 	pMsg_cond_ = &msg_cond;
 }
 
 RobotStateRT::~RobotStateRT() {
 	/* Make sure nobody is waiting after this thread is destroyed */
-	new_data_available_ = true;
+	data_published_ = true;
+	controller_updated_ = true;
 	pMsg_cond_->notify_all();
 }
 
-bool RobotStateRT::getNewDataAvailable() {
-	return new_data_available_;
+
+void RobotStateRT::setDataPublished() {
+	data_published_ = false;
+}
+bool RobotStateRT::getDataPublished() {
+	return data_published_;
 }
 
-void RobotStateRT::finishedReading() {
-	new_data_available_ = false;
+void RobotStateRT::setControllerUpdated() {
+	controller_updated_ = false;
 }
+bool RobotStateRT::getControllerUpdated() {
+	return controller_updated_;
+}
+
 
 double RobotStateRT::ntohd(uint64_t nf) {
 	double x;
@@ -394,7 +404,8 @@ void RobotStateRT::unpack(uint8_t * buf) {
 		v_actual_ = unpackVector(buf, offset, 6);
 	}
 	val_lock_.unlock();
-	new_data_available_ = true;
+	controller_updated_ = true;
+	data_published_ = true;
 	pMsg_cond_->notify_all();
 
 }

src/ur_hardware_interface.cpp

@@ -0,0 +1,133 @@
+/*
+ * ur_hardware_control_loop.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */
+
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman
+ Desc:   Example ros_control hardware interface that performs a perfect control loop for simulation
+ */
+
+#include <ur_modern_driver/ur_hardware_interface.h>
+
+namespace ros_control_ur {
+
+UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) :
+		nh_(nh), robot_(robot) {
+	// Initialize shared memory and interfaces here
+	init(); // this implementation loads from rosparam
+
+	ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
+}
+
+void UrHardwareInterface::init() {
+	ROS_INFO_STREAM_NAMED("ur_hardware_interface",
+			"Reading rosparams from namespace: " << nh_.getNamespace());
+
+	// Get joint names
+	nh_.getParam("hardware_interface/joints", joint_names_);
+	if (joint_names_.size() == 0) {
+		ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
+				"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
+		exit(-1);
+	}
+	num_joints_ = joint_names_.size();
+
+	// Resize vectors
+	joint_position_.resize(num_joints_);
+	joint_velocity_.resize(num_joints_);
+	joint_effort_.resize(num_joints_);
+	joint_velocity_command_.resize(num_joints_);
+
+	// Initialize controller
+	for (std::size_t i = 0; i < num_joints_; ++i) {
+		ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
+				"Loading joint name: " << joint_names_[i]);
+
+		// Create joint state interface
+		joint_state_interface_.registerHandle(
+				hardware_interface::JointStateHandle(joint_names_[i],
+						&joint_position_[i], &joint_velocity_[i],
+						&joint_effort_[i]));
+
+		// Create velocity joint interface
+		velocity_joint_interface_.registerHandle(
+				hardware_interface::JointHandle(
+						joint_state_interface_.getHandle(joint_names_[i]),
+						&joint_velocity_command_[i]));
+	}
+
+	// Create joint state interface
+	force_torque_interface_.registerHandle(
+			hardware_interface::ForceTorqueSensorHandle("wrench", "",
+					robot_force_, robot_torque_));
+
+	registerInterface(&joint_state_interface_); // From RobotHW base class.
+	registerInterface(&velocity_joint_interface_); // From RobotHW base class.
+	registerInterface(&force_torque_interface_); // From RobotHW base class.
+}
+
+void UrHardwareInterface::read() {
+	std::vector<double> pos, vel, current, tcp;
+	pos = robot_->rt_interface_->robot_state_->getQActual();
+	vel = robot_->rt_interface_->robot_state_->getQdActual();
+	current = robot_->rt_interface_->robot_state_->getIActual();
+	tcp = robot_->rt_interface_->robot_state_->getTcpForce();
+	for (std::size_t i = 0; i < num_joints_; ++i) {
+		joint_position_[i] = pos[i];
+		joint_velocity_[i] = vel[i];
+		joint_effort_[i] = current[i];
+	}
+	for (std::size_t i = 0; i < 3; ++i) {
+		robot_force_[i] = tcp[i];
+		robot_torque_[i] = tcp[i + 3];
+	}
+
+}
+
+void UrHardwareInterface::write() {
+	robot_->setSpeed(joint_velocity_command_[0],joint_velocity_command_[1],joint_velocity_command_[2],joint_velocity_command_[3],joint_velocity_command_[4],joint_velocity_command_[5],100);
+}
+
+} // namespace

src/ur_ros_wrapper.cpp

@@ -10,6 +10,7 @@
  */
 
 #include "ur_modern_driver/ur_driver.h"
+#include "ur_modern_driver/ur_hardware_interface.h"
 #include <string.h>
 #include <vector>
 #include <mutex>
@@ -17,6 +18,7 @@
 #include <thread>
 #include <algorithm>
 #include <cmath>
+#include <time.h>
 
 #include "ros/ros.h"
 #include <ros/console.h>
@@ -35,6 +37,7 @@
 #include "ur_msgs/Digital.h"
 #include "ur_msgs/Analog.h"
 #include "std_msgs/String.h"
+#include <controller_manager/controller_manager.h>
 
 class RosWrapper {
 protected:
@@ -52,9 +55,12 @@ protected:
 	ros::ServiceServer payload_srv_;
 	std::thread* rt_publish_thread_;
 	std::thread* mb_publish_thread_;
+	std::thread* ros_control_thread_;
 	double io_flag_delay_;
 	double max_velocity_;
 	std::vector<double> joint_offsets_;
+	boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
+	boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
 
 public:
 	RosWrapper(std::string host) :
@@ -76,6 +82,11 @@ public:
 		joint_names.push_back(joint_prefix + "wrist_3_joint");
 		robot_.setJointNames(joint_names);
 
+		hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_));
+		controller_manager_.reset(
+				new controller_manager::ControllerManager(
+						hardware_interface_.get(), nh_));
+
 		//Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
 		max_velocity_ = 10.;
 		if (ros::param::get("~max_velocity", max_velocity_)) {
@@ -102,13 +113,13 @@ public:
 		robot_.start();
 
 		//register the goal and feedback callbacks
-		as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
-		as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
+		//as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this));
+		//as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this));
 
-		as_.start();
+		//as_.start();
 
 		//subscribe to the data topic of interest
-		speed_sub_ = nh_.subscribe("joint_speed", 1,
+		speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
 				&RosWrapper::speedInterface, this);
 		urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
 				&RosWrapper::urscriptInterface, this);
@@ -118,10 +129,12 @@ public:
 		payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
 				&RosWrapper::setPayload, this);
 
-		rt_publish_thread_ = new std::thread(
+		ros_control_thread_ = new std::thread(
+				boost::bind(&RosWrapper::rosControlLoop, this));
+		/*rt_publish_thread_ = new std::thread(
 				boost::bind(&RosWrapper::publishRTMsg, this));
 		mb_publish_thread_ = new std::thread(
-				boost::bind(&RosWrapper::publishMbMsg, this));
+				boost::bind(&RosWrapper::publishMbMsg, this)); */
 		ROS_DEBUG("The action server for this driver has been started");
 	}
 
@@ -269,7 +282,7 @@ private:
 						traj.points[i].velocities[mapping[j]]);
 				if (traj.points[i].accelerations.size() != 0)
 					new_point.accelerations.push_back(
-						traj.points[i].accelerations[mapping[j]]);
+							traj.points[i].accelerations[mapping[j]]);
 			}
 			new_point.time_from_start = traj.points[i].time_from_start;
 			new_traj.push_back(new_point);
@@ -330,18 +343,49 @@ private:
 
 	}
 
+	void rosControlLoop() {
+		ros::Duration elapsed_time;
+		struct timespec last_time, current_time;
+		static const double BILLION = 1000000000.0;
+
+		clock_gettime(CLOCK_MONOTONIC, &last_time);
+		while (ros::ok()) {
+			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_->getControllerUpdated()) {
+				rt_msg_cond_.wait(locker);
+			}
+			clock_gettime(CLOCK_MONOTONIC, &current_time);
+			elapsed_time = ros::Duration(
+					current_time.tv_sec - last_time.tv_sec
+							+ (current_time.tv_nsec - last_time.tv_nsec)
+									/ BILLION);
+			last_time = current_time;
+			// Input
+			hardware_interface_->read();
+
+			// Control
+			//controller_manager_->update(ros::Time::now(), elapsed_time);
+			controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time);
+
+			// Output
+			hardware_interface_->write();
+			robot_.rt_interface_->robot_state_->setControllerUpdated();
+		}
+	}
+
 	void publishRTMsg() {
 		ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
-				"/joint_states", 1);
+				"joint_states", 1);
 		ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
-				"/wrench", 1);
+				"wrench", 1);
 		while (ros::ok()) {
 			sensor_msgs::JointState joint_msg;
 			joint_msg.name = robot_.getJointNames();
 			geometry_msgs::WrenchStamped wrench_msg;
 			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()) {
+			while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
 				rt_msg_cond_.wait(locker);
 			}
 			joint_msg.header.stamp = ros::Time::now();
@@ -365,13 +409,13 @@ private:
 			wrench_msg.wrench.torque.z = tcp_force[5];
 			wrench_pub.publish(wrench_msg);
 
-			robot_.rt_interface_->robot_state_->finishedReading();
+			robot_.rt_interface_->robot_state_->setDataPublished();
 
 		}
 	}
 
 	void publishMbMsg() {
-		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("/io_states",
+		ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states",
 				1);
 
 		while (ros::ok()) {
@@ -443,7 +487,11 @@ int main(int argc, char **argv) {
 
 	RosWrapper interface(host);
 
-	ros::spin();
+	ros::AsyncSpinner spinner(3);
+	spinner.start();
+
+	ros::waitForShutdown();
+
 	interface.halt();
 
 	exit(0);