ros_bridge_mbed.h

00001 #pragma once
00002 
00003 #include <ros.h>
00004 #include <std_msgs/Int8.h>
00005 #include <std_msgs/Bool.h>
00006 #include <std_msgs/Float32MultiArray.h>
00007 #include <sensor_msgs/JointState.h>
00008 #include <catchrobo_msgs/MyRosCmdArray.h>
00009 #include <catchrobo_msgs/EnableCmd.h>
00010 #include <catchrobo_msgs/ErrorCode.h>
00011 
00012 class RosBridge
00013 {
00014 public:
00015     RosBridge() : pub2ros_("joint_rad", new std_msgs::Float32MultiArray),
00016                   //                  pub_finished_flag_("finished_flag_topic", new std_msgs::Int8),
00017                   pub_error_("error", new catchrobo_msgs::ErrorCode),
00018                   //                  sub_from_ros_("my_joint_control", &RosBridge::rosCallback, this),
00019                   sub_ros_cmd_("ros_cmd", &RosBridge::rosCallback2, this),
00020                   sub_enable_("enable_cmd", &RosBridge::enableCallback, this),
00021                   led_(LED3){};
00022 
00023     void init(int ros_baudrate, void (*callback_function)(const catchrobo_msgs::MyRosCmd &command),
00024               void (*enable_callback_function)(const catchrobo_msgs::EnableCmd &input))
00025 
00026     {
00027         callback_function_ = callback_function;
00028         enable_callback_function_ = enable_callback_function;
00029         nh_.getHardware()->setBaud(ros_baudrate);
00030         wait(0.5);
00031         nh_.initNode();
00032         wait(0.5);
00033         nh_.advertise(pub2ros_);
00034         wait(0.5);
00035         nh_.advertise(pub_error_);
00036         wait(0.5);
00037         nh_.subscribe(sub_enable_);
00038         wait(0.5);
00039         nh_.subscribe(sub_ros_cmd_);
00040     };
00041     void publishJointState(const std_msgs::Float32MultiArray &joint_state)
00042     {
00043         pub2ros_.publish(&joint_state);
00044     };
00045 
00046     void spin()
00047     {
00048         while (1)
00049         {
00050             spinOnce();
00051         }
00052     };
00053     void publishError(const catchrobo_msgs::ErrorCode &msg)
00054     {
00055         pub_error_.publish(&msg);
00056     }
00057 
00058     void spinOnce()
00059     {
00060         nh_.spinOnce();
00061         led_ = !led_;
00062     }
00063 
00064 private:
00065     ros::NodeHandle nh_;
00066     ros::Publisher pub2ros_;
00067     ros::Publisher pub_error_;
00068 
00069     ros::Subscriber<catchrobo_msgs::MyRosCmd, RosBridge> sub_ros_cmd_;
00070     ros::Subscriber<catchrobo_msgs::EnableCmd, RosBridge> sub_enable_;
00071 
00072     DigitalOut led_;
00073 
00074     void (*callback_function_)(const catchrobo_msgs::MyRosCmd &command);
00075     void (*enable_callback_function_)(const catchrobo_msgs::EnableCmd &input);
00076 
00077     void rosCallback(const catchrobo_msgs::MyRosCmdArray &input)
00078     {
00079         for (int i = 0; i < input.command_array_length; i++)
00080         {
00081             (*callback_function_)(input.command_array[i]);
00082         }
00083     };
00084     void rosCallback2(const catchrobo_msgs::MyRosCmd &input)
00085     {
00086         (*callback_function_)(input);
00087     };
00088     void enableCallback(const catchrobo_msgs::EnableCmd &input)
00089     {
00090         (*enable_callback_function_)(input);
00091     }
00092 };