16 #include <modelica_bridge/ModComm.h> 18 #include <sys/types.h> 19 #include <sys/socket.h> 23 #include <netinet/in.h> 36 #define MAX_BUF 1024 // define maximum length of character buffer 37 #define MAX_ARRAY 256 // define maximum size of internal buffer arrays 97 void error(
const char *msg);
121 pub_ =
nh_.advertise<modelica_bridge::ModComm>(
"model_values", 1);
123 ros::NodeHandle nh_param(
"~");
124 nh_param.param<
int>(
"port_num",
port_num_, 9091);
128 if (socket_fd_ < 0) {
129 error(
"ERROR opening socket");
138 error(
"ERROR on binding");
148 modelica_bridge::ModComm outVal;
155 error(
"ERROR on accept");
160 error(
"ERROR reading from socket");
168 error(
"ERROR writing to socket");
175 for (
int rosI = 1; rosI <= outVal.size; rosI ++) {
178 pub_.publish(outVal);
189 for(i = 0; i < inVal->size; i++) {
198 const char * delim =
",";
200 token[0] = strtok(
buffer_, delim);
201 while (token[i] != NULL) {
203 token[i] = strtok(NULL, delim);
206 for (j=0; j<=i-1; j++) {
215 for (
int i = 0; i<
sizeof(*socket_buffer_); i++)
217 sprintf(s1,
"%f,",socket_buffer_[i]);
228 int main(
int argc,
char** argv)
230 ros::init(argc, argv,
"modros_node");
int new_socket_fd_
Modelica socket binding.
double ros_buffer_[MAX_ARRAY]
buffer array for transmission via ROS
int main(int argc, char **argv)
#define MAX_BUF
Defines the maximum length of the character buffer.
char buffer_[MAX_BUF]
string buffer used to transmit information over the socket
int update_rate_
ROS node maximum loop rate.
ros::Publisher pub_
publisher for model feedback values
ros::NodeHandle nh_
ROS node handle.
int error_check_
Parameter for checking errors on socket interaction.
struct sockaddr_in serv_addr
ROS socket address.
#define MAX_ARRAY
Defines the maximum size of internal buffer arrays.
socklen_t client_len_
Modelica socket size.
void controllerCallback(const modelica_bridge::ModComm::ConstPtr &inVal)
struct sockaddr_in client_address_
Modelica socket address.
int socket_fd_
ROS socket binding.
double socket_buffer_[MAX_ARRAY]
buffer array for interaction with socket
void error(const char *msg)
int port_num_
Port number.
ros::Subscriber sub_
subscriber to ROS controller values