SVHSinTest.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK SVH Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2014 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 //
00014 // -- END LICENSE BLOCK ------------------------------------------------
00015 
00016 //----------------------------------------------------------------------
00027 //----------------------------------------------------------------------
00028 
00029 // ROS includes.
00030 #include <ros/ros.h>
00031 
00032 // Messages
00033 #include <sensor_msgs/JointState.h>
00034 #include <std_msgs/Float32.h>
00035 #include <std_msgs/Empty.h>
00036 #include <std_msgs/Int8.h>
00037 
00038 #include <icl_core/EnumHelper.h>
00039 
00040 #include <stdlib.h> /* srand, rand */
00041 #include <time.h>   /* time */
00042 
00043 
00044 // Consts
00045 // Loop Rate (i.e Frequency) of the ROS node -> 50 = 50HZ
00046 double loop_rate = 50;
00047 // Time of a half Sin. i.e. 10 = In 10 Seconds the selected fingers will perform a close and open
00048 // (Sin to 1PI)
00049 double sin_duration = 10;
00050 
00051 
00052 // Local Vars
00053 bool running = true;
00054 /*--------------------------------------------------------------------
00055  * Callback functions
00056  *------------------------------------------------------------------*/
00057 // Toggle on off
00058 void runCallback(const std_msgs::Empty&)
00059 {
00060   running = !running;
00061 }
00062 
00063 void speedCallback(const std_msgs::Float32ConstPtr &msg){
00064   sin_duration = msg->data;
00065 }
00066 void loopCallback(const std_msgs::Float32ConstPtr &msg){
00067   loop_rate = msg->data;
00068 }
00069 
00070 /*--------------------------------------------------------------------
00071  * main()
00072  * Main function to set up ROS node.
00073  *------------------------------------------------------------------*/
00074 
00075 int main(int argc, char** argv)
00076 {
00079   std::string name_prefix;
00080 
00081   // Set up ROS.
00082   ros::init(argc, argv, "svh_sine_test");
00083   ros::NodeHandle nh("~");
00084 
00085   try
00086   {
00087     nh.param<std::string>("name_prefix", name_prefix, "left_hand");
00088   }
00089   catch (ros::InvalidNameException e)
00090   {
00091     ROS_ERROR("Parameter Error!");
00092   }
00093 
00094   // Subscribe connect topic (Empty)
00095   ros::Subscriber run_sub = nh.subscribe("toggle_run", 1, runCallback);
00096   ros::Subscriber speed_sub = nh.subscribe("speed", 1, speedCallback);
00097   ros::Subscriber loop_sub = nh.subscribe("loop", 1, loopCallback);
00098 
00099   // Publish current target positions
00100   ros::Publisher channel_pos_pub = nh.advertise<sensor_msgs::JointState>("channel_targets", 1);
00101 
00102   // joint state message template
00103   sensor_msgs::JointState channel_pos;
00104   channel_pos.name.resize(9);
00105   channel_pos.position.resize(9, 0.0);
00106 
00107   // Pre Fill the Names of the Finger
00108   channel_pos.name[0] = name_prefix + "_" + "Thumb_Flexion";
00109   channel_pos.name[1] = name_prefix + "_" + "Thumb_Opposition";
00110   channel_pos.name[2] = name_prefix + "_" + "Index_Finger_Distal";
00111   channel_pos.name[3] = name_prefix + "_" + "Index_Finger_Proximal";
00112   channel_pos.name[4] = name_prefix + "_" + "Middle_Finger_Distal";
00113   channel_pos.name[5] = name_prefix + "_" + "Middle_Finger_Proximal";
00114   channel_pos.name[6] = name_prefix + "_" + "Ring_Finger";
00115   channel_pos.name[7] = name_prefix + "_" + "Pinky";
00116   channel_pos.name[8] = name_prefix + "_" + "Finger_Spread";
00117 
00118   // Set up the normalized time
00119   ros::Time counter      = ros::Time::now();
00120   double normalized_time = 0;
00121 
00122   // Init the Random
00123   srand(time(NULL));
00124 
00125   // Tell ROS how fast to run this node. (100 = 100 Hz = 10 ms)
00126 
00127   // Main loop.
00128   while (nh.ok())
00129   {
00130     // Only when toggled on (std empty message)
00131     if (running)
00132     {
00133       // Get a normalized time
00134       if ((ros::Time::now() - counter) > ros::Duration(sin_duration))
00135       {
00136         counter = ros::Time::now();
00137         normalized_time = 0;
00138       }
00139       else
00140       {
00141         normalized_time = (ros::Time::now() - counter).toSec() / sin_duration;
00142       }
00143 
00144       // Publish channel positions in RAD.
00145       // Everything is 0 by default
00146       for (size_t channel = 0; channel < 9; ++channel)
00147       {
00148         double cur_pos = 0.0;
00149 
00150         channel_pos.position[channel] = cur_pos;
00151       }
00152 
00153       channel_pos.header.stamp = ros::Time::now();
00154 
00155 
00156       // Calculate a halfe sin for the fingers
00157       double cur_pos = 0.4 + 0.3 * sin(normalized_time * 2 * 3.14);
00158 
00159       // Set the Spread to 0.3 (to avoid any collisions)
00160       channel_pos.position[8] = 0.3; // Finger Spread
00161 
00162       channel_pos.position[0] = 1 - cur_pos; // Thumb_Flexion
00163 
00164       // Set the 2 Test fingers to the sin value
00165       channel_pos.position[7] = cur_pos; // Pinky
00166       channel_pos.position[2] = cur_pos; // Index Distal
00167       channel_pos.position[3] = cur_pos; // Index proximal
00168       channel_pos.position[4] = cur_pos; // Middle Distal
00169       channel_pos.position[5] = cur_pos; // Middle proximal
00170       channel_pos.position[6] = cur_pos; // Ring Finger
00171 
00172       // Publish
00173       channel_pos_pub.publish(channel_pos);
00174     }
00175 
00176     ros::Rate rate(loop_rate);
00177     rate.sleep();
00178     ros::spinOnce();
00179 
00180     // TO INDTRODUCE A VARIIING TIME RATE (in This case 50 - 100 HZ ) Uncomment this (discouraged!
00181     // Will result in strange things obiously)
00182     // Was meant to test jitter in the trajectory generation
00183     // rate = 50+(rand() % 10 )*5;
00184   }
00185 
00186   return 0;
00187 } // end main()


schunk_svh_driver
Author(s): Georg Heppner
autogenerated on Thu Jun 6 2019 18:29:08