Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
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
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 }