netft_utils_cpp_test.cpp
Go to the documentation of this file.
00001 
00002 /*
00003 Copyright (c) 2016, Los Alamos National Security, LLC
00004 All rights reserved.
00005 Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software.  NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE.  If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL.
00006 
00007 Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00008 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 
00009 2. 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. 
00010 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 
00011 
00012 THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC 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 LOS ALAMOS NATIONAL SECURITY, LLC 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.
00013 
00014 Author: Andy Zelenak
00015 */
00016 
00017 #ifndef KEY_COMPLIANCE_H_
00018 #define KEY_COMPLIANCE_H_
00019 #endif
00020 
00021 #include "ros/ros.h"
00022 #include "std_msgs/Bool.h"
00023 #include "std_msgs/String.h"
00024 #include "geometry_msgs/WrenchStamped.h"
00025 #include "geometry_msgs/TwistStamped.h"
00026 #include "ros/console.h"
00027 #include "netft_utils_lean.h"
00028 
00029 // Test the C++ interface for the ATI force/torque sensor with a Netbox.
00030 
00031 // Most of the action happens in this class.
00032 // The underscores denote members of the class.
00033 class netftExample
00034 {
00035   public:
00036 
00037     ros::NodeHandle n_;
00038     double ftSleep_; // Controls the data acquisition rate
00039     NetftUtilsLean* fti_;
00040     geometry_msgs::WrenchStamped wrench_; // For data from the sensor
00041 
00042     // Constructor
00043     netftExample();
00044 
00045     // Most of the action happens here
00046     void getFTData();
00047 };
00048 
00049 int main(int argc, char **argv)
00050 {
00051   ros::init(argc, argv, "netft_utils_cpp_test");
00052   ros::AsyncSpinner* spinner;
00053   spinner = new ros::AsyncSpinner(3);
00054   spinner->start();
00055 
00056   // Create a new netftExample object.
00057   // Most of the action happens here
00058   netftExample *myNetftExample = new netftExample();
00059   
00060   ros::waitForShutdown();
00061  
00062   return 0;
00063 }
00064 
00065 // Constructor for the netftExample class
00066 netftExample::netftExample()
00067 {
00068   // Connect and bias the ft sensor
00069   fti_ = new NetftUtilsLean(&n_);
00070   fti_->setFTAddress("192.168.1.84");
00071   // Adjust the data acquisition rate and set the World and Sensor frames, respectively
00072   ftSleep_ = 0.05; // Will yield a 20 Hz data acquisition rate
00073   fti_->initialize(1/ftSleep_, "right_ur5_base_link", "right_ur5_ee_link");
00074   
00075   // Set max and threshold force/torque readings
00076   // Vaules below the thresholds will register as zero
00077   fti_->setMax(80.0, 8.0, 60.0, 6.0);
00078   std::future<bool> ftThread;
00079   ftThread = std::async(std::launch::async, &NetftUtilsLean::run, fti_);
00080   fti_->biasSensor(1);
00081 
00082   getFTData();
00083 }
00084 
00085 void netftExample::getFTData()
00086 {
00087   while (ros::ok())
00088   {
00089     fti_->getToolData(wrench_);
00090     ROS_INFO_STREAM("X force in the tool frame: " << wrench_.wrench.force.x);
00091 
00092     fti_->getWorldData(wrench_);
00093     ROS_INFO_STREAM("X force in the world frame: " << wrench_.wrench.force.x);
00094 
00095     sleep(ftSleep_);
00096   }
00097 }


netft_utils
Author(s): Alex von Sternberg , Derek King, Andy Zelenak
autogenerated on Sat Jul 15 2017 02:45:58