CraftDuino v2.0
  • - это CraftDuino - наш вариант полностью Arduino-совместимой платы.
  • CraftDuino - настоящий конструктор, для очень быстрого прототипирования и реализации идей.
  • Любая возможность автоматизировать что-то с лёгкостью реализуется с CraftDuino!
Просто добавьте CraftDuino!

Попытки запуска 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_<ArduinoHardware, 2, 2, 124, 124> NodeHandle;     

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

/* Node Handle */
  template<class Hardware,
           int MAX_SUBSCRIBERS=25,
           int MAX_PUBLISHERS=25,
           int INPUT_SIZE=512,
           int OUTPUT_SIZE=512>
  class NodeHandle_
  {
    protected:
      Hardware hardware_;
      NodeOutput<Hardware, OUTPUT_SIZE> 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; i<l+6; i++)
          chk += message_out[i];
        l += 6;
        message_out[l++] = 255 - (chk%256);

        hardware_->write(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; i<res; i++){
						printf("%02X ", (unsigned char)buf[i]);
						if(i>0 && (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<l+6; i++)
          chk += message_out[i];
        l += 6;
        message_out[l++] = 255 - (chk%256);

пакет с меткой времени:
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_<ArduinoHardware, 2, 2, 124, 124> 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_<ArduinoHardware, 2, 2, 64, 64> 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


Ссылки:
ros.org/wiki/rosserial/Overview/Protocol

По теме:
LaunchPad MSP-EXP430G2 от Texas Instruments — альтернатива Arduino?
Rosserial — связь двух Arduino через ROS
  • +1
  • 5 октября 2012, 10:25
  • noonv

Комментарии (1)

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

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

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

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

snop

  • 20 ноября 2012, 20:09

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.