teleop.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Aldebaran
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * LOCAL includes
00020  */
00021 #include "teleop.hpp"
00022 
00023 
00024 namespace naoqi
00025 {
00026 namespace subscriber
00027 {
00028 
00029 TeleopSubscriber::TeleopSubscriber( const std::string& name, const std::string& cmd_vel_topic, const std::string& joint_angles_topic, const qi::SessionPtr& session ):
00030   cmd_vel_topic_(cmd_vel_topic),
00031   joint_angles_topic_(joint_angles_topic),
00032   BaseSubscriber( name, cmd_vel_topic, session ),
00033   p_motion_( session->service("ALMotion") )
00034 {}
00035 
00036 void TeleopSubscriber::reset( ros::NodeHandle& nh )
00037 {
00038   sub_cmd_vel_ = nh.subscribe( cmd_vel_topic_, 10, &TeleopSubscriber::cmd_vel_callback, this );
00039   sub_joint_angles_ = nh.subscribe( joint_angles_topic_, 10, &TeleopSubscriber::joint_angles_callback, this );
00040 
00041   is_initialized_ = true;
00042 }
00043 
00044 void TeleopSubscriber::cmd_vel_callback( const geometry_msgs::TwistConstPtr& twist_msg )
00045 {
00046   // no need to check for max velocity since motion clamps the velocities internally
00047   const float& vel_x = twist_msg->linear.x;
00048   const float& vel_y = twist_msg->linear.y;
00049   const float& vel_th = twist_msg->angular.z;
00050 
00051   std::cout << "going to move x: " << vel_x << " y: " << vel_y << " th: " << vel_th << std::endl;
00052   p_motion_.async<void>("move", vel_x, vel_y, vel_th );
00053 }
00054 
00055 void TeleopSubscriber::joint_angles_callback( const naoqi_bridge_msgs::JointAnglesWithSpeedConstPtr& js_msg )
00056 {
00057   if ( js_msg->relative==0 )
00058   {
00059     p_motion_.async<void>("setAngles", js_msg->joint_names, js_msg->joint_angles, js_msg->speed);
00060   }
00061   else
00062   {
00063     p_motion_.async<void>("changeAngles", js_msg->joint_names, js_msg->joint_angles, js_msg->speed);
00064   }
00065 }
00066 
00067 } //publisher
00068 } // naoqi


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14