Попытки запуска rosserial на LaunchPad MSP-EXP430G2

Так как пока других вариантов использования прибывшего контроллера LaunchPad MSP-EXP430G2 от Texas Instruments не придумалось — я решил, для развлечения, портировать для него rosserial — протокол точка-точка для общения с ROS.

Скопируем ros_lib из Arduino IDE в energia\hardware\msp430\libraries\

у меня под рукой оказалась только старая версия для ROS electric

первым делом поправим

ArduinoHardware.h

#ifndef ROS_ARDUINO_HARDWARE_H_
#define ROS_ARDUINO_HARDWARE_H_

#include <Arduino.h> // Energia
#include <HardwareSerial.h>

class ArduinoHardware {
  public:
    ArduinoHardware(HardwareSerial* io , long baud= 9600){
      iostream = io;
      baud_ = baud;
    }
    ArduinoHardware()
    {
      iostream = &Serial;
      baud_ = 9600;
    }

Да-да, Arduino-вские 57600 придётся заменить на LaunchPad-кие 9600 :/
Серъёзные данные не прокачаешь, но и МК у нас так себе — так что для простеньких сообщений пойдёт 😉

попробуем собрать идущий в комплекте пример HelloWorld

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */

#include <ros.h>
#include <std_msgs/String.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

void setup()
{
  nh.initNode();
  nh.advertise(chatter);
}

void loop()
{
  str_msg.data = hello;
  chatter.publish( &str_msg );
  nh.spinOnce();
  delay(1000);
}

запускаем компиляцию и смотрим на появляющиеся ошибки 🙂

не находится стандартная функция realloc:

energia\hardware\msp430\libraries\ros_lib/ros/../rosserial_msgs/RequestParam.h:152:87: error: 'realloc' was not declared in this scope

а потом не находится round:

time.h:57:106: error: 'round' was not declared in this scope

то что у msp430 нету realloc() — вполне логично — памяти то кот наплакал.
Даже в дуиновком классе String вместо realloc() используется malloc() (в методе String::changeBuffer()).
Реализуем недостающие методы самостоятельно %)

подключим в ros/msg.h и ros/time.h

#include "ros2energia.h"

содержание ros2energia.h:

/*
 * Copyright (c) 2012, RoboCraft
 * All rights reserved.
 */

#ifndef ROS_2_ENERGIA_H_
#define ROS_2_ENERGIA_H_

#include <Energia.h> // Energia

#define round(x)     ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))

void *realloc( void *ptr, size_t size );

#endif

содержание ros2energia.сpp:

/*
 * Copyright (c) 2012, RoboCraft
 * All rights reserved.
 */

#include "ros2energia.h"

void *realloc( void *ptr, size_t size )
{
   if(ptr) {
		if(size){
			free(ptr);
			ptr = malloc(size);
		}
		else{ //size==0
			free(ptr);
			ptr = NULL;
		}
   }
   else { //ptr==0
	if(size)
		return malloc(size);
   }
   return ptr;
}

Теперь сборка проходит без ошибок, но, к сожалению, прошивка не влезает в память 🙁

