#include <stdint.h> #include <ros/ros.h> #include “cosmotechs_driver/pcio32ha.h” #include “cosmotechs_driver/SetPort.h”
// Board ID static uint8_t board_id;
bool SetPort(cosmotechs_driver::SetPort::Request &req, cosmotechs_driver::SetPort::Request &res) { ROS_INFO(“request: port_id=%d, data=%x”, (int)req.port_id, (int)req.data); //Pcio32hwOutPort(board_id, req.port_id, req.data); return true; }
/**
- @brief main function of the ROS node
*/ int main(int argc, char **argv) { ros::init(argc, argv, “pcio32ha_node”); ros::NodeHandle n;
#if 0 if(Pcio32hwCreate(board_id)==-1){ printf(“Pcio32hwCreate: ERROR\n”); return -1; } #endif
ros::ServiceServer service = n.advertiseService(“set_port”, SetPort); ros::spin();
//Pcio32hwClose();
return 0; }