hydra_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * This is free and unencumbered software released into the public domain.
00004 * 
00005 * Anyone is free to copy, modify, publish, use, compile, sell, or
00006 * distribute this software, either in source code form or as a compiled
00007 * binary, for any purpose, commercial or non-commercial, and by any
00008 * means.
00009 * 
00010 * In jurisdictions that recognize copyright laws, the author or authors
00011 * of this software dedicate any and all copyright interest in the
00012 * software to the public domain. We make this dedication for the benefit
00013 * of the public at large and to the detriment of our heirs and
00014 * successors. We intend this dedication to be an overt act of
00015 * relinquishment in perpetuity of all present and future rights to this
00016 * software under copyright law.
00017 * 
00018 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
00019 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00020 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00021 * IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
00022 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00023 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00024 * OTHER DEALINGS IN THE SOFTWARE.
00025 * 
00026 * For more information, please refer to <http://unlicense.org/>
00027 * 
00028 **********************************************************************/
00029 #include <string>
00030 #include <ros/ros.h>
00031 #include "razer_hydra/hydra.h"
00032 #include "razer_hydra/HydraRaw.h"
00033 #include "razer_hydra/Hydra.h"
00034 #include "tf/tf.h"
00035 
00036 #include "sensor_msgs/Joy.h"
00037 
00038 // Visualization
00039 #include <tf/transform_broadcaster.h>
00040 
00041 using namespace razer_hydra;
00042 using std::string;
00043 
00044 int main(int argc, char **argv)
00045 {
00046     ros::init(argc, argv, "hydra_driver");
00047     ros::NodeHandle n, n_private("~");
00048 
00049     // Get configuration params
00050     string device;
00051     n_private.param<std::string>("device", device, "/dev/hidraw0");
00052 
00053     bool publish_tf = false;
00054     n_private.param<bool>("publish_tf", publish_tf, false);
00055 
00056     int polling_ms = 5;
00057     n_private.param<int>("polling_ms", polling_ms, 5);
00058     if( polling_ms <= 0 ) {
00059       ROS_WARN("polling_ms must be >= 1. Setting to default rate of 5 ms.");
00060       polling_ms = 5;
00061     }
00062 //    ros::Rate rate(1/(0.001*polling_ms));
00063 
00064 
00065     double corner_hz = 2.5;
00066     n_private.param<double>("corner_hz", corner_hz, 2.5);
00067     ROS_INFO("Using filter corner frequency of %.1f Hz", corner_hz);
00068 
00069 
00070     double pivot_arr[3];
00071     n_private.param<double>("pivot_x", pivot_arr[0], 0.0);
00072     n_private.param<double>("pivot_y", pivot_arr[1], 0.0);
00073     n_private.param<double>("pivot_z", pivot_arr[2], 0.0);
00074     tf::Vector3 pivot_vec(pivot_arr[0], pivot_arr[1], pivot_arr[2]);
00075 
00076     double grab_arr[3];
00077     n_private.param<double>("grab_x", grab_arr[0], 0.0);
00078     n_private.param<double>("grab_y", grab_arr[1], 0.0);
00079     n_private.param<double>("grab_z", grab_arr[2], 0.0);
00080     tf::Vector3 grab_vec(grab_arr[0], grab_arr[1], grab_arr[2]);
00081 
00082     bool use_grab_frame = false;
00083     n_private.param<bool>("use_grab_frame", use_grab_frame, false);
00084 
00085     ROS_INFO("Setting pivot offset [%.3f, %.3f, %.3f]", pivot_vec.x(), pivot_vec.y(), pivot_vec.z());
00086     ROS_INFO("Setting grab offset [%.3f, %.3f, %.3f]", grab_vec.x(), grab_vec.y(), grab_vec.z());
00087     if(use_grab_frame)
00088       ROS_INFO("Sending messages using the 'grab' frame.");
00089     else
00090       ROS_INFO("Sending messages using the 'pivot' frame.");
00091 
00092     if(publish_tf)
00093       ROS_INFO("Publishing frame data to TF.");
00094 
00095     // Initialize ROS stuff
00096     ros::Publisher raw_pub = n.advertise<razer_hydra::HydraRaw>("hydra_raw", 1);
00097     ros::Publisher calib_pub = n.advertise<razer_hydra::Hydra>("hydra_calib", 1);
00098     ros::Publisher joy_pub = n.advertise<sensor_msgs::Joy>("hydra_joy", 1);
00099     tf::TransformBroadcaster *broadcaster = 0;
00100     if(publish_tf) broadcaster = new tf::TransformBroadcaster();
00101 
00102 
00103     RazerHydra hydra;
00104     ROS_INFO("opening hydra on %s", device.c_str());
00105     if (!hydra.init(device.c_str()))
00106     {
00107       ROS_FATAL("couldn't open hydra on %s", device.c_str());
00108       return 1;
00109     }
00110     ROS_INFO("starting stream... (first packet takes a few seconds)");
00111     while (n.ok())
00112     {
00113       if (hydra.poll(polling_ms, corner_hz))
00114       {
00115         // publish raw data
00116         razer_hydra::HydraRaw msg;
00117         msg.header.stamp = ros::Time::now();
00118         for (int i = 0; i < 6; i++)
00119           msg.pos[i] = hydra.raw_pos[i];
00120         for (int i = 0; i < 8; i++)
00121           msg.quat[i] = hydra.raw_quat[i];
00122         for (int i = 0; i < 2; i++)
00123           msg.buttons[i] = hydra.raw_buttons[i];
00124         for (int i = 0; i < 6; i++)
00125           msg.analog[i] = hydra.raw_analog[i];
00126         raw_pub.publish(msg);
00127 
00128         std::vector<geometry_msgs::TransformStamped> transforms(4);
00129 
00130         // publish calibrated data
00131         razer_hydra::Hydra h_msg;
00132         h_msg.header.stamp = msg.header.stamp;
00133         for (int i = 0; i < 2; i++)
00134         {
00135           tf::Transform transform(hydra.quat[i], hydra.pos[i]); // original transform!
00136           tf::Transform t_pivot = transform, t_grab = transform;
00137 
00138           // pivot
00139           t_pivot.setOrigin(transform.getOrigin() + transform.getBasis()*pivot_vec);
00140           tf::transformTFToMsg(t_pivot, transforms[i].transform);
00141 
00142           // grab point
00143           t_grab.setOrigin(transform.getOrigin() + transform.getBasis()*grab_vec);
00144           tf::transformTFToMsg(t_grab, transforms[2+i].transform);
00145 
00146           h_msg.paddles[i].transform = use_grab_frame ? transforms[2+i].transform : transforms[i].transform;
00147         }
00148         for (int i = 0; i < 7; i++)
00149         {
00150           h_msg.paddles[0].buttons[i] = hydra.buttons[i];
00151           h_msg.paddles[1].buttons[i] = hydra.buttons[i+7];
00152         }
00153         for (int i = 0; i < 2; i++)
00154         {
00155           h_msg.paddles[0].joy[i] = hydra.analog[i];
00156           h_msg.paddles[1].joy[i] = hydra.analog[i+3];
00157         }
00158         h_msg.paddles[0].trigger = hydra.analog[2];
00159         h_msg.paddles[1].trigger = hydra.analog[5];
00160         calib_pub.publish(h_msg);
00161 
00162         if(broadcaster)
00163         {
00164           std::string frames[4] = {"hydra_left_pivot", "hydra_right_pivot", "hydra_left_grab", "hydra_right_grab"};
00165           for(int kk = 0; kk < 4; kk++)
00166           {
00167             transforms[kk].header.stamp = h_msg.header.stamp;
00168             transforms[kk].header.frame_id = "hydra_base";
00169             transforms[kk].child_frame_id = frames[kk];
00170           }
00171 
00172           broadcaster->sendTransform(transforms);
00173         }
00174 
00175         // emulate PS3 joystick message
00176         sensor_msgs::Joy joy_msg;
00177 
00178         joy_msg.axes.resize(16);
00179         joy_msg.buttons.resize(16);
00180         joy_msg.header.stamp = h_msg.header.stamp;
00181 
00182         // analog pads
00183         // left
00184         joy_msg.axes[0] = -h_msg.paddles[0].joy[0]; // x
00185         joy_msg.axes[1] = h_msg.paddles[0].joy[1]; // y
00186         joy_msg.buttons[1] = h_msg.paddles[0].buttons[6]; // push
00187         //right
00188         joy_msg.axes[2] = -h_msg.paddles[1].joy[0]; // x
00189         joy_msg.axes[3] = h_msg.paddles[1].joy[1]; // y
00190         joy_msg.buttons[2] = h_msg.paddles[1].buttons[6]; // push
00191 
00192         // push buttons
00193         joy_msg.buttons[7] = h_msg.paddles[0].buttons[1]; // left
00194         joy_msg.buttons[5] = h_msg.paddles[0].buttons[3]; // right
00195         joy_msg.buttons[6] = h_msg.paddles[0].buttons[2]; // down
00196         joy_msg.buttons[4] = h_msg.paddles[0].buttons[4]; // up
00197 
00198         joy_msg.buttons[15] = h_msg.paddles[1].buttons[1]; // square
00199         joy_msg.buttons[13] = h_msg.paddles[1].buttons[3]; // circle
00200         joy_msg.buttons[14] = h_msg.paddles[1].buttons[2]; // triangle
00201         joy_msg.buttons[12] = h_msg.paddles[1].buttons[4]; // cross
00202 
00203         // menu buttons
00204         joy_msg.buttons[0] = h_msg.paddles[0].buttons[5]; // select
00205         joy_msg.buttons[3] = h_msg.paddles[1].buttons[5]; // start
00206 
00207         // top buttons
00208         joy_msg.buttons[10] = h_msg.paddles[0].buttons[0]; // L1
00209         joy_msg.buttons[8] = h_msg.paddles[0].trigger > 0.15; // L2
00210         joy_msg.axes[8] = h_msg.paddles[0].trigger; // L2
00211 
00212         joy_msg.buttons[11] = h_msg.paddles[1].buttons[0]; // R1
00213         joy_msg.buttons[9] = h_msg.paddles[1].trigger > 0.15; // R2
00214         joy_msg.axes[9] = h_msg.paddles[1].trigger; // R2
00215 
00216         // fill in fake axes for binary buttons
00217         for ( int i=4; i<8; i++ )
00218         {
00219           joy_msg.axes[i] = joy_msg.buttons[i] ? 1.0 : 0.0;
00220         }
00221         for ( int i=10; i<16; i++ )
00222         {
00223           joy_msg.axes[i] = joy_msg.buttons[i] ? 1.0 : 0.0;
00224         }
00225 
00226         joy_pub.publish(joy_msg);
00227 
00228         ros::spinOnce();
00229       }
00230     }
00231 
00232     // clean up
00233     if(broadcaster) delete broadcaster;
00234     return 0;
00235 }
00236 


razer_hydra
Author(s): Adam Leeper
autogenerated on Fri Aug 28 2015 12:21:51