rosserial how to read subscribed array mavros_msg/RCOut.h
Hello,
i was wondering if someone would be so kind to give me a clue of what the callback loop would look like for the bellow mavros_msgs? mainly how to read the info in the channels array
uint16_t RC;
void messageCb( const mavros_msgs::RCOut& msg){
RC =msg.channels_length;
}
this callback returns 16
uint16_t RC;
void messageCb( const mavros_msgs::RCOut& msg){
RC =msg.channels[16];
}
this callback returns 65535,
whats the correct way to extract any array from the callback routine? bellow is the mavros_msgs im trying to extract the channel array to use channel 1&3. i have searched for examples of how to sub to a array with rosserial but have not found anything education.
#define _ROS_mavros_msgs_RCOut_h
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include "ros/msg.h"
#include "std_msgs/Header.h"
namespace mavros_msgs
{
class RCOut : public ros::Msg
{
public:
std_msgs::Header header;
uint8_t channels_length;
uint16_t st_channels;
uint16_t * channels;
RCOut():
header(),
channels_length(0), channels(NULL)
{
}
virtual int serialize(unsigned char *outbuffer) const
{
int offset = 0;
offset += this->header.serialize(outbuffer + offset);
*(outbuffer + offset++) = channels_length;
*(outbuffer + offset++) = 0;
*(outbuffer + offset++) = 0;
*(outbuffer + offset++) = 0;
for( uint8_t i = 0; i < channels_length; i++){
*(outbuffer + offset + 0) = (this->channels[i] >> (8 * 0)) & 0xFF;
*(outbuffer + offset + 1) = (this->channels[i] >> (8 * 1)) & 0xFF;
offset += sizeof(this->channels[i]);
}
return offset;
}
virtual int deserialize(unsigned char *inbuffer)
{
int offset = 0;
offset += this->header.deserialize(inbuffer + offset);
uint8_t channels_lengthT = *(inbuffer + offset++);
if(channels_lengthT > channels_length)
this->channels = (uint16_t*)realloc(this->channels, channels_lengthT * sizeof(uint16_t));
offset += 3;
channels_length = channels_lengthT;
for( uint8_t i = 0; i < channels_length; i++){
this->st_channels = ((uint16_t) (*(inbuffer + offset)));
this->st_channels |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
offset += sizeof(this->st_channels);
memcpy( &(this->channels[i]), &(this->st_channels), sizeof(uint16_t));
}
return offset;
}
const char * getType(){ return "mavros_msgs/RCOut"; };
const char * getMD5(){ return "52cacf104bab5ae3b103cfe176590713"; };
};
}
#endif
Asked by plantface on 2022-09-19 04:29:18 UTC
Answers
Let's look at the message definition of mavros_msgs/RCOut:
# RAW Servo out state
std_msgs/Header header
uint16[] channels
It means that the mavros_msgs/RCOut
- Contains a standard header.
- Contains an array of channel.
- By seeing the message definition, we do not know how many elements this array contains. In other words, we do not know the number of channels.
- However, we know that the datatype of this array which is unsigned short integer.
Therefore, inside the subscriber function, the safest option is to iterate over the channels
array. However, as @Mike Scheutzow mentioned in the comment below, we can not call size()
on an array. Instead, we have to use the generated variable channels_length
. Please read Section 2.4 Arrays for information on arrays. Below is a sample code to iterate over channels
array:
nh.loginfo("Printing channels");
void messageCb(const mavros_msgs::RCOut& msg){
for(size_t i = 0; i < msg.channels_length; i++);
nh.loginfo(msg.channels[i]);
}
Please use the above snippet only for debugging purposes. Once you have confirmed the values and corresponding indices, please disable/remove it, as the page Logging in rosserial suggests that this kind of logging is very expensive.
Asked by ravijoshi on 2022-09-20 03:48:47 UTC
Comments
@ravijoshi the client side of a rosserial app uses special rules, intended to conserve RAM on tiny cpus. You can't call size()
on an array; instead an extra variable is automatically created in the message to hold the array length. In this particular example, the variable is channel_length
.
Asked by Mike Scheutzow on 2022-09-20 16:04:38 UTC
Thank you for you reply! i have some further question if you don't mind? Is ROS_INFO_STREAM a function of rosserial?
for(size_t i = 0; i < msg.channels.size(); i++);
The for() does not compile on Arduino ide due to
request for member 'size' in 'msg.mavros_msgs::RCOut::channels', which is of non-class type 'uint16_t* const' {aka 'short unsigned int* const'}
Im not understanding the error could you point me in the best direction?
best regards
Asked by plantface on 2022-09-20 16:27:40 UTC
Thanks @Mike Scheutzow for your wonderful comment. I updated the answer accordingly. Thanks again for the kind review! BTW, the channel_length
should be channels_length
, as per the documentation.
Asked by ravijoshi on 2022-09-20 22:46:34 UTC
@plantface: I did not realize that the question is targeted for rosserial
. My bad! Thanks to @Mike Scheutzow 's supervision, we can understand the errors you have reported in the comment above. Please see the updated answer.
Asked by ravijoshi on 2022-09-20 22:52:25 UTC
@Mike Scheutzow @ravijoshi
Thank you so much for your guidance! nh.loginfo(msg.channels[i]); This doesn't do much for some reason as it output black lines. Serial.print worked with the correct out put.
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
#include <mavros_msgs/RCOut.h>
ros::NodeHandle nh;
uint16_t RC[16];
void messageCb(const mavros_msgs::RCOut& msg){
for(size_t i = 0; i < msg.channels_length; i++){
RC[i]=msg.channels[i];}
}
ros::Subscriber<mavros_msgs::RCOut> sub("mavros/rc/out", messageCb );
std_msgs::UInt16MultiArray str_msg;
ros::Publisher chatter("Recieved", &str_msg);
void setup()
{
nh.getHardware()->setBaud(57600);
nh.initNode();
str_msg.data_length=16;
nh.advertise(chatter);
nh.subscribe(sub);
}
void loop()
{
str_msg.data = RC;
chatter.publish( &str_msg );
nh.spinOnce();
delay(50);
}
This code looks to off worked, publishing the correct information, hopefully this might help someone one day too.
thanks again!
Asked by plantface on 2022-09-21 03:42:40 UTC
Comments
Glad you made it work. BTW, the nh.loginfo( ... )
is just for printing as you noticed correctly. Serial.print
is a good alternative for doing the same.
Asked by ravijoshi on 2022-09-21 04:03:54 UTC
Comments
May I request you to please:
Then we can help you.
ROS Answers is not a place where we expect other people to just do your work for you. Of course, we help you try to get your work done, but we have to see a little effort on your side first.
Asked by ravijoshi on 2022-09-19 23:31:07 UTC
ok to clarify i have searched but have found no example for within rosserial,
my expectation is to receive a array[16] i think?
this returns 16 channels
this callback returns 65535,
i am confused to what uint16_t * channels; means
am i missing a for loop or some means of structuring the data in the callback?
best regards
Asked by plantface on 2022-09-20 01:56:26 UTC