movement_publisher.cpp
Go to the documentation of this file.
00001 
00026 #include "sr_movements/movement_publisher.hpp"
00027 
00028 #include <math.h>
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 
00031 namespace shadowrobot
00032 {
00033   MovementPublisher::MovementPublisher(std::string joint_name, double rate, unsigned int repetition, unsigned int nb_mvt_step, std::string controller_type, bool testing, HandCommander* hand_commander)
00034     : joint_name_(joint_name), nh_tilde("~"), publishing_rate( rate ), repetition(repetition),
00035       min(0.0), max(1.5), last_target_(0.0), nb_mvt_step(nb_mvt_step),
00036       SError_(0.0), MSError_(0.0), n_samples_(0), controller_type(controller_type)
00037   {
00038     //this is a gazebo test, sleep for a long while to make sure gazebo is started.
00039     if(testing)
00040     {
00041       ROS_INFO("This is a test: sleeping 10 seconds for Gazebo to start.");
00042       sleep(20.0);
00043     }
00044 
00045     if( hand_commander != NULL )
00046       hand_commander_.reset(hand_commander);
00047     else
00048       hand_commander_.reset(new HandCommander());
00049 
00050     //if using the HandCommander, we're initialising the
00051     // vector of joints to send here, out of the loop.
00052     sr_robot_msgs::joint joint;
00053     joint.joint_name = joint_name_;
00054     joint_vector_.push_back(joint);
00055 
00056     std::pair<double, double> min_max = hand_commander_->get_min_max(joint_name_);
00057     min = min_max.first;
00058     max = min_max.second;
00059 
00060     std::string input = hand_commander_->get_controller_state_topic(joint_name_);
00061     subscribe_and_default_pub_(input);
00062   }
00063 
00064   MovementPublisher::MovementPublisher(double min_value, double max_value,
00065                                        double rate, unsigned int repetition,
00066                                        unsigned int nb_mvt_step, std::string controller_type)
00067     : nh_tilde("~"), publishing_rate( rate ), repetition(repetition),
00068       min(min_value), max(max_value), last_target_(0.0), nb_mvt_step(nb_mvt_step),
00069       SError_(0.0), MSError_(0.0), n_samples_(0), controller_type(controller_type)
00070   {
00071     pub = nh_tilde.advertise<std_msgs::Float64>("targets", 5);
00072 
00073     subscribe_and_default_pub_("inputs");
00074   }
00075 
00076   void MovementPublisher::subscribe_and_default_pub_(std::string input)
00077   {
00078     pub_mse_ = nh_tilde.advertise<std_msgs::Float64>("mse_out", 5);
00079 
00080     if(controller_type.compare("sr")==0)
00081     {
00082       //nb_mvt_step is used to set the size of the buffer
00083       sub_ = nh_tilde.subscribe(input, nb_mvt_step, &MovementPublisher::calculateErrorCallback, this);
00084     }
00085     else if(controller_type.compare("pr2")==0)
00086     {
00087       sub_ = nh_tilde.subscribe(input, nb_mvt_step, &MovementPublisher::pr2_calculateErrorCallback, this);
00088     }
00089     else
00090     {
00091       ROS_WARN_STREAM("Warning: You didn't choose a msg_type to listen. sr_robot_msgs was chosen by default");
00092       //nb_mvt_step is used to set the size of the buffer
00093       sub_ = nh_tilde.subscribe(input, nb_mvt_step, &MovementPublisher::calculateErrorCallback, this);
00094     }
00095   }
00096 
00097   MovementPublisher::~MovementPublisher()
00098   {}
00099 
00100   void MovementPublisher::start()
00101   {
00102     double last_target = 0.0;
00103 
00104     for(unsigned int i_rep = 0; i_rep < repetition; ++i_rep)
00105     {
00106       for( unsigned int i=0; i<partial_movements.size(); ++i)
00107       {
00108         for(unsigned int j=0; j<nb_mvt_step; ++j)
00109         {
00110           if( !ros::ok() )
00111             return;
00112 
00113           //get the target
00114           msg.data = partial_movements[i].get_target( static_cast<double>(j) / static_cast<double>(nb_mvt_step));
00115 
00116           //there was not target -> resend the last target
00117           if( msg.data == -1.0 )
00118           {
00119             msg.data = last_target;
00120           }
00121           else
00122           {
00123             //interpolate to the correct range
00124             msg.data = min + msg.data * (max - min);
00125           }
00126           //publish the message
00127           publish_();
00128 
00129           //wait for a bit
00130           publishing_rate.sleep();
00131 
00132           ros::spinOnce();
00133 
00134           last_target = msg.data;
00135         }
00136       }
00137       //print the error information
00138       ROS_INFO_STREAM("MSE: " << MSError_ << " Error(deg): " << sr_math_utils::to_degrees( sqrt(MSError_) ) );
00139 
00140       //publish the error information
00141       msg.data = MSError_;
00142       pub_mse_.publish( msg );
00143 
00144       //Reset the error counter
00145       SError_ = 0.0;
00146       n_samples_ = 0;
00147     }
00148   }
00149 
00150   void MovementPublisher::calculateErrorCallback(const sr_robot_msgs::JointControllerState::ConstPtr& msg)
00151   {
00152     double error = msg->set_point - msg->process_value;
00153     ROS_DEBUG_STREAM("Error: " << error);
00154     SError_ = SError_ + ( error * error );
00155     ROS_DEBUG_STREAM("SError: " << SError_);
00156     n_samples_++;
00157     ROS_DEBUG_STREAM("Samples: " << n_samples_);
00158     MSError_ = SError_ / ( static_cast<double>(n_samples_) );
00159     ROS_DEBUG_STREAM("MSe: " << MSError_);
00160   }
00161 
00162   void MovementPublisher::pr2_calculateErrorCallback(const pr2_controllers_msgs::JointControllerState::ConstPtr& msg)
00163   {
00164     double error = msg->set_point - msg->process_value;
00165     ROS_DEBUG_STREAM("Error: " << error);
00166     SError_ = SError_ + ( error * error );
00167     ROS_DEBUG_STREAM("SError: " << SError_);
00168     n_samples_++;
00169     ROS_DEBUG_STREAM("Samples: " << n_samples_);
00170     MSError_ = SError_ / ( static_cast<double>(n_samples_) );
00171     ROS_DEBUG_STREAM("MSe: " << MSError_);
00172   }
00173 
00174   void MovementPublisher::execute_step(int index_mvt_step, int index_partial_movement)
00175   {
00176     if( !ros::ok() )
00177       return;
00178 
00179     //get the target
00180     msg.data = partial_movements[index_partial_movement].get_target( static_cast<double>(index_mvt_step) / static_cast<double>(nb_mvt_step));
00181     //interpolate to the correct range
00182     msg.data = min + msg.data * (max - min);
00183 
00184     //there was not target -> resend the last target
00185     if( msg.data == -1.0 )
00186       msg.data = last_target_;
00187 
00188     publish_();
00189 
00190     //wait for a bit
00191     publishing_rate.sleep();
00192 
00193     last_target_ = msg.data;
00194   }
00195 
00196   void MovementPublisher::publish_()
00197   {
00198     //publish the message
00199     //use the default publisher if the HandCommander is not instantiated
00200     // (for compatibility reason)
00201     if( hand_commander_ == NULL )
00202       pub.publish( msg );
00203     //otherwise use the HandCommander
00204     else
00205     {
00206       joint_vector_[0].joint_target = sr_math_utils::to_degrees(msg.data);
00207       hand_commander_->sendCommands(joint_vector_);
00208     }
00209   }
00210 
00211   void MovementPublisher::stop()
00212   {}
00213 
00214   void MovementPublisher::add_movement(PartialMovement mvt)
00215   {
00216     partial_movements.push_back( mvt );
00217   }
00218 
00219   void MovementPublisher::set_publisher(ros::Publisher publisher)
00220   {
00221     pub = publisher;
00222   }
00223 
00224   void MovementPublisher::set_subscriber(ros::Subscriber subscriber)
00225   {
00226     sub_ = subscriber;
00227   }
00228 
00229   std::string MovementPublisher::get_subscriber_topic()
00230   {
00231     return hand_commander_->get_controller_state_topic(joint_name_);
00232   }
00233 }
00234 
00235 /* For the emacs weenies in the crowd.
00236 Local Variables:
00237    c-basic-offset: 2
00238 End:
00239 */


sr_movements
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:09:30