diff --git a/lib/ros_lib/march_shared_resources/Alive.h b/lib/ros_lib/march_shared_resources/Alive.h new file mode 100644 index 0000000..511fb9c --- /dev/null +++ b/lib/ros_lib/march_shared_resources/Alive.h @@ -0,0 +1,79 @@ +#ifndef _ROS_march_shared_resources_Alive_h +#define _ROS_march_shared_resources_Alive_h + +#include +#include +#include +#include "ros/msg.h" +#include "ros/time.h" + +namespace march_shared_resources +{ + + class Alive : public ros::Msg + { + public: + typedef ros::Time _stamp_type; + _stamp_type stamp; + typedef const char* _id_type; + _id_type id; + + Alive(): + stamp(), + id("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + *(outbuffer + offset + 0) = (this->stamp.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->stamp.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->stamp.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->stamp.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->stamp.sec); + *(outbuffer + offset + 0) = (this->stamp.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->stamp.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->stamp.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->stamp.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->stamp.nsec); + uint32_t length_id = strlen(this->id); + varToArr(outbuffer + offset, length_id); + offset += 4; + memcpy(outbuffer + offset, this->id, length_id); + offset += length_id; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + this->stamp.sec = ((uint32_t) (*(inbuffer + offset))); + this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->stamp.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->stamp.sec); + this->stamp.nsec = ((uint32_t) (*(inbuffer + offset))); + this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->stamp.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->stamp.nsec); + uint32_t length_id; + arrToVar(length_id, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_id; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_id-1]=0; + this->id = (char *)(inbuffer + offset-1); + offset += length_id; + return offset; + } + + const char * getType(){ return "march_shared_resources/Alive"; }; + const char * getMD5(){ return "302881f31927c1df708a2dbab0e80ee8"; }; + + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/ContainsGait.h b/lib/ros_lib/march_shared_resources/ContainsGait.h new file mode 100644 index 0000000..84a3c46 --- /dev/null +++ b/lib/ros_lib/march_shared_resources/ContainsGait.h @@ -0,0 +1,142 @@ +#ifndef _ROS_SERVICE_ContainsGait_h +#define _ROS_SERVICE_ContainsGait_h +#include +#include +#include +#include "ros/msg.h" + +namespace march_shared_resources +{ + +static const char CONTAINSGAIT[] = "march_shared_resources/ContainsGait"; + + class ContainsGaitRequest : public ros::Msg + { + public: + typedef const char* _gait_type; + _gait_type gait; + uint32_t subgaits_length; + typedef char* _subgaits_type; + _subgaits_type st_subgaits; + _subgaits_type * subgaits; + + ContainsGaitRequest(): + gait(""), + subgaits_length(0), subgaits(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_gait = strlen(this->gait); + varToArr(outbuffer + offset, length_gait); + offset += 4; + memcpy(outbuffer + offset, this->gait, length_gait); + offset += length_gait; + *(outbuffer + offset + 0) = (this->subgaits_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->subgaits_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->subgaits_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->subgaits_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->subgaits_length); + for( uint32_t i = 0; i < subgaits_length; i++){ + uint32_t length_subgaitsi = strlen(this->subgaits[i]); + varToArr(outbuffer + offset, length_subgaitsi); + offset += 4; + memcpy(outbuffer + offset, this->subgaits[i], length_subgaitsi); + offset += length_subgaitsi; + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_gait; + arrToVar(length_gait, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_gait; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_gait-1]=0; + this->gait = (char *)(inbuffer + offset-1); + offset += length_gait; + uint32_t subgaits_lengthT = ((uint32_t) (*(inbuffer + offset))); + subgaits_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + subgaits_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + subgaits_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->subgaits_length); + if(subgaits_lengthT > subgaits_length) + this->subgaits = (char**)realloc(this->subgaits, subgaits_lengthT * sizeof(char*)); + subgaits_length = subgaits_lengthT; + for( uint32_t i = 0; i < subgaits_length; i++){ + uint32_t length_st_subgaits; + arrToVar(length_st_subgaits, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_subgaits; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_subgaits-1]=0; + this->st_subgaits = (char *)(inbuffer + offset-1); + offset += length_st_subgaits; + memcpy( &(this->subgaits[i]), &(this->st_subgaits), sizeof(char*)); + } + return offset; + } + + const char * getType(){ return CONTAINSGAIT; }; + const char * getMD5(){ return "c234d34b1f77b3f85ab7f59d681e4aab"; }; + + }; + + class ContainsGaitResponse : public ros::Msg + { + public: + typedef bool _contains_type; + _contains_type contains; + + ContainsGaitResponse(): + contains(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_contains; + u_contains.real = this->contains; + *(outbuffer + offset + 0) = (u_contains.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->contains); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_contains; + u_contains.base = 0; + u_contains.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->contains = u_contains.real; + offset += sizeof(this->contains); + return offset; + } + + const char * getType(){ return CONTAINSGAIT; }; + const char * getMD5(){ return "5865510046ec078baae5a62527defb32"; }; + + }; + + class ContainsGait { + public: + typedef ContainsGaitRequest Request; + typedef ContainsGaitResponse Response; + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/CurrentGait.h b/lib/ros_lib/march_shared_resources/CurrentGait.h new file mode 100644 index 0000000..16d2a75 --- /dev/null +++ b/lib/ros_lib/march_shared_resources/CurrentGait.h @@ -0,0 +1,136 @@ +#ifndef _ROS_march_shared_resources_CurrentGait_h +#define _ROS_march_shared_resources_CurrentGait_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "ros/duration.h" + +namespace march_shared_resources +{ + + class CurrentGait : public ros::Msg + { + public: + typedef std_msgs::Header _header_type; + _header_type header; + typedef const char* _gait_type; + _gait_type gait; + typedef const char* _subgait_type; + _subgait_type subgait; + typedef const char* _version_type; + _version_type version; + typedef ros::Duration _duration_type; + _duration_type duration; + typedef const char* _gait_type_type; + _gait_type_type gait_type; + + CurrentGait(): + header(), + gait(""), + subgait(""), + version(""), + duration(), + gait_type("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + uint32_t length_gait = strlen(this->gait); + varToArr(outbuffer + offset, length_gait); + offset += 4; + memcpy(outbuffer + offset, this->gait, length_gait); + offset += length_gait; + uint32_t length_subgait = strlen(this->subgait); + varToArr(outbuffer + offset, length_subgait); + offset += 4; + memcpy(outbuffer + offset, this->subgait, length_subgait); + offset += length_subgait; + uint32_t length_version = strlen(this->version); + varToArr(outbuffer + offset, length_version); + offset += 4; + memcpy(outbuffer + offset, this->version, length_version); + offset += length_version; + *(outbuffer + offset + 0) = (this->duration.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->duration.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->duration.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->duration.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->duration.sec); + *(outbuffer + offset + 0) = (this->duration.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->duration.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->duration.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->duration.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->duration.nsec); + uint32_t length_gait_type = strlen(this->gait_type); + varToArr(outbuffer + offset, length_gait_type); + offset += 4; + memcpy(outbuffer + offset, this->gait_type, length_gait_type); + offset += length_gait_type; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t length_gait; + arrToVar(length_gait, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_gait; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_gait-1]=0; + this->gait = (char *)(inbuffer + offset-1); + offset += length_gait; + uint32_t length_subgait; + arrToVar(length_subgait, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_subgait; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_subgait-1]=0; + this->subgait = (char *)(inbuffer + offset-1); + offset += length_subgait; + uint32_t length_version; + arrToVar(length_version, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_version; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_version-1]=0; + this->version = (char *)(inbuffer + offset-1); + offset += length_version; + this->duration.sec = ((uint32_t) (*(inbuffer + offset))); + this->duration.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->duration.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->duration.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->duration.sec); + this->duration.nsec = ((uint32_t) (*(inbuffer + offset))); + this->duration.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->duration.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->duration.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->duration.nsec); + uint32_t length_gait_type; + arrToVar(length_gait_type, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_gait_type; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_gait_type-1]=0; + this->gait_type = (char *)(inbuffer + offset-1); + offset += length_gait_type; + return offset; + } + + const char * getType(){ return "march_shared_resources/CurrentGait"; }; + const char * getMD5(){ return "05545223cf87f9c70c68a04b825e8621"; }; + + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/CurrentState.h b/lib/ros_lib/march_shared_resources/CurrentState.h new file mode 100644 index 0000000..9f1119c --- /dev/null +++ b/lib/ros_lib/march_shared_resources/CurrentState.h @@ -0,0 +1,81 @@ +#ifndef _ROS_march_shared_resources_CurrentState_h +#define _ROS_march_shared_resources_CurrentState_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace march_shared_resources +{ + + class CurrentState : public ros::Msg + { + public: + typedef std_msgs::Header _header_type; + _header_type header; + typedef const char* _state_type; + _state_type state; + typedef int8_t _state_type_type; + _state_type_type state_type; + enum { IDLE = 0 }; + enum { GAIT = 1 }; + + CurrentState(): + header(), + state(""), + state_type(0) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + uint32_t length_state = strlen(this->state); + varToArr(outbuffer + offset, length_state); + offset += 4; + memcpy(outbuffer + offset, this->state, length_state); + offset += length_state; + union { + int8_t real; + uint8_t base; + } u_state_type; + u_state_type.real = this->state_type; + *(outbuffer + offset + 0) = (u_state_type.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->state_type); + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t length_state; + arrToVar(length_state, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_state; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_state-1]=0; + this->state = (char *)(inbuffer + offset-1); + offset += length_state; + union { + int8_t real; + uint8_t base; + } u_state_type; + u_state_type.base = 0; + u_state_type.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->state_type = u_state_type.real; + offset += sizeof(this->state_type); + return offset; + } + + const char * getType(){ return "march_shared_resources/CurrentState"; }; + const char * getMD5(){ return "0fde5ff34496644ac8d85338bbbcbdcd"; }; + + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/GaitAction.h b/lib/ros_lib/march_shared_resources/GaitAction.h deleted file mode 100644 index 98dab1c..0000000 --- a/lib/ros_lib/march_shared_resources/GaitAction.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitAction_h -#define _ROS_march_shared_resources_GaitAction_h - -#include -#include -#include -#include "ros/msg.h" -#include "march_shared_resources/GaitActionGoal.h" -#include "march_shared_resources/GaitActionResult.h" -#include "march_shared_resources/GaitActionFeedback.h" - -namespace march_shared_resources -{ - - class GaitAction : public ros::Msg - { - public: - typedef march_shared_resources::GaitActionGoal _action_goal_type; - _action_goal_type action_goal; - typedef march_shared_resources::GaitActionResult _action_result_type; - _action_result_type action_result; - typedef march_shared_resources::GaitActionFeedback _action_feedback_type; - _action_feedback_type action_feedback; - - GaitAction(): - action_goal(), - action_result(), - action_feedback() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->action_goal.serialize(outbuffer + offset); - offset += this->action_result.serialize(outbuffer + offset); - offset += this->action_feedback.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->action_goal.deserialize(inbuffer + offset); - offset += this->action_result.deserialize(inbuffer + offset); - offset += this->action_feedback.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitAction"; }; - const char * getMD5(){ return "07f5ae1dc17f9138b251fe67b0edca5f"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitActionFeedback.h b/lib/ros_lib/march_shared_resources/GaitActionFeedback.h deleted file mode 100644 index fec97e6..0000000 --- a/lib/ros_lib/march_shared_resources/GaitActionFeedback.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitActionFeedback_h -#define _ROS_march_shared_resources_GaitActionFeedback_h - -#include -#include -#include -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "actionlib_msgs/GoalStatus.h" -#include "march_shared_resources/GaitFeedback.h" - -namespace march_shared_resources -{ - - class GaitActionFeedback : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef actionlib_msgs::GoalStatus _status_type; - _status_type status; - typedef march_shared_resources::GaitFeedback _feedback_type; - _feedback_type feedback; - - GaitActionFeedback(): - header(), - status(), - feedback() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->status.serialize(outbuffer + offset); - offset += this->feedback.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->status.deserialize(inbuffer + offset); - offset += this->feedback.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitActionFeedback"; }; - const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitActionGoal.h b/lib/ros_lib/march_shared_resources/GaitActionGoal.h deleted file mode 100644 index 1c36d26..0000000 --- a/lib/ros_lib/march_shared_resources/GaitActionGoal.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitActionGoal_h -#define _ROS_march_shared_resources_GaitActionGoal_h - -#include -#include -#include -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "actionlib_msgs/GoalID.h" -#include "march_shared_resources/GaitGoal.h" - -namespace march_shared_resources -{ - - class GaitActionGoal : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef actionlib_msgs::GoalID _goal_id_type; - _goal_id_type goal_id; - typedef march_shared_resources::GaitGoal _goal_type; - _goal_type goal; - - GaitActionGoal(): - header(), - goal_id(), - goal() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->goal_id.serialize(outbuffer + offset); - offset += this->goal.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->goal_id.deserialize(inbuffer + offset); - offset += this->goal.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitActionGoal"; }; - const char * getMD5(){ return "44a947358a5cd53af8f616bb03543558"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitActionResult.h b/lib/ros_lib/march_shared_resources/GaitActionResult.h deleted file mode 100644 index 5009639..0000000 --- a/lib/ros_lib/march_shared_resources/GaitActionResult.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitActionResult_h -#define _ROS_march_shared_resources_GaitActionResult_h - -#include -#include -#include -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "actionlib_msgs/GoalStatus.h" -#include "march_shared_resources/GaitResult.h" - -namespace march_shared_resources -{ - - class GaitActionResult : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef actionlib_msgs::GoalStatus _status_type; - _status_type status; - typedef march_shared_resources::GaitResult _result_type; - _result_type result; - - GaitActionResult(): - header(), - status(), - result() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->status.serialize(outbuffer + offset); - offset += this->result.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->status.deserialize(inbuffer + offset); - offset += this->result.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitActionResult"; }; - const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitFeedback.h b/lib/ros_lib/march_shared_resources/GaitFeedback.h deleted file mode 100644 index 7251fb7..0000000 --- a/lib/ros_lib/march_shared_resources/GaitFeedback.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitFeedback_h -#define _ROS_march_shared_resources_GaitFeedback_h - -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - - class GaitFeedback : public ros::Msg - { - public: - - GaitFeedback() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitFeedback"; }; - const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitGoal.h b/lib/ros_lib/march_shared_resources/GaitGoal.h deleted file mode 100644 index 9e7fae3..0000000 --- a/lib/ros_lib/march_shared_resources/GaitGoal.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitGoal_h -#define _ROS_march_shared_resources_GaitGoal_h - -#include -#include -#include -#include "ros/msg.h" -#include "march_shared_resources/Graph.h" -#include "march_shared_resources/Subgait.h" - -namespace march_shared_resources -{ - - class GaitGoal : public ros::Msg - { - public: - typedef const char* _name_type; - _name_type name; - typedef march_shared_resources::Graph _graph_type; - _graph_type graph; - typedef march_shared_resources::Subgait _current_subgait_type; - _current_subgait_type current_subgait; - - GaitGoal(): - name(""), - graph(), - current_subgait() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - uint32_t length_name = strlen(this->name); - varToArr(outbuffer + offset, length_name); - offset += 4; - memcpy(outbuffer + offset, this->name, length_name); - offset += length_name; - offset += this->graph.serialize(outbuffer + offset); - offset += this->current_subgait.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t length_name; - arrToVar(length_name, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_name; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_name-1]=0; - this->name = (char *)(inbuffer + offset-1); - offset += length_name; - offset += this->graph.deserialize(inbuffer + offset); - offset += this->current_subgait.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitGoal"; }; - const char * getMD5(){ return "652841bb01d73666e6e5775b958bca6c"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitInstruction.h b/lib/ros_lib/march_shared_resources/GaitInstruction.h index ff152b5..0fe6a3f 100644 --- a/lib/ros_lib/march_shared_resources/GaitInstruction.h +++ b/lib/ros_lib/march_shared_resources/GaitInstruction.h @@ -19,6 +19,8 @@ namespace march_shared_resources _type_type type; typedef const char* _gait_name_type; _gait_name_type gait_name; + typedef const char* _id_type; + _id_type id; enum { UNKNOWN = -1 }; enum { STOP = 0 }; enum { GAIT = 1 }; @@ -30,7 +32,8 @@ namespace march_shared_resources GaitInstruction(): header(), type(0), - gait_name("") + gait_name(""), + id("") { } @@ -50,6 +53,11 @@ namespace march_shared_resources offset += 4; memcpy(outbuffer + offset, this->gait_name, length_gait_name); offset += length_gait_name; + uint32_t length_id = strlen(this->id); + varToArr(outbuffer + offset, length_id); + offset += 4; + memcpy(outbuffer + offset, this->id, length_id); + offset += length_id; return offset; } @@ -74,11 +82,20 @@ namespace march_shared_resources inbuffer[offset+length_gait_name-1]=0; this->gait_name = (char *)(inbuffer + offset-1); offset += length_gait_name; + uint32_t length_id; + arrToVar(length_id, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_id; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_id-1]=0; + this->id = (char *)(inbuffer + offset-1); + offset += length_id; return offset; } const char * getType(){ return "march_shared_resources/GaitInstruction"; }; - const char * getMD5(){ return "c548bd1201c18293dcff8514a06fc253"; }; + const char * getMD5(){ return "5f6950a3a79440fadfc00e50e0e09c76"; }; }; diff --git a/lib/ros_lib/march_shared_resources/GaitNameAction.h b/lib/ros_lib/march_shared_resources/GaitNameAction.h deleted file mode 100644 index a08e8e0..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameAction.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameAction_h -#define _ROS_march_shared_resources_GaitNameAction_h - -#include -#include -#include -#include "ros/msg.h" -#include "march_shared_resources/GaitNameActionGoal.h" -#include "march_shared_resources/GaitNameActionResult.h" -#include "march_shared_resources/GaitNameActionFeedback.h" - -namespace march_shared_resources -{ - - class GaitNameAction : public ros::Msg - { - public: - typedef march_shared_resources::GaitNameActionGoal _action_goal_type; - _action_goal_type action_goal; - typedef march_shared_resources::GaitNameActionResult _action_result_type; - _action_result_type action_result; - typedef march_shared_resources::GaitNameActionFeedback _action_feedback_type; - _action_feedback_type action_feedback; - - GaitNameAction(): - action_goal(), - action_result(), - action_feedback() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->action_goal.serialize(outbuffer + offset); - offset += this->action_result.serialize(outbuffer + offset); - offset += this->action_feedback.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->action_goal.deserialize(inbuffer + offset); - offset += this->action_result.deserialize(inbuffer + offset); - offset += this->action_feedback.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameAction"; }; - const char * getMD5(){ return "3d9dbfcd7eb406596122704f6e675e77"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitNameActionFeedback.h b/lib/ros_lib/march_shared_resources/GaitNameActionFeedback.h deleted file mode 100644 index c6995d1..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameActionFeedback.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameActionFeedback_h -#define _ROS_march_shared_resources_GaitNameActionFeedback_h - -#include -#include -#include -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "actionlib_msgs/GoalStatus.h" -#include "march_shared_resources/GaitNameFeedback.h" - -namespace march_shared_resources -{ - - class GaitNameActionFeedback : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef actionlib_msgs::GoalStatus _status_type; - _status_type status; - typedef march_shared_resources::GaitNameFeedback _feedback_type; - _feedback_type feedback; - - GaitNameActionFeedback(): - header(), - status(), - feedback() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->status.serialize(outbuffer + offset); - offset += this->feedback.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->status.deserialize(inbuffer + offset); - offset += this->feedback.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameActionFeedback"; }; - const char * getMD5(){ return "aae20e09065c3809e8a8e87c4c8953fd"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitNameActionGoal.h b/lib/ros_lib/march_shared_resources/GaitNameActionGoal.h deleted file mode 100644 index ef6cd0f..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameActionGoal.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameActionGoal_h -#define _ROS_march_shared_resources_GaitNameActionGoal_h - -#include -#include -#include -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "actionlib_msgs/GoalID.h" -#include "march_shared_resources/GaitNameGoal.h" - -namespace march_shared_resources -{ - - class GaitNameActionGoal : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef actionlib_msgs::GoalID _goal_id_type; - _goal_id_type goal_id; - typedef march_shared_resources::GaitNameGoal _goal_type; - _goal_type goal; - - GaitNameActionGoal(): - header(), - goal_id(), - goal() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->goal_id.serialize(outbuffer + offset); - offset += this->goal.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->goal_id.deserialize(inbuffer + offset); - offset += this->goal.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameActionGoal"; }; - const char * getMD5(){ return "c3fcbf5324c7c2a9996a8ed899520f78"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitNameActionResult.h b/lib/ros_lib/march_shared_resources/GaitNameActionResult.h deleted file mode 100644 index b1269b9..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameActionResult.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameActionResult_h -#define _ROS_march_shared_resources_GaitNameActionResult_h - -#include -#include -#include -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "actionlib_msgs/GoalStatus.h" -#include "march_shared_resources/GaitNameResult.h" - -namespace march_shared_resources -{ - - class GaitNameActionResult : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef actionlib_msgs::GoalStatus _status_type; - _status_type status; - typedef march_shared_resources::GaitNameResult _result_type; - _result_type result; - - GaitNameActionResult(): - header(), - status(), - result() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->status.serialize(outbuffer + offset); - offset += this->result.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->status.deserialize(inbuffer + offset); - offset += this->result.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameActionResult"; }; - const char * getMD5(){ return "1eb06eeff08fa7ea874431638cb52332"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitNameFeedback.h b/lib/ros_lib/march_shared_resources/GaitNameFeedback.h deleted file mode 100644 index 4ad35a4..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameFeedback.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameFeedback_h -#define _ROS_march_shared_resources_GaitNameFeedback_h - -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - - class GaitNameFeedback : public ros::Msg - { - public: - - GaitNameFeedback() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameFeedback"; }; - const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitNameGoal.h b/lib/ros_lib/march_shared_resources/GaitNameGoal.h deleted file mode 100644 index 535d80f..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameGoal.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameGoal_h -#define _ROS_march_shared_resources_GaitNameGoal_h - -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - - class GaitNameGoal : public ros::Msg - { - public: - typedef const char* _name_type; - _name_type name; - typedef const char* _old_name_type; - _old_name_type old_name; - typedef const char* _subgait_name_type; - _subgait_name_type subgait_name; - - GaitNameGoal(): - name(""), - old_name(""), - subgait_name("") - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - uint32_t length_name = strlen(this->name); - varToArr(outbuffer + offset, length_name); - offset += 4; - memcpy(outbuffer + offset, this->name, length_name); - offset += length_name; - uint32_t length_old_name = strlen(this->old_name); - varToArr(outbuffer + offset, length_old_name); - offset += 4; - memcpy(outbuffer + offset, this->old_name, length_old_name); - offset += length_old_name; - uint32_t length_subgait_name = strlen(this->subgait_name); - varToArr(outbuffer + offset, length_subgait_name); - offset += 4; - memcpy(outbuffer + offset, this->subgait_name, length_subgait_name); - offset += length_subgait_name; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t length_name; - arrToVar(length_name, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_name; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_name-1]=0; - this->name = (char *)(inbuffer + offset-1); - offset += length_name; - uint32_t length_old_name; - arrToVar(length_old_name, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_old_name; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_old_name-1]=0; - this->old_name = (char *)(inbuffer + offset-1); - offset += length_old_name; - uint32_t length_subgait_name; - arrToVar(length_subgait_name, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_subgait_name; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_subgait_name-1]=0; - this->subgait_name = (char *)(inbuffer + offset-1); - offset += length_subgait_name; - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameGoal"; }; - const char * getMD5(){ return "80ea09d68b3242bfdc28492fd72d785d"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitNameResult.h b/lib/ros_lib/march_shared_resources/GaitNameResult.h deleted file mode 100644 index 19aaff2..0000000 --- a/lib/ros_lib/march_shared_resources/GaitNameResult.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitNameResult_h -#define _ROS_march_shared_resources_GaitNameResult_h - -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - - class GaitNameResult : public ros::Msg - { - public: - - GaitNameResult() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitNameResult"; }; - const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/GaitResult.h b/lib/ros_lib/march_shared_resources/GaitResult.h deleted file mode 100644 index bc638fb..0000000 --- a/lib/ros_lib/march_shared_resources/GaitResult.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef _ROS_march_shared_resources_GaitResult_h -#define _ROS_march_shared_resources_GaitResult_h - -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - - class GaitResult : public ros::Msg - { - public: - - GaitResult() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - return offset; - } - - const char * getType(){ return "march_shared_resources/GaitResult"; }; - const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/Graph.h b/lib/ros_lib/march_shared_resources/Graph.h deleted file mode 100644 index 6ec5322..0000000 --- a/lib/ros_lib/march_shared_resources/Graph.h +++ /dev/null @@ -1,112 +0,0 @@ -#ifndef _ROS_march_shared_resources_Graph_h -#define _ROS_march_shared_resources_Graph_h - -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - - class Graph : public ros::Msg - { - public: - uint32_t from_subgait_length; - typedef char* _from_subgait_type; - _from_subgait_type st_from_subgait; - _from_subgait_type * from_subgait; - uint32_t to_subgait_length; - typedef char* _to_subgait_type; - _to_subgait_type st_to_subgait; - _to_subgait_type * to_subgait; - - Graph(): - from_subgait_length(0), from_subgait(NULL), - to_subgait_length(0), to_subgait(NULL) - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - *(outbuffer + offset + 0) = (this->from_subgait_length >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->from_subgait_length >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->from_subgait_length >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->from_subgait_length >> (8 * 3)) & 0xFF; - offset += sizeof(this->from_subgait_length); - for( uint32_t i = 0; i < from_subgait_length; i++){ - uint32_t length_from_subgaiti = strlen(this->from_subgait[i]); - varToArr(outbuffer + offset, length_from_subgaiti); - offset += 4; - memcpy(outbuffer + offset, this->from_subgait[i], length_from_subgaiti); - offset += length_from_subgaiti; - } - *(outbuffer + offset + 0) = (this->to_subgait_length >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->to_subgait_length >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->to_subgait_length >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->to_subgait_length >> (8 * 3)) & 0xFF; - offset += sizeof(this->to_subgait_length); - for( uint32_t i = 0; i < to_subgait_length; i++){ - uint32_t length_to_subgaiti = strlen(this->to_subgait[i]); - varToArr(outbuffer + offset, length_to_subgaiti); - offset += 4; - memcpy(outbuffer + offset, this->to_subgait[i], length_to_subgaiti); - offset += length_to_subgaiti; - } - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t from_subgait_lengthT = ((uint32_t) (*(inbuffer + offset))); - from_subgait_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - from_subgait_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - from_subgait_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->from_subgait_length); - if(from_subgait_lengthT > from_subgait_length) - this->from_subgait = (char**)realloc(this->from_subgait, from_subgait_lengthT * sizeof(char*)); - from_subgait_length = from_subgait_lengthT; - for( uint32_t i = 0; i < from_subgait_length; i++){ - uint32_t length_st_from_subgait; - arrToVar(length_st_from_subgait, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_st_from_subgait; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_st_from_subgait-1]=0; - this->st_from_subgait = (char *)(inbuffer + offset-1); - offset += length_st_from_subgait; - memcpy( &(this->from_subgait[i]), &(this->st_from_subgait), sizeof(char*)); - } - uint32_t to_subgait_lengthT = ((uint32_t) (*(inbuffer + offset))); - to_subgait_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - to_subgait_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - to_subgait_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->to_subgait_length); - if(to_subgait_lengthT > to_subgait_length) - this->to_subgait = (char**)realloc(this->to_subgait, to_subgait_lengthT * sizeof(char*)); - to_subgait_length = to_subgait_lengthT; - for( uint32_t i = 0; i < to_subgait_length; i++){ - uint32_t length_st_to_subgait; - arrToVar(length_st_to_subgait, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_st_to_subgait; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_st_to_subgait-1]=0; - this->st_to_subgait = (char *)(inbuffer + offset-1); - offset += length_st_to_subgait; - memcpy( &(this->to_subgait[i]), &(this->st_to_subgait), sizeof(char*)); - } - return offset; - } - - const char * getType(){ return "march_shared_resources/Graph"; }; - const char * getMD5(){ return "2200ec54949ab38ab4e43720b46cd756"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/ImcState.h b/lib/ros_lib/march_shared_resources/ImcState.h new file mode 100644 index 0000000..ebfa780 --- /dev/null +++ b/lib/ros_lib/march_shared_resources/ImcState.h @@ -0,0 +1,611 @@ +#ifndef _ROS_march_shared_resources_ImcState_h +#define _ROS_march_shared_resources_ImcState_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" + +namespace march_shared_resources +{ + + class ImcState : public ros::Msg + { + public: + typedef std_msgs::Header _header_type; + _header_type header; + uint32_t joint_names_length; + typedef char* _joint_names_type; + _joint_names_type st_joint_names; + _joint_names_type * joint_names; + uint32_t status_word_length; + typedef char* _status_word_type; + _status_word_type st_status_word; + _status_word_type * status_word; + uint32_t detailed_error_length; + typedef char* _detailed_error_type; + _detailed_error_type st_detailed_error; + _detailed_error_type * detailed_error; + uint32_t motion_error_length; + typedef char* _motion_error_type; + _motion_error_type st_motion_error; + _motion_error_type * motion_error; + uint32_t state_length; + typedef char* _state_type; + _state_type st_state; + _state_type * state; + uint32_t detailed_error_description_length; + typedef char* _detailed_error_description_type; + _detailed_error_description_type st_detailed_error_description; + _detailed_error_description_type * detailed_error_description; + uint32_t motion_error_description_length; + typedef char* _motion_error_description_type; + _motion_error_description_type st_motion_error_description; + _motion_error_description_type * motion_error_description; + uint32_t motor_current_length; + typedef float _motor_current_type; + _motor_current_type st_motor_current; + _motor_current_type * motor_current; + uint32_t imc_voltage_length; + typedef float _imc_voltage_type; + _imc_voltage_type st_imc_voltage; + _imc_voltage_type * imc_voltage; + uint32_t motor_voltage_length; + typedef float _motor_voltage_type; + _motor_voltage_type st_motor_voltage; + _motor_voltage_type * motor_voltage; + uint32_t absolute_encoder_value_length; + typedef int32_t _absolute_encoder_value_type; + _absolute_encoder_value_type st_absolute_encoder_value; + _absolute_encoder_value_type * absolute_encoder_value; + uint32_t incremental_encoder_value_length; + typedef int32_t _incremental_encoder_value_type; + _incremental_encoder_value_type st_incremental_encoder_value; + _incremental_encoder_value_type * incremental_encoder_value; + uint32_t absolute_velocity_length; + typedef float _absolute_velocity_type; + _absolute_velocity_type st_absolute_velocity; + _absolute_velocity_type * absolute_velocity; + uint32_t incremental_velocity_length; + typedef float _incremental_velocity_type; + _incremental_velocity_type st_incremental_velocity; + _incremental_velocity_type * incremental_velocity; + + ImcState(): + header(), + joint_names_length(0), joint_names(NULL), + status_word_length(0), status_word(NULL), + detailed_error_length(0), detailed_error(NULL), + motion_error_length(0), motion_error(NULL), + state_length(0), state(NULL), + detailed_error_description_length(0), detailed_error_description(NULL), + motion_error_description_length(0), motion_error_description(NULL), + motor_current_length(0), motor_current(NULL), + imc_voltage_length(0), imc_voltage(NULL), + motor_voltage_length(0), motor_voltage(NULL), + absolute_encoder_value_length(0), absolute_encoder_value(NULL), + incremental_encoder_value_length(0), incremental_encoder_value(NULL), + absolute_velocity_length(0), absolute_velocity(NULL), + incremental_velocity_length(0), incremental_velocity(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->joint_names_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->joint_names_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->joint_names_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->joint_names_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->joint_names_length); + for( uint32_t i = 0; i < joint_names_length; i++){ + uint32_t length_joint_namesi = strlen(this->joint_names[i]); + varToArr(outbuffer + offset, length_joint_namesi); + offset += 4; + memcpy(outbuffer + offset, this->joint_names[i], length_joint_namesi); + offset += length_joint_namesi; + } + *(outbuffer + offset + 0) = (this->status_word_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->status_word_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->status_word_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->status_word_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->status_word_length); + for( uint32_t i = 0; i < status_word_length; i++){ + uint32_t length_status_wordi = strlen(this->status_word[i]); + varToArr(outbuffer + offset, length_status_wordi); + offset += 4; + memcpy(outbuffer + offset, this->status_word[i], length_status_wordi); + offset += length_status_wordi; + } + *(outbuffer + offset + 0) = (this->detailed_error_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->detailed_error_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->detailed_error_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->detailed_error_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->detailed_error_length); + for( uint32_t i = 0; i < detailed_error_length; i++){ + uint32_t length_detailed_errori = strlen(this->detailed_error[i]); + varToArr(outbuffer + offset, length_detailed_errori); + offset += 4; + memcpy(outbuffer + offset, this->detailed_error[i], length_detailed_errori); + offset += length_detailed_errori; + } + *(outbuffer + offset + 0) = (this->motion_error_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->motion_error_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->motion_error_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->motion_error_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->motion_error_length); + for( uint32_t i = 0; i < motion_error_length; i++){ + uint32_t length_motion_errori = strlen(this->motion_error[i]); + varToArr(outbuffer + offset, length_motion_errori); + offset += 4; + memcpy(outbuffer + offset, this->motion_error[i], length_motion_errori); + offset += length_motion_errori; + } + *(outbuffer + offset + 0) = (this->state_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->state_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->state_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->state_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->state_length); + for( uint32_t i = 0; i < state_length; i++){ + uint32_t length_statei = strlen(this->state[i]); + varToArr(outbuffer + offset, length_statei); + offset += 4; + memcpy(outbuffer + offset, this->state[i], length_statei); + offset += length_statei; + } + *(outbuffer + offset + 0) = (this->detailed_error_description_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->detailed_error_description_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->detailed_error_description_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->detailed_error_description_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->detailed_error_description_length); + for( uint32_t i = 0; i < detailed_error_description_length; i++){ + uint32_t length_detailed_error_descriptioni = strlen(this->detailed_error_description[i]); + varToArr(outbuffer + offset, length_detailed_error_descriptioni); + offset += 4; + memcpy(outbuffer + offset, this->detailed_error_description[i], length_detailed_error_descriptioni); + offset += length_detailed_error_descriptioni; + } + *(outbuffer + offset + 0) = (this->motion_error_description_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->motion_error_description_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->motion_error_description_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->motion_error_description_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->motion_error_description_length); + for( uint32_t i = 0; i < motion_error_description_length; i++){ + uint32_t length_motion_error_descriptioni = strlen(this->motion_error_description[i]); + varToArr(outbuffer + offset, length_motion_error_descriptioni); + offset += 4; + memcpy(outbuffer + offset, this->motion_error_description[i], length_motion_error_descriptioni); + offset += length_motion_error_descriptioni; + } + *(outbuffer + offset + 0) = (this->motor_current_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->motor_current_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->motor_current_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->motor_current_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->motor_current_length); + for( uint32_t i = 0; i < motor_current_length; i++){ + union { + float real; + uint32_t base; + } u_motor_currenti; + u_motor_currenti.real = this->motor_current[i]; + *(outbuffer + offset + 0) = (u_motor_currenti.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_motor_currenti.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_motor_currenti.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_motor_currenti.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->motor_current[i]); + } + *(outbuffer + offset + 0) = (this->imc_voltage_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->imc_voltage_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->imc_voltage_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->imc_voltage_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->imc_voltage_length); + for( uint32_t i = 0; i < imc_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_imc_voltagei; + u_imc_voltagei.real = this->imc_voltage[i]; + *(outbuffer + offset + 0) = (u_imc_voltagei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_imc_voltagei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_imc_voltagei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_imc_voltagei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->imc_voltage[i]); + } + *(outbuffer + offset + 0) = (this->motor_voltage_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->motor_voltage_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->motor_voltage_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->motor_voltage_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->motor_voltage_length); + for( uint32_t i = 0; i < motor_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_motor_voltagei; + u_motor_voltagei.real = this->motor_voltage[i]; + *(outbuffer + offset + 0) = (u_motor_voltagei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_motor_voltagei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_motor_voltagei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_motor_voltagei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->motor_voltage[i]); + } + *(outbuffer + offset + 0) = (this->absolute_encoder_value_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->absolute_encoder_value_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->absolute_encoder_value_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->absolute_encoder_value_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->absolute_encoder_value_length); + for( uint32_t i = 0; i < absolute_encoder_value_length; i++){ + union { + int32_t real; + uint32_t base; + } u_absolute_encoder_valuei; + u_absolute_encoder_valuei.real = this->absolute_encoder_value[i]; + *(outbuffer + offset + 0) = (u_absolute_encoder_valuei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_absolute_encoder_valuei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_absolute_encoder_valuei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_absolute_encoder_valuei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->absolute_encoder_value[i]); + } + *(outbuffer + offset + 0) = (this->incremental_encoder_value_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->incremental_encoder_value_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->incremental_encoder_value_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->incremental_encoder_value_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->incremental_encoder_value_length); + for( uint32_t i = 0; i < incremental_encoder_value_length; i++){ + union { + int32_t real; + uint32_t base; + } u_incremental_encoder_valuei; + u_incremental_encoder_valuei.real = this->incremental_encoder_value[i]; + *(outbuffer + offset + 0) = (u_incremental_encoder_valuei.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_incremental_encoder_valuei.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_incremental_encoder_valuei.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_incremental_encoder_valuei.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->incremental_encoder_value[i]); + } + *(outbuffer + offset + 0) = (this->absolute_velocity_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->absolute_velocity_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->absolute_velocity_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->absolute_velocity_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->absolute_velocity_length); + for( uint32_t i = 0; i < absolute_velocity_length; i++){ + union { + float real; + uint32_t base; + } u_absolute_velocityi; + u_absolute_velocityi.real = this->absolute_velocity[i]; + *(outbuffer + offset + 0) = (u_absolute_velocityi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_absolute_velocityi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_absolute_velocityi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_absolute_velocityi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->absolute_velocity[i]); + } + *(outbuffer + offset + 0) = (this->incremental_velocity_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->incremental_velocity_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->incremental_velocity_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->incremental_velocity_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->incremental_velocity_length); + for( uint32_t i = 0; i < incremental_velocity_length; i++){ + union { + float real; + uint32_t base; + } u_incremental_velocityi; + u_incremental_velocityi.real = this->incremental_velocity[i]; + *(outbuffer + offset + 0) = (u_incremental_velocityi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_incremental_velocityi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_incremental_velocityi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_incremental_velocityi.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->incremental_velocity[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + uint32_t joint_names_lengthT = ((uint32_t) (*(inbuffer + offset))); + joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->joint_names_length); + if(joint_names_lengthT > joint_names_length) + this->joint_names = (char**)realloc(this->joint_names, joint_names_lengthT * sizeof(char*)); + joint_names_length = joint_names_lengthT; + for( uint32_t i = 0; i < joint_names_length; i++){ + uint32_t length_st_joint_names; + arrToVar(length_st_joint_names, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_joint_names; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_joint_names-1]=0; + this->st_joint_names = (char *)(inbuffer + offset-1); + offset += length_st_joint_names; + memcpy( &(this->joint_names[i]), &(this->st_joint_names), sizeof(char*)); + } + uint32_t status_word_lengthT = ((uint32_t) (*(inbuffer + offset))); + status_word_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + status_word_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + status_word_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->status_word_length); + if(status_word_lengthT > status_word_length) + this->status_word = (char**)realloc(this->status_word, status_word_lengthT * sizeof(char*)); + status_word_length = status_word_lengthT; + for( uint32_t i = 0; i < status_word_length; i++){ + uint32_t length_st_status_word; + arrToVar(length_st_status_word, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_status_word; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_status_word-1]=0; + this->st_status_word = (char *)(inbuffer + offset-1); + offset += length_st_status_word; + memcpy( &(this->status_word[i]), &(this->st_status_word), sizeof(char*)); + } + uint32_t detailed_error_lengthT = ((uint32_t) (*(inbuffer + offset))); + detailed_error_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + detailed_error_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + detailed_error_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->detailed_error_length); + if(detailed_error_lengthT > detailed_error_length) + this->detailed_error = (char**)realloc(this->detailed_error, detailed_error_lengthT * sizeof(char*)); + detailed_error_length = detailed_error_lengthT; + for( uint32_t i = 0; i < detailed_error_length; i++){ + uint32_t length_st_detailed_error; + arrToVar(length_st_detailed_error, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_detailed_error; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_detailed_error-1]=0; + this->st_detailed_error = (char *)(inbuffer + offset-1); + offset += length_st_detailed_error; + memcpy( &(this->detailed_error[i]), &(this->st_detailed_error), sizeof(char*)); + } + uint32_t motion_error_lengthT = ((uint32_t) (*(inbuffer + offset))); + motion_error_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + motion_error_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + motion_error_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->motion_error_length); + if(motion_error_lengthT > motion_error_length) + this->motion_error = (char**)realloc(this->motion_error, motion_error_lengthT * sizeof(char*)); + motion_error_length = motion_error_lengthT; + for( uint32_t i = 0; i < motion_error_length; i++){ + uint32_t length_st_motion_error; + arrToVar(length_st_motion_error, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_motion_error; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_motion_error-1]=0; + this->st_motion_error = (char *)(inbuffer + offset-1); + offset += length_st_motion_error; + memcpy( &(this->motion_error[i]), &(this->st_motion_error), sizeof(char*)); + } + uint32_t state_lengthT = ((uint32_t) (*(inbuffer + offset))); + state_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + state_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + state_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->state_length); + if(state_lengthT > state_length) + this->state = (char**)realloc(this->state, state_lengthT * sizeof(char*)); + state_length = state_lengthT; + for( uint32_t i = 0; i < state_length; i++){ + uint32_t length_st_state; + arrToVar(length_st_state, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_state; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_state-1]=0; + this->st_state = (char *)(inbuffer + offset-1); + offset += length_st_state; + memcpy( &(this->state[i]), &(this->st_state), sizeof(char*)); + } + uint32_t detailed_error_description_lengthT = ((uint32_t) (*(inbuffer + offset))); + detailed_error_description_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + detailed_error_description_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + detailed_error_description_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->detailed_error_description_length); + if(detailed_error_description_lengthT > detailed_error_description_length) + this->detailed_error_description = (char**)realloc(this->detailed_error_description, detailed_error_description_lengthT * sizeof(char*)); + detailed_error_description_length = detailed_error_description_lengthT; + for( uint32_t i = 0; i < detailed_error_description_length; i++){ + uint32_t length_st_detailed_error_description; + arrToVar(length_st_detailed_error_description, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_detailed_error_description; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_detailed_error_description-1]=0; + this->st_detailed_error_description = (char *)(inbuffer + offset-1); + offset += length_st_detailed_error_description; + memcpy( &(this->detailed_error_description[i]), &(this->st_detailed_error_description), sizeof(char*)); + } + uint32_t motion_error_description_lengthT = ((uint32_t) (*(inbuffer + offset))); + motion_error_description_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + motion_error_description_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + motion_error_description_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->motion_error_description_length); + if(motion_error_description_lengthT > motion_error_description_length) + this->motion_error_description = (char**)realloc(this->motion_error_description, motion_error_description_lengthT * sizeof(char*)); + motion_error_description_length = motion_error_description_lengthT; + for( uint32_t i = 0; i < motion_error_description_length; i++){ + uint32_t length_st_motion_error_description; + arrToVar(length_st_motion_error_description, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_motion_error_description; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_motion_error_description-1]=0; + this->st_motion_error_description = (char *)(inbuffer + offset-1); + offset += length_st_motion_error_description; + memcpy( &(this->motion_error_description[i]), &(this->st_motion_error_description), sizeof(char*)); + } + uint32_t motor_current_lengthT = ((uint32_t) (*(inbuffer + offset))); + motor_current_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + motor_current_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + motor_current_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->motor_current_length); + if(motor_current_lengthT > motor_current_length) + this->motor_current = (float*)realloc(this->motor_current, motor_current_lengthT * sizeof(float)); + motor_current_length = motor_current_lengthT; + for( uint32_t i = 0; i < motor_current_length; i++){ + union { + float real; + uint32_t base; + } u_st_motor_current; + u_st_motor_current.base = 0; + u_st_motor_current.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_motor_current.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_motor_current.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_motor_current.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_motor_current = u_st_motor_current.real; + offset += sizeof(this->st_motor_current); + memcpy( &(this->motor_current[i]), &(this->st_motor_current), sizeof(float)); + } + uint32_t imc_voltage_lengthT = ((uint32_t) (*(inbuffer + offset))); + imc_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + imc_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + imc_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->imc_voltage_length); + if(imc_voltage_lengthT > imc_voltage_length) + this->imc_voltage = (float*)realloc(this->imc_voltage, imc_voltage_lengthT * sizeof(float)); + imc_voltage_length = imc_voltage_lengthT; + for( uint32_t i = 0; i < imc_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_st_imc_voltage; + u_st_imc_voltage.base = 0; + u_st_imc_voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_imc_voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_imc_voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_imc_voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_imc_voltage = u_st_imc_voltage.real; + offset += sizeof(this->st_imc_voltage); + memcpy( &(this->imc_voltage[i]), &(this->st_imc_voltage), sizeof(float)); + } + uint32_t motor_voltage_lengthT = ((uint32_t) (*(inbuffer + offset))); + motor_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + motor_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + motor_voltage_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->motor_voltage_length); + if(motor_voltage_lengthT > motor_voltage_length) + this->motor_voltage = (float*)realloc(this->motor_voltage, motor_voltage_lengthT * sizeof(float)); + motor_voltage_length = motor_voltage_lengthT; + for( uint32_t i = 0; i < motor_voltage_length; i++){ + union { + float real; + uint32_t base; + } u_st_motor_voltage; + u_st_motor_voltage.base = 0; + u_st_motor_voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_motor_voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_motor_voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_motor_voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_motor_voltage = u_st_motor_voltage.real; + offset += sizeof(this->st_motor_voltage); + memcpy( &(this->motor_voltage[i]), &(this->st_motor_voltage), sizeof(float)); + } + uint32_t absolute_encoder_value_lengthT = ((uint32_t) (*(inbuffer + offset))); + absolute_encoder_value_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + absolute_encoder_value_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + absolute_encoder_value_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->absolute_encoder_value_length); + if(absolute_encoder_value_lengthT > absolute_encoder_value_length) + this->absolute_encoder_value = (int32_t*)realloc(this->absolute_encoder_value, absolute_encoder_value_lengthT * sizeof(int32_t)); + absolute_encoder_value_length = absolute_encoder_value_lengthT; + for( uint32_t i = 0; i < absolute_encoder_value_length; i++){ + union { + int32_t real; + uint32_t base; + } u_st_absolute_encoder_value; + u_st_absolute_encoder_value.base = 0; + u_st_absolute_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_absolute_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_absolute_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_absolute_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_absolute_encoder_value = u_st_absolute_encoder_value.real; + offset += sizeof(this->st_absolute_encoder_value); + memcpy( &(this->absolute_encoder_value[i]), &(this->st_absolute_encoder_value), sizeof(int32_t)); + } + uint32_t incremental_encoder_value_lengthT = ((uint32_t) (*(inbuffer + offset))); + incremental_encoder_value_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + incremental_encoder_value_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + incremental_encoder_value_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->incremental_encoder_value_length); + if(incremental_encoder_value_lengthT > incremental_encoder_value_length) + this->incremental_encoder_value = (int32_t*)realloc(this->incremental_encoder_value, incremental_encoder_value_lengthT * sizeof(int32_t)); + incremental_encoder_value_length = incremental_encoder_value_lengthT; + for( uint32_t i = 0; i < incremental_encoder_value_length; i++){ + union { + int32_t real; + uint32_t base; + } u_st_incremental_encoder_value; + u_st_incremental_encoder_value.base = 0; + u_st_incremental_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_incremental_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_incremental_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_incremental_encoder_value.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_incremental_encoder_value = u_st_incremental_encoder_value.real; + offset += sizeof(this->st_incremental_encoder_value); + memcpy( &(this->incremental_encoder_value[i]), &(this->st_incremental_encoder_value), sizeof(int32_t)); + } + uint32_t absolute_velocity_lengthT = ((uint32_t) (*(inbuffer + offset))); + absolute_velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + absolute_velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + absolute_velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->absolute_velocity_length); + if(absolute_velocity_lengthT > absolute_velocity_length) + this->absolute_velocity = (float*)realloc(this->absolute_velocity, absolute_velocity_lengthT * sizeof(float)); + absolute_velocity_length = absolute_velocity_lengthT; + for( uint32_t i = 0; i < absolute_velocity_length; i++){ + union { + float real; + uint32_t base; + } u_st_absolute_velocity; + u_st_absolute_velocity.base = 0; + u_st_absolute_velocity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_absolute_velocity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_absolute_velocity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_absolute_velocity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_absolute_velocity = u_st_absolute_velocity.real; + offset += sizeof(this->st_absolute_velocity); + memcpy( &(this->absolute_velocity[i]), &(this->st_absolute_velocity), sizeof(float)); + } + uint32_t incremental_velocity_lengthT = ((uint32_t) (*(inbuffer + offset))); + incremental_velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + incremental_velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + incremental_velocity_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->incremental_velocity_length); + if(incremental_velocity_lengthT > incremental_velocity_length) + this->incremental_velocity = (float*)realloc(this->incremental_velocity, incremental_velocity_lengthT * sizeof(float)); + incremental_velocity_length = incremental_velocity_lengthT; + for( uint32_t i = 0; i < incremental_velocity_length; i++){ + union { + float real; + uint32_t base; + } u_st_incremental_velocity; + u_st_incremental_velocity.base = 0; + u_st_incremental_velocity.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_incremental_velocity.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_incremental_velocity.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_incremental_velocity.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_incremental_velocity = u_st_incremental_velocity.real; + offset += sizeof(this->st_incremental_velocity); + memcpy( &(this->incremental_velocity[i]), &(this->st_incremental_velocity), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "march_shared_resources/ImcState"; }; + const char * getMD5(){ return "ed4be4b991ee6e262d33c8b84a0ed150"; }; + + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/JointValues.h b/lib/ros_lib/march_shared_resources/JointValues.h new file mode 100644 index 0000000..0e8cf64 --- /dev/null +++ b/lib/ros_lib/march_shared_resources/JointValues.h @@ -0,0 +1,200 @@ +#ifndef _ROS_march_shared_resources_JointValues_h +#define _ROS_march_shared_resources_JointValues_h + +#include +#include +#include +#include "ros/msg.h" +#include "control_msgs/FollowJointTrajectoryFeedback.h" + +namespace march_shared_resources +{ + + class JointValues : public ros::Msg + { + public: + typedef control_msgs::FollowJointTrajectoryFeedback _controller_output_type; + _controller_output_type controller_output; + uint32_t velocities_length; + typedef double _velocities_type; + _velocities_type st_velocities; + _velocities_type * velocities; + uint32_t accelerations_length; + typedef double _accelerations_type; + _accelerations_type st_accelerations; + _accelerations_type * accelerations; + uint32_t jerks_length; + typedef double _jerks_type; + _jerks_type st_jerks; + _jerks_type * jerks; + + JointValues(): + controller_output(), + velocities_length(0), velocities(NULL), + accelerations_length(0), accelerations(NULL), + jerks_length(0), jerks(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->controller_output.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->velocities_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->velocities_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->velocities_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->velocities_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->velocities_length); + for( uint32_t i = 0; i < velocities_length; i++){ + union { + double real; + uint64_t base; + } u_velocitiesi; + u_velocitiesi.real = this->velocities[i]; + *(outbuffer + offset + 0) = (u_velocitiesi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_velocitiesi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_velocitiesi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_velocitiesi.base >> (8 * 3)) & 0xFF; + *(outbuffer + offset + 4) = (u_velocitiesi.base >> (8 * 4)) & 0xFF; + *(outbuffer + offset + 5) = (u_velocitiesi.base >> (8 * 5)) & 0xFF; + *(outbuffer + offset + 6) = (u_velocitiesi.base >> (8 * 6)) & 0xFF; + *(outbuffer + offset + 7) = (u_velocitiesi.base >> (8 * 7)) & 0xFF; + offset += sizeof(this->velocities[i]); + } + *(outbuffer + offset + 0) = (this->accelerations_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->accelerations_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->accelerations_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->accelerations_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->accelerations_length); + for( uint32_t i = 0; i < accelerations_length; i++){ + union { + double real; + uint64_t base; + } u_accelerationsi; + u_accelerationsi.real = this->accelerations[i]; + *(outbuffer + offset + 0) = (u_accelerationsi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_accelerationsi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_accelerationsi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_accelerationsi.base >> (8 * 3)) & 0xFF; + *(outbuffer + offset + 4) = (u_accelerationsi.base >> (8 * 4)) & 0xFF; + *(outbuffer + offset + 5) = (u_accelerationsi.base >> (8 * 5)) & 0xFF; + *(outbuffer + offset + 6) = (u_accelerationsi.base >> (8 * 6)) & 0xFF; + *(outbuffer + offset + 7) = (u_accelerationsi.base >> (8 * 7)) & 0xFF; + offset += sizeof(this->accelerations[i]); + } + *(outbuffer + offset + 0) = (this->jerks_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->jerks_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->jerks_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->jerks_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->jerks_length); + for( uint32_t i = 0; i < jerks_length; i++){ + union { + double real; + uint64_t base; + } u_jerksi; + u_jerksi.real = this->jerks[i]; + *(outbuffer + offset + 0) = (u_jerksi.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_jerksi.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_jerksi.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_jerksi.base >> (8 * 3)) & 0xFF; + *(outbuffer + offset + 4) = (u_jerksi.base >> (8 * 4)) & 0xFF; + *(outbuffer + offset + 5) = (u_jerksi.base >> (8 * 5)) & 0xFF; + *(outbuffer + offset + 6) = (u_jerksi.base >> (8 * 6)) & 0xFF; + *(outbuffer + offset + 7) = (u_jerksi.base >> (8 * 7)) & 0xFF; + offset += sizeof(this->jerks[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->controller_output.deserialize(inbuffer + offset); + uint32_t velocities_lengthT = ((uint32_t) (*(inbuffer + offset))); + velocities_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + velocities_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + velocities_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->velocities_length); + if(velocities_lengthT > velocities_length) + this->velocities = (double*)realloc(this->velocities, velocities_lengthT * sizeof(double)); + velocities_length = velocities_lengthT; + for( uint32_t i = 0; i < velocities_length; i++){ + union { + double real; + uint64_t base; + } u_st_velocities; + u_st_velocities.base = 0; + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); + u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); + this->st_velocities = u_st_velocities.real; + offset += sizeof(this->st_velocities); + memcpy( &(this->velocities[i]), &(this->st_velocities), sizeof(double)); + } + uint32_t accelerations_lengthT = ((uint32_t) (*(inbuffer + offset))); + accelerations_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + accelerations_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + accelerations_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->accelerations_length); + if(accelerations_lengthT > accelerations_length) + this->accelerations = (double*)realloc(this->accelerations, accelerations_lengthT * sizeof(double)); + accelerations_length = accelerations_lengthT; + for( uint32_t i = 0; i < accelerations_length; i++){ + union { + double real; + uint64_t base; + } u_st_accelerations; + u_st_accelerations.base = 0; + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); + u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); + this->st_accelerations = u_st_accelerations.real; + offset += sizeof(this->st_accelerations); + memcpy( &(this->accelerations[i]), &(this->st_accelerations), sizeof(double)); + } + uint32_t jerks_lengthT = ((uint32_t) (*(inbuffer + offset))); + jerks_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + jerks_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + jerks_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->jerks_length); + if(jerks_lengthT > jerks_length) + this->jerks = (double*)realloc(this->jerks, jerks_lengthT * sizeof(double)); + jerks_length = jerks_lengthT; + for( uint32_t i = 0; i < jerks_length; i++){ + union { + double real; + uint64_t base; + } u_st_jerks; + u_st_jerks.base = 0; + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); + u_st_jerks.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); + this->st_jerks = u_st_jerks.real; + offset += sizeof(this->st_jerks); + memcpy( &(this->jerks[i]), &(this->st_jerks), sizeof(double)); + } + return offset; + } + + const char * getType(){ return "march_shared_resources/JointValues"; }; + const char * getMD5(){ return "8c2d87ce75da8d6f89c63a524cb72481"; }; + + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/PressureSole.h b/lib/ros_lib/march_shared_resources/PressureSole.h new file mode 100644 index 0000000..00bb531 --- /dev/null +++ b/lib/ros_lib/march_shared_resources/PressureSole.h @@ -0,0 +1,292 @@ +#ifndef _ROS_march_shared_resources_PressureSole_h +#define _ROS_march_shared_resources_PressureSole_h + +#include +#include +#include +#include "ros/msg.h" +#include "std_msgs/Header.h" +#include "ros/time.h" + +namespace march_shared_resources +{ + + class PressureSole : public ros::Msg + { + public: + typedef std_msgs::Header _header_type; + _header_type header; + typedef ros::Time _pressure_soles_time_type; + _pressure_soles_time_type pressure_soles_time; + typedef float _total_force_left_type; + _total_force_left_type total_force_left; + typedef float _total_force_right_type; + _total_force_right_type total_force_right; + uint32_t pressure_left_length; + typedef float _pressure_left_type; + _pressure_left_type st_pressure_left; + _pressure_left_type * pressure_left; + uint32_t pressure_right_length; + typedef float _pressure_right_type; + _pressure_right_type st_pressure_right; + _pressure_right_type * pressure_right; + uint32_t cop_left_length; + typedef float _cop_left_type; + _cop_left_type st_cop_left; + _cop_left_type * cop_left; + uint32_t cop_right_length; + typedef float _cop_right_type; + _cop_right_type st_cop_right; + _cop_right_type * cop_right; + + PressureSole(): + header(), + pressure_soles_time(), + total_force_left(0), + total_force_right(0), + pressure_left_length(0), pressure_left(NULL), + pressure_right_length(0), pressure_right(NULL), + cop_left_length(0), cop_left(NULL), + cop_right_length(0), cop_right(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + offset += this->header.serialize(outbuffer + offset); + *(outbuffer + offset + 0) = (this->pressure_soles_time.sec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->pressure_soles_time.sec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->pressure_soles_time.sec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->pressure_soles_time.sec >> (8 * 3)) & 0xFF; + offset += sizeof(this->pressure_soles_time.sec); + *(outbuffer + offset + 0) = (this->pressure_soles_time.nsec >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->pressure_soles_time.nsec >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->pressure_soles_time.nsec >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->pressure_soles_time.nsec >> (8 * 3)) & 0xFF; + offset += sizeof(this->pressure_soles_time.nsec); + union { + float real; + uint32_t base; + } u_total_force_left; + u_total_force_left.real = this->total_force_left; + *(outbuffer + offset + 0) = (u_total_force_left.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_total_force_left.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_total_force_left.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_total_force_left.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->total_force_left); + union { + float real; + uint32_t base; + } u_total_force_right; + u_total_force_right.real = this->total_force_right; + *(outbuffer + offset + 0) = (u_total_force_right.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_total_force_right.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_total_force_right.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_total_force_right.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->total_force_right); + *(outbuffer + offset + 0) = (this->pressure_left_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->pressure_left_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->pressure_left_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->pressure_left_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->pressure_left_length); + for( uint32_t i = 0; i < pressure_left_length; i++){ + union { + float real; + uint32_t base; + } u_pressure_lefti; + u_pressure_lefti.real = this->pressure_left[i]; + *(outbuffer + offset + 0) = (u_pressure_lefti.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_pressure_lefti.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_pressure_lefti.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_pressure_lefti.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->pressure_left[i]); + } + *(outbuffer + offset + 0) = (this->pressure_right_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->pressure_right_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->pressure_right_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->pressure_right_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->pressure_right_length); + for( uint32_t i = 0; i < pressure_right_length; i++){ + union { + float real; + uint32_t base; + } u_pressure_righti; + u_pressure_righti.real = this->pressure_right[i]; + *(outbuffer + offset + 0) = (u_pressure_righti.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_pressure_righti.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_pressure_righti.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_pressure_righti.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->pressure_right[i]); + } + *(outbuffer + offset + 0) = (this->cop_left_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->cop_left_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->cop_left_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->cop_left_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->cop_left_length); + for( uint32_t i = 0; i < cop_left_length; i++){ + union { + float real; + uint32_t base; + } u_cop_lefti; + u_cop_lefti.real = this->cop_left[i]; + *(outbuffer + offset + 0) = (u_cop_lefti.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cop_lefti.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cop_lefti.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cop_lefti.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cop_left[i]); + } + *(outbuffer + offset + 0) = (this->cop_right_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->cop_right_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->cop_right_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->cop_right_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->cop_right_length); + for( uint32_t i = 0; i < cop_right_length; i++){ + union { + float real; + uint32_t base; + } u_cop_righti; + u_cop_righti.real = this->cop_right[i]; + *(outbuffer + offset + 0) = (u_cop_righti.base >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (u_cop_righti.base >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (u_cop_righti.base >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (u_cop_righti.base >> (8 * 3)) & 0xFF; + offset += sizeof(this->cop_right[i]); + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + offset += this->header.deserialize(inbuffer + offset); + this->pressure_soles_time.sec = ((uint32_t) (*(inbuffer + offset))); + this->pressure_soles_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->pressure_soles_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->pressure_soles_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->pressure_soles_time.sec); + this->pressure_soles_time.nsec = ((uint32_t) (*(inbuffer + offset))); + this->pressure_soles_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + this->pressure_soles_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + this->pressure_soles_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->pressure_soles_time.nsec); + union { + float real; + uint32_t base; + } u_total_force_left; + u_total_force_left.base = 0; + u_total_force_left.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_total_force_left.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_total_force_left.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_total_force_left.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->total_force_left = u_total_force_left.real; + offset += sizeof(this->total_force_left); + union { + float real; + uint32_t base; + } u_total_force_right; + u_total_force_right.base = 0; + u_total_force_right.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_total_force_right.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_total_force_right.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_total_force_right.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->total_force_right = u_total_force_right.real; + offset += sizeof(this->total_force_right); + uint32_t pressure_left_lengthT = ((uint32_t) (*(inbuffer + offset))); + pressure_left_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + pressure_left_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + pressure_left_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->pressure_left_length); + if(pressure_left_lengthT > pressure_left_length) + this->pressure_left = (float*)realloc(this->pressure_left, pressure_left_lengthT * sizeof(float)); + pressure_left_length = pressure_left_lengthT; + for( uint32_t i = 0; i < pressure_left_length; i++){ + union { + float real; + uint32_t base; + } u_st_pressure_left; + u_st_pressure_left.base = 0; + u_st_pressure_left.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_pressure_left.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_pressure_left.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_pressure_left.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_pressure_left = u_st_pressure_left.real; + offset += sizeof(this->st_pressure_left); + memcpy( &(this->pressure_left[i]), &(this->st_pressure_left), sizeof(float)); + } + uint32_t pressure_right_lengthT = ((uint32_t) (*(inbuffer + offset))); + pressure_right_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + pressure_right_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + pressure_right_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->pressure_right_length); + if(pressure_right_lengthT > pressure_right_length) + this->pressure_right = (float*)realloc(this->pressure_right, pressure_right_lengthT * sizeof(float)); + pressure_right_length = pressure_right_lengthT; + for( uint32_t i = 0; i < pressure_right_length; i++){ + union { + float real; + uint32_t base; + } u_st_pressure_right; + u_st_pressure_right.base = 0; + u_st_pressure_right.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_pressure_right.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_pressure_right.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_pressure_right.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_pressure_right = u_st_pressure_right.real; + offset += sizeof(this->st_pressure_right); + memcpy( &(this->pressure_right[i]), &(this->st_pressure_right), sizeof(float)); + } + uint32_t cop_left_lengthT = ((uint32_t) (*(inbuffer + offset))); + cop_left_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + cop_left_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + cop_left_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->cop_left_length); + if(cop_left_lengthT > cop_left_length) + this->cop_left = (float*)realloc(this->cop_left, cop_left_lengthT * sizeof(float)); + cop_left_length = cop_left_lengthT; + for( uint32_t i = 0; i < cop_left_length; i++){ + union { + float real; + uint32_t base; + } u_st_cop_left; + u_st_cop_left.base = 0; + u_st_cop_left.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_cop_left.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_cop_left.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_cop_left.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_cop_left = u_st_cop_left.real; + offset += sizeof(this->st_cop_left); + memcpy( &(this->cop_left[i]), &(this->st_cop_left), sizeof(float)); + } + uint32_t cop_right_lengthT = ((uint32_t) (*(inbuffer + offset))); + cop_right_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + cop_right_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + cop_right_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->cop_right_length); + if(cop_right_lengthT > cop_right_length) + this->cop_right = (float*)realloc(this->cop_right, cop_right_lengthT * sizeof(float)); + cop_right_length = cop_right_lengthT; + for( uint32_t i = 0; i < cop_right_length; i++){ + union { + float real; + uint32_t base; + } u_st_cop_right; + u_st_cop_right.base = 0; + u_st_cop_right.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + u_st_cop_right.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + u_st_cop_right.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + u_st_cop_right.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + this->st_cop_right = u_st_cop_right.real; + offset += sizeof(this->st_cop_right); + memcpy( &(this->cop_right[i]), &(this->st_cop_right), sizeof(float)); + } + return offset; + } + + const char * getType(){ return "march_shared_resources/PressureSole"; }; + const char * getMD5(){ return "13b27f467193798354308188ddb84202"; }; + + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/SetGaitVersion.h b/lib/ros_lib/march_shared_resources/SetGaitVersion.h new file mode 100644 index 0000000..64cb94f --- /dev/null +++ b/lib/ros_lib/march_shared_resources/SetGaitVersion.h @@ -0,0 +1,196 @@ +#ifndef _ROS_SERVICE_SetGaitVersion_h +#define _ROS_SERVICE_SetGaitVersion_h +#include +#include +#include +#include "ros/msg.h" + +namespace march_shared_resources +{ + +static const char SETGAITVERSION[] = "march_shared_resources/SetGaitVersion"; + + class SetGaitVersionRequest : public ros::Msg + { + public: + typedef const char* _gait_type; + _gait_type gait; + uint32_t subgaits_length; + typedef char* _subgaits_type; + _subgaits_type st_subgaits; + _subgaits_type * subgaits; + uint32_t versions_length; + typedef char* _versions_type; + _versions_type st_versions; + _versions_type * versions; + + SetGaitVersionRequest(): + gait(""), + subgaits_length(0), subgaits(NULL), + versions_length(0), versions(NULL) + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + uint32_t length_gait = strlen(this->gait); + varToArr(outbuffer + offset, length_gait); + offset += 4; + memcpy(outbuffer + offset, this->gait, length_gait); + offset += length_gait; + *(outbuffer + offset + 0) = (this->subgaits_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->subgaits_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->subgaits_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->subgaits_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->subgaits_length); + for( uint32_t i = 0; i < subgaits_length; i++){ + uint32_t length_subgaitsi = strlen(this->subgaits[i]); + varToArr(outbuffer + offset, length_subgaitsi); + offset += 4; + memcpy(outbuffer + offset, this->subgaits[i], length_subgaitsi); + offset += length_subgaitsi; + } + *(outbuffer + offset + 0) = (this->versions_length >> (8 * 0)) & 0xFF; + *(outbuffer + offset + 1) = (this->versions_length >> (8 * 1)) & 0xFF; + *(outbuffer + offset + 2) = (this->versions_length >> (8 * 2)) & 0xFF; + *(outbuffer + offset + 3) = (this->versions_length >> (8 * 3)) & 0xFF; + offset += sizeof(this->versions_length); + for( uint32_t i = 0; i < versions_length; i++){ + uint32_t length_versionsi = strlen(this->versions[i]); + varToArr(outbuffer + offset, length_versionsi); + offset += 4; + memcpy(outbuffer + offset, this->versions[i], length_versionsi); + offset += length_versionsi; + } + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + uint32_t length_gait; + arrToVar(length_gait, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_gait; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_gait-1]=0; + this->gait = (char *)(inbuffer + offset-1); + offset += length_gait; + uint32_t subgaits_lengthT = ((uint32_t) (*(inbuffer + offset))); + subgaits_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + subgaits_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + subgaits_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->subgaits_length); + if(subgaits_lengthT > subgaits_length) + this->subgaits = (char**)realloc(this->subgaits, subgaits_lengthT * sizeof(char*)); + subgaits_length = subgaits_lengthT; + for( uint32_t i = 0; i < subgaits_length; i++){ + uint32_t length_st_subgaits; + arrToVar(length_st_subgaits, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_subgaits; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_subgaits-1]=0; + this->st_subgaits = (char *)(inbuffer + offset-1); + offset += length_st_subgaits; + memcpy( &(this->subgaits[i]), &(this->st_subgaits), sizeof(char*)); + } + uint32_t versions_lengthT = ((uint32_t) (*(inbuffer + offset))); + versions_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); + versions_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); + versions_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); + offset += sizeof(this->versions_length); + if(versions_lengthT > versions_length) + this->versions = (char**)realloc(this->versions, versions_lengthT * sizeof(char*)); + versions_length = versions_lengthT; + for( uint32_t i = 0; i < versions_length; i++){ + uint32_t length_st_versions; + arrToVar(length_st_versions, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_st_versions; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_st_versions-1]=0; + this->st_versions = (char *)(inbuffer + offset-1); + offset += length_st_versions; + memcpy( &(this->versions[i]), &(this->st_versions), sizeof(char*)); + } + return offset; + } + + const char * getType(){ return SETGAITVERSION; }; + const char * getMD5(){ return "f7723c86866cf380148082df35441a88"; }; + + }; + + class SetGaitVersionResponse : public ros::Msg + { + public: + typedef bool _success_type; + _success_type success; + typedef const char* _message_type; + _message_type message; + + SetGaitVersionResponse(): + success(0), + message("") + { + } + + virtual int serialize(unsigned char *outbuffer) const + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.real = this->success; + *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->success); + uint32_t length_message = strlen(this->message); + varToArr(outbuffer + offset, length_message); + offset += 4; + memcpy(outbuffer + offset, this->message, length_message); + offset += length_message; + return offset; + } + + virtual int deserialize(unsigned char *inbuffer) + { + int offset = 0; + union { + bool real; + uint8_t base; + } u_success; + u_success.base = 0; + u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->success = u_success.real; + offset += sizeof(this->success); + uint32_t length_message; + arrToVar(length_message, (inbuffer + offset)); + offset += 4; + for(unsigned int k= offset; k< offset+length_message; ++k){ + inbuffer[k-1]=inbuffer[k]; + } + inbuffer[offset+length_message-1]=0; + this->message = (char *)(inbuffer + offset-1); + offset += length_message; + return offset; + } + + const char * getType(){ return SETGAITVERSION; }; + const char * getMD5(){ return "937c9679a518e3a18d831e57125ea522"; }; + + }; + + class SetGaitVersion { + public: + typedef SetGaitVersionRequest Request; + typedef SetGaitVersionResponse Response; + }; + +} +#endif diff --git a/lib/ros_lib/march_shared_resources/Setpoint.h b/lib/ros_lib/march_shared_resources/Setpoint.h deleted file mode 100644 index 9746e4f..0000000 --- a/lib/ros_lib/march_shared_resources/Setpoint.h +++ /dev/null @@ -1,99 +0,0 @@ -#ifndef _ROS_march_shared_resources_Setpoint_h -#define _ROS_march_shared_resources_Setpoint_h - -#include -#include -#include -#include "ros/msg.h" -#include "ros/duration.h" - -namespace march_shared_resources -{ - - class Setpoint : public ros::Msg - { - public: - typedef ros::Duration _time_from_start_type; - _time_from_start_type time_from_start; - uint32_t joint_names_length; - typedef char* _joint_names_type; - _joint_names_type st_joint_names; - _joint_names_type * joint_names; - - Setpoint(): - time_from_start(), - joint_names_length(0), joint_names(NULL) - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - *(outbuffer + offset + 0) = (this->time_from_start.sec >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->time_from_start.sec >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->time_from_start.sec >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->time_from_start.sec >> (8 * 3)) & 0xFF; - offset += sizeof(this->time_from_start.sec); - *(outbuffer + offset + 0) = (this->time_from_start.nsec >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->time_from_start.nsec >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->time_from_start.nsec >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->time_from_start.nsec >> (8 * 3)) & 0xFF; - offset += sizeof(this->time_from_start.nsec); - *(outbuffer + offset + 0) = (this->joint_names_length >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->joint_names_length >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->joint_names_length >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->joint_names_length >> (8 * 3)) & 0xFF; - offset += sizeof(this->joint_names_length); - for( uint32_t i = 0; i < joint_names_length; i++){ - uint32_t length_joint_namesi = strlen(this->joint_names[i]); - varToArr(outbuffer + offset, length_joint_namesi); - offset += 4; - memcpy(outbuffer + offset, this->joint_names[i], length_joint_namesi); - offset += length_joint_namesi; - } - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - this->time_from_start.sec = ((uint32_t) (*(inbuffer + offset))); - this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->time_from_start.sec); - this->time_from_start.nsec = ((uint32_t) (*(inbuffer + offset))); - this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->time_from_start.nsec); - uint32_t joint_names_lengthT = ((uint32_t) (*(inbuffer + offset))); - joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->joint_names_length); - if(joint_names_lengthT > joint_names_length) - this->joint_names = (char**)realloc(this->joint_names, joint_names_lengthT * sizeof(char*)); - joint_names_length = joint_names_lengthT; - for( uint32_t i = 0; i < joint_names_length; i++){ - uint32_t length_st_joint_names; - arrToVar(length_st_joint_names, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_st_joint_names; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_st_joint_names-1]=0; - this->st_joint_names = (char *)(inbuffer + offset-1); - offset += length_st_joint_names; - memcpy( &(this->joint_names[i]), &(this->st_joint_names), sizeof(char*)); - } - return offset; - } - - const char * getType(){ return "march_shared_resources/Setpoint"; }; - const char * getMD5(){ return "eb16a7760f76a92e80028db52923860d"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/StringTrigger.h b/lib/ros_lib/march_shared_resources/StringTrigger.h deleted file mode 100644 index 6de67b5..0000000 --- a/lib/ros_lib/march_shared_resources/StringTrigger.h +++ /dev/null @@ -1,122 +0,0 @@ -#ifndef _ROS_SERVICE_StringTrigger_h -#define _ROS_SERVICE_StringTrigger_h -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - -static const char STRINGTRIGGER[] = "march_shared_resources/StringTrigger"; - - class StringTriggerRequest : public ros::Msg - { - public: - typedef const char* _string_type; - _string_type string; - - StringTriggerRequest(): - string("") - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - uint32_t length_string = strlen(this->string); - varToArr(outbuffer + offset, length_string); - offset += 4; - memcpy(outbuffer + offset, this->string, length_string); - offset += length_string; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t length_string; - arrToVar(length_string, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_string; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_string-1]=0; - this->string = (char *)(inbuffer + offset-1); - offset += length_string; - return offset; - } - - const char * getType(){ return STRINGTRIGGER; }; - const char * getMD5(){ return "923dd8c946bcf1b66f509c36c23d7b88"; }; - - }; - - class StringTriggerResponse : public ros::Msg - { - public: - typedef bool _success_type; - _success_type success; - typedef const char* _message_type; - _message_type message; - - StringTriggerResponse(): - success(0), - message("") - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - union { - bool real; - uint8_t base; - } u_success; - u_success.real = this->success; - *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; - offset += sizeof(this->success); - uint32_t length_message = strlen(this->message); - varToArr(outbuffer + offset, length_message); - offset += 4; - memcpy(outbuffer + offset, this->message, length_message); - offset += length_message; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - union { - bool real; - uint8_t base; - } u_success; - u_success.base = 0; - u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); - this->success = u_success.real; - offset += sizeof(this->success); - uint32_t length_message; - arrToVar(length_message, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_message; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_message-1]=0; - this->message = (char *)(inbuffer + offset-1); - offset += length_message; - return offset; - } - - const char * getType(){ return STRINGTRIGGER; }; - const char * getMD5(){ return "937c9679a518e3a18d831e57125ea522"; }; - - }; - - class StringTrigger { - public: - typedef StringTriggerRequest Request; - typedef StringTriggerResponse Response; - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/Subgait.h b/lib/ros_lib/march_shared_resources/Subgait.h deleted file mode 100644 index 968a8bb..0000000 --- a/lib/ros_lib/march_shared_resources/Subgait.h +++ /dev/null @@ -1,199 +0,0 @@ -#ifndef _ROS_march_shared_resources_Subgait_h -#define _ROS_march_shared_resources_Subgait_h - -#include -#include -#include -#include "ros/msg.h" -#include "march_shared_resources/Setpoint.h" -#include "trajectory_msgs/JointTrajectory.h" -#include "ros/duration.h" - -namespace march_shared_resources -{ - - class Subgait : public ros::Msg - { - public: - typedef const char* _name_type; - _name_type name; - typedef const char* _version_type; - _version_type version; - typedef const char* _description_type; - _description_type description; - typedef const char* _gait_type_type; - _gait_type_type gait_type; - uint32_t setpoints_length; - typedef march_shared_resources::Setpoint _setpoints_type; - _setpoints_type st_setpoints; - _setpoints_type * setpoints; - typedef trajectory_msgs::JointTrajectory _trajectory_type; - _trajectory_type trajectory; - typedef ros::Duration _duration_type; - _duration_type duration; - uint32_t sounds_length; - typedef char* _sounds_type; - _sounds_type st_sounds; - _sounds_type * sounds; - - Subgait(): - name(""), - version(""), - description(""), - gait_type(""), - setpoints_length(0), setpoints(NULL), - trajectory(), - duration(), - sounds_length(0), sounds(NULL) - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - uint32_t length_name = strlen(this->name); - varToArr(outbuffer + offset, length_name); - offset += 4; - memcpy(outbuffer + offset, this->name, length_name); - offset += length_name; - uint32_t length_version = strlen(this->version); - varToArr(outbuffer + offset, length_version); - offset += 4; - memcpy(outbuffer + offset, this->version, length_version); - offset += length_version; - uint32_t length_description = strlen(this->description); - varToArr(outbuffer + offset, length_description); - offset += 4; - memcpy(outbuffer + offset, this->description, length_description); - offset += length_description; - uint32_t length_gait_type = strlen(this->gait_type); - varToArr(outbuffer + offset, length_gait_type); - offset += 4; - memcpy(outbuffer + offset, this->gait_type, length_gait_type); - offset += length_gait_type; - *(outbuffer + offset + 0) = (this->setpoints_length >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->setpoints_length >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->setpoints_length >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->setpoints_length >> (8 * 3)) & 0xFF; - offset += sizeof(this->setpoints_length); - for( uint32_t i = 0; i < setpoints_length; i++){ - offset += this->setpoints[i].serialize(outbuffer + offset); - } - offset += this->trajectory.serialize(outbuffer + offset); - *(outbuffer + offset + 0) = (this->duration.sec >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->duration.sec >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->duration.sec >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->duration.sec >> (8 * 3)) & 0xFF; - offset += sizeof(this->duration.sec); - *(outbuffer + offset + 0) = (this->duration.nsec >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->duration.nsec >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->duration.nsec >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->duration.nsec >> (8 * 3)) & 0xFF; - offset += sizeof(this->duration.nsec); - *(outbuffer + offset + 0) = (this->sounds_length >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->sounds_length >> (8 * 1)) & 0xFF; - *(outbuffer + offset + 2) = (this->sounds_length >> (8 * 2)) & 0xFF; - *(outbuffer + offset + 3) = (this->sounds_length >> (8 * 3)) & 0xFF; - offset += sizeof(this->sounds_length); - for( uint32_t i = 0; i < sounds_length; i++){ - uint32_t length_soundsi = strlen(this->sounds[i]); - varToArr(outbuffer + offset, length_soundsi); - offset += 4; - memcpy(outbuffer + offset, this->sounds[i], length_soundsi); - offset += length_soundsi; - } - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t length_name; - arrToVar(length_name, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_name; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_name-1]=0; - this->name = (char *)(inbuffer + offset-1); - offset += length_name; - uint32_t length_version; - arrToVar(length_version, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_version; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_version-1]=0; - this->version = (char *)(inbuffer + offset-1); - offset += length_version; - uint32_t length_description; - arrToVar(length_description, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_description; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_description-1]=0; - this->description = (char *)(inbuffer + offset-1); - offset += length_description; - uint32_t length_gait_type; - arrToVar(length_gait_type, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_gait_type; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_gait_type-1]=0; - this->gait_type = (char *)(inbuffer + offset-1); - offset += length_gait_type; - uint32_t setpoints_lengthT = ((uint32_t) (*(inbuffer + offset))); - setpoints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - setpoints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - setpoints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->setpoints_length); - if(setpoints_lengthT > setpoints_length) - this->setpoints = (march_shared_resources::Setpoint*)realloc(this->setpoints, setpoints_lengthT * sizeof(march_shared_resources::Setpoint)); - setpoints_length = setpoints_lengthT; - for( uint32_t i = 0; i < setpoints_length; i++){ - offset += this->st_setpoints.deserialize(inbuffer + offset); - memcpy( &(this->setpoints[i]), &(this->st_setpoints), sizeof(march_shared_resources::Setpoint)); - } - offset += this->trajectory.deserialize(inbuffer + offset); - this->duration.sec = ((uint32_t) (*(inbuffer + offset))); - this->duration.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->duration.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - this->duration.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->duration.sec); - this->duration.nsec = ((uint32_t) (*(inbuffer + offset))); - this->duration.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->duration.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - this->duration.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->duration.nsec); - uint32_t sounds_lengthT = ((uint32_t) (*(inbuffer + offset))); - sounds_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); - sounds_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); - sounds_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); - offset += sizeof(this->sounds_length); - if(sounds_lengthT > sounds_length) - this->sounds = (char**)realloc(this->sounds, sounds_lengthT * sizeof(char*)); - sounds_length = sounds_lengthT; - for( uint32_t i = 0; i < sounds_length; i++){ - uint32_t length_st_sounds; - arrToVar(length_st_sounds, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_st_sounds; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_st_sounds-1]=0; - this->st_sounds = (char *)(inbuffer + offset-1); - offset += length_st_sounds; - memcpy( &(this->sounds[i]), &(this->st_sounds), sizeof(char*)); - } - return offset; - } - - const char * getType(){ return "march_shared_resources/Subgait"; }; - const char * getMD5(){ return "ddf34d2f35048d108042f0cf4303dde5"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/march_shared_resources/Trigger.h b/lib/ros_lib/march_shared_resources/Trigger.h deleted file mode 100644 index 5edfa9f..0000000 --- a/lib/ros_lib/march_shared_resources/Trigger.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef _ROS_SERVICE_Trigger_h -#define _ROS_SERVICE_Trigger_h -#include -#include -#include -#include "ros/msg.h" - -namespace march_shared_resources -{ - -static const char TRIGGER[] = "march_shared_resources/Trigger"; - - class TriggerRequest : public ros::Msg - { - public: - - TriggerRequest() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - return offset; - } - - const char * getType(){ return TRIGGER; }; - const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; - - }; - - class TriggerResponse : public ros::Msg - { - public: - typedef bool _success_type; - _success_type success; - typedef const char* _message_type; - _message_type message; - - TriggerResponse(): - success(0), - message("") - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - union { - bool real; - uint8_t base; - } u_success; - u_success.real = this->success; - *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF; - offset += sizeof(this->success); - uint32_t length_message = strlen(this->message); - varToArr(outbuffer + offset, length_message); - offset += 4; - memcpy(outbuffer + offset, this->message, length_message); - offset += length_message; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - union { - bool real; - uint8_t base; - } u_success; - u_success.base = 0; - u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); - this->success = u_success.real; - offset += sizeof(this->success); - uint32_t length_message; - arrToVar(length_message, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_message; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_message-1]=0; - this->message = (char *)(inbuffer + offset-1); - offset += length_message; - return offset; - } - - const char * getType(){ return TRIGGER; }; - const char * getMD5(){ return "937c9679a518e3a18d831e57125ea522"; }; - - }; - - class Trigger { - public: - typedef TriggerRequest Request; - typedef TriggerResponse Response; - }; - -} -#endif diff --git a/lib/ros_lib/rosserial_arduino/Adc.h b/lib/ros_lib/rosserial_arduino/Adc.h deleted file mode 100644 index 23e48d6..0000000 --- a/lib/ros_lib/rosserial_arduino/Adc.h +++ /dev/null @@ -1,92 +0,0 @@ -#ifndef _ROS_rosserial_arduino_Adc_h -#define _ROS_rosserial_arduino_Adc_h - -#include -#include -#include -#include "ros/msg.h" - -namespace rosserial_arduino -{ - - class Adc : public ros::Msg - { - public: - typedef uint16_t _adc0_type; - _adc0_type adc0; - typedef uint16_t _adc1_type; - _adc1_type adc1; - typedef uint16_t _adc2_type; - _adc2_type adc2; - typedef uint16_t _adc3_type; - _adc3_type adc3; - typedef uint16_t _adc4_type; - _adc4_type adc4; - typedef uint16_t _adc5_type; - _adc5_type adc5; - - Adc(): - adc0(0), - adc1(0), - adc2(0), - adc3(0), - adc4(0), - adc5(0) - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - *(outbuffer + offset + 0) = (this->adc0 >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->adc0 >> (8 * 1)) & 0xFF; - offset += sizeof(this->adc0); - *(outbuffer + offset + 0) = (this->adc1 >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->adc1 >> (8 * 1)) & 0xFF; - offset += sizeof(this->adc1); - *(outbuffer + offset + 0) = (this->adc2 >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->adc2 >> (8 * 1)) & 0xFF; - offset += sizeof(this->adc2); - *(outbuffer + offset + 0) = (this->adc3 >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->adc3 >> (8 * 1)) & 0xFF; - offset += sizeof(this->adc3); - *(outbuffer + offset + 0) = (this->adc4 >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->adc4 >> (8 * 1)) & 0xFF; - offset += sizeof(this->adc4); - *(outbuffer + offset + 0) = (this->adc5 >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (this->adc5 >> (8 * 1)) & 0xFF; - offset += sizeof(this->adc5); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - this->adc0 = ((uint16_t) (*(inbuffer + offset))); - this->adc0 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - offset += sizeof(this->adc0); - this->adc1 = ((uint16_t) (*(inbuffer + offset))); - this->adc1 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - offset += sizeof(this->adc1); - this->adc2 = ((uint16_t) (*(inbuffer + offset))); - this->adc2 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - offset += sizeof(this->adc2); - this->adc3 = ((uint16_t) (*(inbuffer + offset))); - this->adc3 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - offset += sizeof(this->adc3); - this->adc4 = ((uint16_t) (*(inbuffer + offset))); - this->adc4 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - offset += sizeof(this->adc4); - this->adc5 = ((uint16_t) (*(inbuffer + offset))); - this->adc5 |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - offset += sizeof(this->adc5); - return offset; - } - - const char * getType(){ return "rosserial_arduino/Adc"; }; - const char * getMD5(){ return "6d7853a614e2e821319068311f2af25b"; }; - - }; - -} -#endif diff --git a/lib/ros_lib/rosserial_arduino/Test.h b/lib/ros_lib/rosserial_arduino/Test.h deleted file mode 100644 index cd806db..0000000 --- a/lib/ros_lib/rosserial_arduino/Test.h +++ /dev/null @@ -1,104 +0,0 @@ -#ifndef _ROS_SERVICE_Test_h -#define _ROS_SERVICE_Test_h -#include -#include -#include -#include "ros/msg.h" - -namespace rosserial_arduino -{ - -static const char TEST[] = "rosserial_arduino/Test"; - - class TestRequest : public ros::Msg - { - public: - typedef const char* _input_type; - _input_type input; - - TestRequest(): - input("") - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - uint32_t length_input = strlen(this->input); - varToArr(outbuffer + offset, length_input); - offset += 4; - memcpy(outbuffer + offset, this->input, length_input); - offset += length_input; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t length_input; - arrToVar(length_input, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_input; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_input-1]=0; - this->input = (char *)(inbuffer + offset-1); - offset += length_input; - return offset; - } - - const char * getType(){ return TEST; }; - const char * getMD5(){ return "39e92f1778057359c64c7b8a7d7b19de"; }; - - }; - - class TestResponse : public ros::Msg - { - public: - typedef const char* _output_type; - _output_type output; - - TestResponse(): - output("") - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - uint32_t length_output = strlen(this->output); - varToArr(outbuffer + offset, length_output); - offset += 4; - memcpy(outbuffer + offset, this->output, length_output); - offset += length_output; - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - uint32_t length_output; - arrToVar(length_output, (inbuffer + offset)); - offset += 4; - for(unsigned int k= offset; k< offset+length_output; ++k){ - inbuffer[k-1]=inbuffer[k]; - } - inbuffer[offset+length_output-1]=0; - this->output = (char *)(inbuffer + offset-1); - offset += length_output; - return offset; - } - - const char * getType(){ return TEST; }; - const char * getMD5(){ return "0825d95fdfa2c8f4bbb4e9c74bccd3fd"; }; - - }; - - class Test { - public: - typedef TestRequest Request; - typedef TestResponse Response; - }; - -} -#endif diff --git a/src/main.cpp b/src/main.cpp index ceabb96..0052d01 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,10 +4,9 @@ #include #include #include -#include +#include #include -#include namespace pins { @@ -28,8 +27,7 @@ const Color ACTIVE(0, 255, 0); void setColorCallback(const std_msgs::ColorRGBA& color); void errorCallback(const march_shared_resources::Error& error); -void instructionResponseCallback(const march_shared_resources::GaitInstructionResponse& response); -void currentGaitCallback(const std_msgs::String& gait); +void currentStateCallback(const march_shared_resources::CurrentState& state); void writeColor(Color color); // High voltage is enabled when HIGH and disabled when LOW @@ -44,10 +42,8 @@ ros::Publisher button_pub("/march/emergency_button/pressed", &button_msg); ros::Subscriber color_sub("/march/rgb_led/set_color", &setColorCallback); ros::Subscriber error_sub("/march/error", &errorCallback); -ros::Subscriber response_sub("/march/input_device/" - "instruction_response", - &instructionResponseCallback); -ros::Subscriber current_gait_sub("/march/gait/current", ¤tGaitCallback); +ros::Subscriber current_state_sub("/march/gait_selection/current_state", + ¤tStateCallback); void setColorCallback(const std_msgs::ColorRGBA& color) { @@ -59,17 +55,16 @@ void errorCallback(const march_shared_resources::Error& /* error */) writeColor(colors::ERROR); } -void instructionResponseCallback(const march_shared_resources::GaitInstructionResponse& response) +void currentStateCallback(const march_shared_resources::CurrentState& state) { - if (response.result == response.GAIT_FINISHED) + if (state.state_type == state.IDLE) { writeColor(colors::IDLE); } -} - -void currentGaitCallback(const std_msgs::String& /* gait */) -{ - writeColor(colors::ACTIVE); + else if (state.state_type == state.GAIT) + { + writeColor(colors::ACTIVE); + } } void writeColor(Color color) @@ -99,8 +94,7 @@ void setup() nh.advertise(button_pub); nh.subscribe(color_sub); nh.subscribe(error_sub); - nh.subscribe(response_sub); - nh.subscribe(current_gait_sub); + nh.subscribe(current_state_sub); } void loop()