energia/hardware/tools/msp430/bin/../lib/gcc/msp430/4.6.3/../../../../msp430/bin/ld.exe: HelloWorld.cpp.elf section `.bss' will not fit in region `ram'
energia/hardware/tools/msp430/bin/../lib/gcc/msp430/4.6.3/../../../../msp430/bin/ld.exe: region `ram' overflowed by 844 bytes
collect2: ld returned 1 exit status

попробуем поправить ros.h, добавив:

#elif defined(__MSP430G2553)

  typedef NodeHandle_ NodeHandle;

— тем самым мы уменьшаем число подписчиков, писателей и размеры буферов приёма-передачи сообщений

/* Node Handle */
  template
  class NodeHandle_
  {
    protected:
      Hardware hardware_;
      NodeOutput no_;

та-да!
Собралось!!!

Binary sketch size: 5 180 bytes (of a 16 384 byte maximum)

попробуем прошить и посмотрим, что будет писаться в порт 🙂

ничего 🙁
разбираемся дальше 🙂

добираемся до ros/node_output.h
видим буфер для выдачи сообщений и функцию отправки, которая возвращает число переданных байт.

для отладки добавим в скетч светодиодной индикации

/*
 * rosserial Publisher Example with blink
 * Prints "hello world!"
 */

#include <ros.h>
#include <std_msgs/String.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

int res=0;

void setup()
{
  pinMode(RED_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  nh.initNode();
  nh.advertise(chatter);
}

void loop()
{
  res=0;
  digitalWrite(GREEN_LED, HIGH);
  str_msg.data = hello;
  res = chatter.publish( &str_msg );
  nh.spinOnce();
  if(res>0)
      digitalWrite(RED_LED, HIGH);
  delay(500);
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(RED_LED, LOW);
  delay(500);
}

т.о. зелёный светодиод будет мигать, а красный должен мигать только при успешной передаче данных в порт.

гм… красный не загорается — значит с записью действительно проблемы :/

ага — в начале метода NodeOutput::publish есть проверка булевого флага configured_, который выставляется методом setConfigured()

class NodeOutput : public NodeOutput_{

	  // ...

	  void setConfigured(bool b){
        configured_ =b;
      }
      bool configured(){return configured_;};

      virtual int publish(int id, Msg * msg){
        if(!configured_) return 0;

        /* serialize message */
        int l = msg->serialize(message_out+6);

        /* setup the header */
        message_out[0] = 0xff;
        message_out[1] = 0xff;
        message_out[2] = (unsigned char) id&255;
        message_out[3] = (unsigned char) id>>8;
        message_out[4] = (unsigned char) l&255;
        message_out[5] = ((unsigned char) l>>8);

        /* calculate checksum */
        int chk = 0;
        for(int i =2; iwrite(message_out, l);
        return l;
      }

в class NodeHandle_ , этот метод вызывается дважды:

	virtual void spinOnce(){
        /* restart if timed out */
        unsigned long c_time = hardware_.time();
        if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
            no_.setConfigured(false);
         }
	...
	void negotiateTopics()
      {
        no_.setConfigured(true);

т.о. сообщения будут печататься только при общении с ROS-овским приложением.

Это можно проверить, заменив в обработке тайм-аута

no_.setConfigured(false);

на

no_.setConfigured(true);

пересобираем, прошиваем, открываем терминал COM-порта и чуть-чуть подождём 🙂
через несколько секунд красный светодиод замигает, а в терминале увидим иероглифы, обрамляющие наше сообщение hello world!

Вернём всё как было и посмотим питоновский код клиента —
rosserial\rosserial_python\src\rosserial_python\SerialClient.py

def requestTopics(self):
        """ Determine topics to subscribe/publish. """
        self.port.flushInput()
        # request topic sync
        self.port.write("\xff\xff\x00\x00\x00\x00\xff")

быстренько набросаем программу, пишущую в порт этот запрос и считывающую ответ:

в формате Gtest:

TEST(Serial, WriteCOM) {

	Serial serial;

	serial.open("COM7", 9600);

	if(!serial.connected()){
		FAIL()<<"cant open serial port!";
	}

	char buf[1024];
	int res = 0;
	while(1){

		char wbuf[] = {0xff, 0xff, 0x00, 0x00, 0x00, 0x00 ,0xff};

		serial.write(wbuf, sizeof(wbuf));

		if( serial.waitInput(1000) > 0){
			if( (res = serial.available()) > 0 ){
				if( res = serial.read(buf, res) ){
					printf("[i] read data(%d): \n", res);
					for(int i=0; i0 && (i+1)%16 == 0) {
							printf("\t");
							for(int j=i-15; j<=i; j++){
								printf("%c", buf[j]);
							}
							printf("\n");
						}
					}
					printf("\n");
					res = 0;
				}
			}
		}
		Sleep(1000);
	}
}

хм... получаются сообщения вида:

[i] read data(85):
FF FF 66 00 10 00 00 00 00 00 68 65 6C 6C 6F 20 	  f       hello
77 6F 72 6C 64 21 0C FF FF 0A 00 08 00 00 00 00 	world!   
00 00 00 00 00 ED FF FF 00 00 28 00 66 07 00 00 	     ?    ( f
00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64 	  chatter    std
5F 6D 73 67 73 2F 53 74 72 69 6E 04 00 00 00 00 	_msgs/Strin
20 20 20 20 49

что-то название типа String полностью не пролазит 🙁

разберём пакеты:

пакет hello world!:

FF FF 66 00 10 00 00 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 0C

FF FF - два ff-а заголовка
00 66 - 102 - id Publisher-а chatter

присваивается в NodeHandle_::advertise()

	p.id_ = i+100+MAX_SUBSCRIBERS;
	// (0+100+MAX_SUBSCRIBERS), где MAX_SUBSCRIBERS=2

00 10 = 16 - длина сериализованного сообщения

00 00 00 00 68 65 6C 6C 6F 20 77 6F 72 6C 64 21 - сериализованное сообщения типа std_msgs::String

String::serialize:
 virtual int serialize(unsigned char *outbuffer)
    {
      int offset = 0;
      long * length_data = (long *)(outbuffer + offset);
      *length_data = strlen( (const char*) this->data);
      offset += 4;
      memcpy(outbuffer + offset, this->data, *length_data);
      offset += *length_data;
      return offset;
    }

strlen() не отработал? О_о

0C - контрольная сумма, высчитывается в NodeOutput::publish()

		/* calculate checksum */
        int chk = 0;
        for(int i =2; i
пакет с меткой времени:
FF FF 0A 00 08 00 00 00 00 00 00 00 00 00 ED
00 0A = 10 - rosserial_msgs::TopicInfo::ID_TIME
	  enum { ID_PUBLISHER = 0 };
      enum { ID_SUBSCRIBER = 1 };
      enum { ID_SERVICE_SERVER = 2 };
      enum { ID_SERVICE_CLIENT = 3 };
      enum { ID_PARAMETER_REQUEST = 4 };
      enum { ID_LOG = 5 };
      enum { ID_TIME = 10 };

00 08 - 8 - длина

00 00 00 00 00 00 00 00 - вместо данных пустой пакет :/

ED - контрольная сумма
пакет с типом отправления:
FF FF 00 00 28 00 66 07 00 00 00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64 5F 6D 73 67 73 2F 53 74 72 69 6E 04 00 00 00 00 20 20 20 20 49

похоже, результат метода NodeHandle_::negotiateTopics()
	void negotiateTopics()
      {
        no_.setConfigured(true);

        rosserial_msgs::TopicInfo ti;
        int i;
        for(i = 0; i < MAX_PUBLISHERS; i++)
        {
          if(publishers[i] != 0) // non-empty slot
          {
            ti.topic_id = publishers[i]->id_;
            ti.topic_name = (char *) publishers[i]->topic_;
            ti.message_type = (char *) publishers[i]->msg_->getType();
            no_.publish( TOPIC_PUBLISHERS, &ti );
          }
        }
        for(i = 0; i < MAX_SUBSCRIBERS; i++)
        {
          if(receivers[i] != 0) // non-empty slot
          {
            ti.topic_id = receivers[i]->id_;
            ti.topic_name = (char *) receivers[i]->topic_;
            ti.message_type = (char *) receivers[i]->getMsgType();
            no_.publish( TOPIC_SUBSCRIBERS, &ti );
          }
        }
      }

00 00 = 0 - TOPIC_PUBLISHERS
из ros\rosserial_ids.h
#define TOPIC_NEGOTIATION   0
#define TOPIC_PUBLISHERS    0
#define TOPIC_SUBSCRIBERS   1
#define TOPIC_SERVICES      2

00 28 = 40 - длина сообщения
66 07 00 00 00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64 5F 6D 73 67 73 2F 53 74 72 69 6E 04 00 00 00 00 20 20 20 20 - сообщение
rosserial_msgs::TopicInfo
      enum { ID_PUBLISHER = 0 };
      enum { ID_SUBSCRIBER = 1 };
      enum { ID_SERVICE_SERVER = 2 };
      enum { ID_SERVICE_CLIENT = 3 };
      enum { ID_PARAMETER_REQUEST = 4 };
      enum { ID_LOG = 5 };
      enum { ID_TIME = 10 };

    virtual int serialize(unsigned char *outbuffer)
    {
      int offset = 0;
      union {
        unsigned int real;
        unsigned int base;
      } u_topic_id;
      u_topic_id.real = this->topic_id;
      *(outbuffer + offset + 0) = (u_topic_id.base >> (8 * 0)) & 0xFF;
      *(outbuffer + offset + 1) = (u_topic_id.base >> (8 * 1)) & 0xFF;
      offset += sizeof(this->topic_id);
      long * length_topic_name = (long *)(outbuffer + offset);
      *length_topic_name = strlen( (const char*) this->topic_name);
      offset += 4;
      memcpy(outbuffer + offset, this->topic_name, *length_topic_name);
      offset += *length_topic_name;
      long * length_message_type = (long *)(outbuffer + offset);
      *length_message_type = strlen( (const char*) this->message_type);
      offset += 4;
      memcpy(outbuffer + offset, this->message_type, *length_message_type);
      offset += *length_message_type;
      long * length_md5_checksum = (long *)(outbuffer + offset);
      *length_md5_checksum = strlen( (const char*) this->md5_checksum);
      offset += 4;
      memcpy(outbuffer + offset, this->md5_checksum, *length_md5_checksum);
      offset += *length_md5_checksum;
      return offset;
    }

49 - контрольная сумма
Сравним выдачу от msp430g2553 с данными на аналогичный запрос от Arduino Mega.

msp430g2553
[i] read data(85):
FF FF 66 00 10 00 00 00 00 00 68 65 6C 6C 6F 20 	  f       hello
77 6F 72 6C 64 21 0C FF FF 0A 00 08 00 00 00 00 	world!   
00 00 00 00 00 ED FF FF 00 00 28 00 66 07 00 00 	     ?    ( f
00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64 	  chatter    std
5F 6D 73 67 73 2F 53 74 72 69 6E 04 00 00 00 00 	_msgs/Strin
20 20 20 20 49

для сравнения - пример HelloWorld на Arduino Mega:
typedef NodeHandle_ NodeHandle;
[i] read data(81):
FF FF 66 00 10 00 0C 00 00 00 68 65 6C 6C 6F 20         ¦  f > +   hello
77 6F 72 6C 64 21 00 FF FF 0A 00 08 00 00 00 00          world!   
00 00 00 00 00 ED FF FF 00 00 24 00 66 00 07 00               э    $ f
00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64            chatter0   std
5F 6D 73 67 73 2F 53 74 72 69 6E 67 00 00 00 00         d_msgs/String
6A

typedef NodeHandle_ NodeHandle;
[i] read data(81):
FF FF 7D 00 10 00 0C 00 00 00 68 65 6C 6C 6F 20         ¦  } > +   hello
77 6F 72 6C 64 21 E9 FF FF 0A 00 08 00 00 00 00          world!щ  
00 00 00 00 00 ED FF FF 00 00 24 00 7D 00 07 00               э    $ }
00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64            chatter0   std
5F 6D 73 67 73 2F 53 74 72 69 6E 67 00 00 00 00         d_msgs/String
53

похоже, существует сдвиг записи значения длины строки на 1 байт, а так же определение длины строки поля md5_checksum
добавим в NodeHandle_::negotiateTopics()

строчку
ti.md5_checksum = "";
(присваивание 0 - оказалось плохой идеей 😉
в результате, получили хотя бы совпадение длины:
[i] read data(81):
FF FF 66 00 10 00 00 00 00 00 68 65 6C 6C 6F 20 	  f       hello
77 6F 72 6C 64 21 0C FF FF 0A 00 08 00 00 00 00 	world!   
00 00 00 00 00 ED FF FF 00 00 24 00 66 07 00 00 	     ?    $ f
00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64 	  chatter    std
5F 6D 73 67 73 2F 53 74 72 69 6E 00 00 00 00 00 	_msgs/Strin
D1

при добавлении в TopicInfo::serialize() смещения на 1:
long * length_topic_name = (long *)(outbuffer + offset+1);
      *length_topic_name = strlen( (const char*) this->topic_name);
      offset += 4;
      memcpy(outbuffer + offset, this->topic_name, *length_topic_name);
      offset += *length_topic_name;
      long * length_message_type = (long *)(outbuffer + offset+1);
      *length_message_type = strlen( (const char*) this->message_type);
      offset += 4;
      memcpy(outbuffer + offset, this->message_type, *length_message_type);
      offset += *length_message_type;
      long * length_md5_checksum = (long *)(outbuffer + offset+1);

[i] read data(81):
FF FF 66 00 10 00 00 00 00 07 68 65 6C 6C 6F 20 	  f       hello
77 6F 72 6C 64 21 05 FF FF 0A 00 08 00 00 00 00 	world!    
00 00 00 00 00 ED FF FF 00 00 24 00 66 00 00 07 	     ?    $ f
00 00 63 68 61 74 74 65 72 0F 00 00 00 73 74 64 	  chatter    std
5F 6D 73 67 73 2F 53 74 72 69 6E 67 00 00 00 00 	_msgs/String
6A

У кого ещё есть какие идеи? 🙂
Уменьшить буферы?

Уменьшение буферов даже до 64 ничего не принесло:
typedef NodeHandle_ NodeHandle;
Справка по формату протокола rosserial
формат пакета протокола rosserial:
  1st Byte - Sync Flag (Value: 0xff)
  2nd Byte - Sync Flag (Value: 0xff)
  3rd Byte - Topic ID - Low Byte
  4th Byte - Topic ID - High Byte
  5th Byte - Message Length (N) - Low Byte
  6th Byte - Message Length (N) - High Byte
  N Bytes  - Serialized Message Data
  Byte N+7 - Checksum

Запрос для получения информации о топиках:
0xff 0xff 0x00 0x00 0x00 0x00 0xff
Последовательность ответных пакетов (тип сообщения - rosserial_msgs/TopicInfo), каждый из которых содержит информацию о соответствующем топике - набор сериализованных данных:
  uint16 topic_id
  string topic_name
  string message_type
  string md5sum
  int32 buffer_size

Проверка размеров переменных в msp430g2553
char hello[13] = "hello world!";

void setup() {
  // P1_0 - red LED
  // P1_6 - green LED
  pinMode(P1_0, OUTPUT);
  pinMode(P1_6, OUTPUT);

  Serial.begin(9600);
  Serial.print("char\t");
  Serial.println(sizeof(char));
  Serial.print("int\t");
  Serial.println(sizeof(int));
  Serial.print("long\t");
  Serial.println(sizeof(long));
  Serial.print("size_t\t");
  Serial.println(sizeof(size_t));
  Serial.print("float\t");
  Serial.println(sizeof(float));
  Serial.print("strlen\t");
  Serial.println(strlen(hello));
}

в терминале получаем:
char	1
int 	2
long	4
size_t	2
float	4
strlen	12

Ссылки

http://ros.org/wiki/rosserial/Overview/Protocol
По теме

LaunchPad MSP-EXP430G2 от Texas Instruments - альтернатива Arduino?

Rosserial - связь двух Arduino через ROS

0 комментариев на «“Попытки запуска rosserial на LaunchPad MSP-EXP430G2”»

snop:

2012-11-20 в 20:09

> Да-да, Arduino-вские 57600 придётся заменить на LaunchPad-кие 9600 :/

Если переключить «UTCTL0 |= SSEL1;», то можно и 230400 бод ставить…

> Серъёзные данные не прокачаешь, но и МК у нас так себе

Ну, в сравнении с Ардуиной разницу в производительности едва ли можно заметить. А вот разница в энергопотреблении впечатляет! (10:1 в пользу MSP430)

Войдите, чтобы ответить

Добавить комментарий Отменить ответ

Для отправки комментария вам необходимо авторизоваться.

Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

если у вас черное окно попробуйте так. #include <opencv2/opencv.hpp> #include<iostream> #include <stdlib.h> #include <stdio.h> using namespace cv; using namespace std;…

Для Python 3.4: pip3 install tensorflow-1.1.0-cp34-cp34m-linux_armv7l.whl https://github.com/samjabrahams/tensorflow-on-raspberry-pi

PORTB = (PORTB & 0b11100111) | (i & 0b00000011); А почему Вы обнуляете биты 3 и 4, а заполняете биты…