Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00024
00025
00026 #include <string>
00027 #include <ros/ros.h>
00028 #include <ros/console.h>
00029 #include <boost/lexical_cast.hpp>
00030
00031 #include "sr_ronex_msgs/PWM.h"
00032 #include "sr_ronex_utilities/sr_ronex_utilities.hpp"
00033
00034
00035
00039 class SrRonexFindGeneralIOModule
00040 {
00041 public:
00042 SrRonexFindGeneralIOModule() {}
00043 ~SrRonexFindGeneralIOModule() {}
00044
00045 public:
00053 bool get_path_(const unsigned int& ronex_id_as_int, std::string& path)
00054 {
00055 std::string ronex_id = this->to_string_(ronex_id_as_int);
00056
00057
00058 ros::Rate loop_rate(10);
00059 std::string param;
00060 while ( ros::param::get("/ronex/devices/0/ronex_id", param ) == false )
00061 {
00062 ROS_INFO_STREAM("Waiting for General I/O module to be loaded properly.\n");
00063 loop_rate.sleep();
00064 }
00065
00066
00067
00068 int ronex_parameter_id = ronex::get_ronex_param_id(ronex_id);
00069 if ( ronex_parameter_id == -1 )
00070 {
00071 ROS_ERROR_STREAM("Did not find the General I/O module with ronex_id " << ronex_id << ".\n");
00072 return false;
00073 }
00074
00075
00076
00077
00078 std::string path_key = ronex::get_ronex_devices_string(ronex_parameter_id, std::string("path"));
00079 ros::param::get(path_key, path);
00080
00081 return true;
00082 }
00083
00084 private:
00091 std::string to_string_(unsigned int d)
00092 {
00093 return boost::lexical_cast<std::string>(d);
00094 }
00095 };
00096
00097
00098
00105 void flash_LED(ros::NodeHandle& n, const std::string& topic)
00106 {
00107 ros::Publisher pub = n.advertise<sr_ronex_msgs::PWM>(topic, 1000);
00108
00109 uint16_t pwm_period = 320;
00110
00111 uint16_t pwm_on_time_0 = pwm_period;
00112
00113 uint16_t pwm_on_time_1 = 0;
00114
00115 ros::Rate loop_rate(100);
00116 while ( ros::ok() )
00117 {
00118
00119 pwm_on_time_0 -= 10;
00120 if (pwm_on_time_0 == 0 || pwm_on_time_0 > pwm_period)
00121 pwm_on_time_0 = pwm_period;
00122
00123 sr_ronex_msgs::PWM msg;
00124 msg.pwm_period = pwm_period;
00125 msg.pwm_on_time_0 = pwm_on_time_0;
00126 msg.pwm_on_time_1 = pwm_on_time_1;
00127
00128
00129
00130 pub.publish(msg);
00131
00132 ros::spinOnce();
00133 loop_rate.sleep();
00134 }
00135 }
00136
00137
00138
00144 int main(int argc, char **argv)
00145 {
00146
00147 ros::init(argc, argv, "sr_ronex_flash_LED_with_PWM");
00148
00149
00150 ros::NodeHandle n;
00151
00152 std::cout <<
00153 "Please configure D0 pin of your ronex module as output (you can use plugin Dynamic Reconfigure in rqt): \n";
00154
00155
00156
00157
00158 std::string input = "";
00159 unsigned int ronex_id;
00160 std::cout << "Please enter the ronex id: ";
00161 std::getline(std::cin, input);
00162 ronex_id = boost::lexical_cast<unsigned int>(input);
00163 std::string path;
00164 SrRonexFindGeneralIOModule findModule;
00165 if ( findModule.get_path_( ronex_id, path ) )
00166 {
00167
00168
00169 std::string topic = path + "/command/pwm/0";
00170 ROS_INFO_STREAM("Topic = " << topic << "\n");
00171 flash_LED(n, topic);
00172 }
00173
00174 return 0;
00175 }
00176
00177