rmp_teleop_node.cpp
Go to the documentation of this file.
00001 /*
00002   COPYRIGHT (c) 2014 SEGWAY Inc.
00003 
00004   Software License Agreement:
00005 
00006   The software supplied herewith by Segway Inc. (the "Company") for its 
00007   RMP Robotic Platforms is intended and supplied to you, the Company's 
00008   customer, for use solely and exclusively with Segway products. The 
00009   software is owned by the Company and/or its supplier, and is protected 
00010   under applicable copyright laws.  All rights are reserved. Any use in 
00011   violation of the foregoing restrictions may subject the user to criminal
00012   sanctions under applicable laws, as well as to civil liability for the 
00013   breach of the terms and conditions of this license. The Company may 
00014   immediately terminate this Agreement upon your use of the software with 
00015   any products that are not Segway products.
00016 
00017   You shall indemnify, defend and hold the Company harmless from any claims, 
00018   demands, liabilities or expenses, including reasonable attorneys fees, incurred 
00019   by the Company as a result of any claim or proceeding against the Company 
00020   arising out of or based upon: 
00021 
00022   (i) The combination, operation or use of the software by you with any hardware, 
00023       products, programs or data not supplied or approved in writing by the Company, 
00024       if such claim or proceeding would have been avoided but for such combination, 
00025       operation or use.
00026 
00027   (ii) The modification of the software by or on behalf of you.
00028 
00029   (iii) Your use of the software.
00030 
00031   THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTIES,
00032   WHETHER EXPRESS, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
00033   TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00034   PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT,
00035   IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR
00036   CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
00037 */
00038 
00039 #include <ros/ros.h>
00040 #include <sensor_msgs/Joy.h>
00041 #include <geometry_msgs/TwistStamped.h>
00042 #include <rmp_msgs/BoolStamped.h>
00043 #include <rmp_msgs/AudioCommand.h>
00044 
00045 #include <RmpJoystickConverter.h>
00046 
00050 class RmpTeleop
00051 {
00052 public:
00056   RmpTeleop();
00057 
00061   ~RmpTeleop();
00062 
00066   void Initialize();
00067 
00068 private:
00072   void ProcessJoystickMessage(const sensor_msgs::Joy::ConstPtr& rpJoyMessage);
00073 
00077   ros::NodeHandle m_NodeHandle;
00078 
00082   ros::Publisher m_VelocityCommandPublisher;
00083 
00087   ros::Publisher m_DeadmanPublisher;
00088 
00092   ros::Publisher m_AudioCommandPublisher;
00093 
00097   ros::Subscriber m_JoystickMessageSubscriber;
00098 
00103   JoystickConverter::Ptr m_pJoystickConverter;
00104 
00109   double m_TranslationalVelocityScale;
00110 
00115   double m_RotationalVelocityScale;
00116 
00122   double m_TranslationalVelocityBoostScale;
00123 
00129   double m_RotationalVelocityBoostScale;
00130 };
00131 
00132 RmpTeleop::RmpTeleop()
00133   : m_NodeHandle("~")
00134   , m_TranslationalVelocityScale(0.0)
00135   , m_RotationalVelocityScale(0.0)
00136   , m_TranslationalVelocityBoostScale(0.0)
00137   , m_RotationalVelocityBoostScale(0.0)
00138 {}
00139 
00140 RmpTeleop::~RmpTeleop()
00141 {}
00142 
00143 void RmpTeleop::Initialize()
00144 {
00145   // Parameters
00146   std::string joyMessageTopic, velocityCommandTopic, deadmanTopic, audioCommandTopic;
00147   double updateFrequency;
00148 
00149   m_NodeHandle.param("joy_topic", joyMessageTopic, std::string("/rmp440le/joy"));
00150   m_NodeHandle.param("velocity_command_topic", velocityCommandTopic, std::string("/rmp440le/base/vel_cmd"));
00151   m_NodeHandle.param("deadman_topic", deadmanTopic, std::string("/rmp440le/deadman"));
00152   m_NodeHandle.param("audio_command_topic", audioCommandTopic, std::string("/rmp440le/audio_cmd"));
00153   m_NodeHandle.param("update_frequency", updateFrequency, 50.0);
00154   m_NodeHandle.param("translational_velocity_scale", m_TranslationalVelocityScale, 2.2352);
00155   m_NodeHandle.param("rotational_velocity_scale", m_RotationalVelocityScale, 3.0);
00156   m_NodeHandle.param("translational_velocity_boost_scale", m_TranslationalVelocityBoostScale, 8.0);
00157   m_NodeHandle.param("rotational_velocity_boost_scale", m_RotationalVelocityBoostScale, 4.4);
00158 
00159   m_pJoystickConverter = JoystickConverter::Create(JoystickConverter::XBOX_WIRELLESS);
00160 
00161   // Set up ROS communication
00162   m_VelocityCommandPublisher = m_NodeHandle.advertise<geometry_msgs::TwistStamped>(velocityCommandTopic, 1);
00163   m_DeadmanPublisher = m_NodeHandle.advertise<rmp_msgs::BoolStamped>(deadmanTopic, 1);
00164   m_AudioCommandPublisher = m_NodeHandle.advertise<rmp_msgs::AudioCommand>(audioCommandTopic, 1);
00165   m_JoystickMessageSubscriber = m_NodeHandle.subscribe<sensor_msgs::Joy>(joyMessageTopic, 1, &RmpTeleop::ProcessJoystickMessage, this);
00166 
00167   ros::Rate rate(updateFrequency);
00168 
00169   while (ros::ok())
00170   {
00171     ros::spinOnce();
00172 
00173     rate.sleep();
00174   }
00175 }
00176 
00177 void RmpTeleop::ProcessJoystickMessage(const sensor_msgs::Joy::ConstPtr& rpJoyMessage)
00178 {
00179   geometry_msgs::TwistStamped velocityCommand;
00180 
00181   if (m_pJoystickConverter->GetBoost(*rpJoyMessage))
00182   {
00183     velocityCommand.twist.linear.x = m_TranslationalVelocityBoostScale * m_pJoystickConverter->GetTranslationalVelocity(*rpJoyMessage);
00184     velocityCommand.twist.angular.z = m_RotationalVelocityBoostScale * m_pJoystickConverter->GetRotationalVelocity(*rpJoyMessage);
00185   }
00186   else
00187   {
00188     velocityCommand.twist.linear.x = m_TranslationalVelocityScale * m_pJoystickConverter->GetTranslationalVelocity(*rpJoyMessage);
00189     velocityCommand.twist.angular.z = m_RotationalVelocityScale * m_pJoystickConverter->GetRotationalVelocity(*rpJoyMessage);
00190   }
00191   
00192 
00193   velocityCommand.header.stamp = ros::Time::now();
00194 
00195   m_VelocityCommandPublisher.publish(velocityCommand);
00196 
00197   rmp_msgs::BoolStamped deadman;
00198   deadman.header.stamp = ros::Time::now();
00199     
00200   if (m_pJoystickConverter->GetDeadman(*rpJoyMessage))
00201   {
00202     deadman.data = true;
00203   }
00204   else
00205   {
00206     deadman.data = false;
00207   }
00208 
00209   m_DeadmanPublisher.publish(deadman);
00210 
00211   if (m_pJoystickConverter->GetAudioCommand(*rpJoyMessage) > 0)
00212   {
00213     rmp_msgs::AudioCommand audioCommand;
00214     audioCommand.header.stamp = ros::Time::now();
00215     audioCommand.command = static_cast<uint32_t>(m_pJoystickConverter->GetAudioCommand(*rpJoyMessage));
00216 
00217     m_AudioCommandPublisher.publish(audioCommand);
00218   }
00219 }
00220 
00221 int main(int argc, char** argv)
00222 {
00223   ros::init(argc, argv, "rmp_teleop_node");
00224   
00225   RmpTeleop teleop;
00226   teleop.Initialize();
00227   
00228   return 0;
00229 }


rmp_teleop
Author(s):
autogenerated on Wed Aug 26 2015 16:24:37