1. 程式人生 > >ROS與stm32L476RG nucleo 基於mbed開發,使用rosserial_python包通訊

ROS與stm32L476RG nucleo 基於mbed開發,使用rosserial_python包通訊

使用rosserial_python包.

報通訊錯誤,因為原serial_node.py及SerialClient作了修改,刪除了fix_pyserial_for_test項.

使用ros自帶的serial_node.py及SerialClient.py,更改一下py名字以區別,import時報錯

'module' object is not callable

原因為Python匯入模組的方法有兩種:import module 和 from module import,區別是前者所有匯入的東西使用時需加上模組名的限定,而後者不要:

>>> import Person
>>> person = Person.Person('dnawo','man')
>>> print person.Name

>>> from Person import *
>>> person = Person('dnawo','man')
>>> print person.Name

https://blog.csdn.net/chenbo163/article/details/52526391

將spq_serial_node中的import spq_SerialClient 改為

from spq_SerialClient import *  即OK

改好後,將檔案置於/opt/ros/kinetic/lib/rosserial_python下,方便使用

在使用rosserial_python時, sudo chmod 777 /dev/ttyACM0 給埠許可權,然後rosserial_python serial_node.py _:port=/dev/ttyACM0...

會報link error之類的,出現連不上的情況,是因為stm32 nucleo的程式裡設定了wait_ms之類的延時函式,試了下在進入這種延時函式後可能會無法連線,微控制器cpu不處理.解決方法是加上其他觸發條件,如按鍵,等連線建立了再執行ros部分的功能.

在連上後,正常收發訊息時,會報Lost sync with device, restarting...   

http://bediyap.com/robotics/rosserial-arduino/ 據說是RAM太小?

因為程式中訂閱了robot_pose話題,估計是此話題頻率太高,硬體資源不夠造成.

還沒有找到解決辦法,但是似乎這個錯誤沒有什麼影響?

報這個錯有機率會直接宕機,重新訂閱一個頻率較低的pose topic.

以及如何修改buffer size,啟動rosserial_python的serial_node時顯示現在是512bytes?

加入舵機控制,SG90 9G舵機,PWM週期20ms,0.5-2.5ms控制0-180度轉動範圍,實際設定0.75-2.25防燒舵機

#include "mbed.h"

#include <ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h>


DigitalOut myled(LED1);
DigitalIn myButton(USER_BUTTON);
InterruptIn buttonInt(USER_BUTTON);


//CAMERA
#define BASEPWMLEN 20000
PwmOut pwm_1(D3);
PwmOut pwm_2(D5);




/*
Timeout timer_1;
Timeout timer_2;
DigitalOut my_pwm_1(D6); // IO used by pwm_io function
DigitalOut my_pwm_2(D9); // IO used by pwm_io function
int on_delay = 0;
int off_delay = 0;
void toggleOff_1(void);
void toggleOff_2(void);
void toggleOn_1(void){
    my_pwm_1 = 1;
    timer_1.attach_us(toggleOff_1, on_delay);
}
void toggleOn_2(void){
    my_pwm_2 = 1;
    timer_2.attach_us(toggleOff_2, on_delay);
}
void toggleOff_1(void){
    my_pwm_1 = 0;
    timer_1.attach_us(toggleOn_1, off_delay);
}
void toggleOff_2(void){
    my_pwm_2 = 0;
    timer_2.attach_us(toggleOn_2, off_delay);
}


// p_us = signal period in micro_seconds
// dc   = signal duty-cycle (0.0 to 1.0)
void pwm_io(int pwmindex,int p_us, float dc) {
    if(pwmindex==1){
      timer_1.detach();
    if ((p_us == 0) || (dc == 0)) {
        my_pwm_1 = 0;
        return;
    }
    if (dc >= 1) {
        my_pwm_1 = 1;
        return;
    }
    on_delay = (int)(p_us * dc);
    off_delay = p_us - on_delay;
    toggleOn_1();
    }
    else if(pwmindex==2){
      timer_2.detach();
    if ((p_us == 0) || (dc == 0)) {
        my_pwm_2 = 0;
        return;
    }
    if (dc >= 1) {
        my_pwm_2 = 1;
        return;
    }
    on_delay = (int)(p_us * dc);
    off_delay = p_us - on_delay;
    toggleOn_2();
    }
}
*/




//std_msgs::String orderResult;
std_msgs::String stmOrder;
std_msgs::String stmTest;
geometry_msgs::Pose robotPose;
geometry_msgs::Twist motoCtrl;
//std_msgs::String getPointList;
//std_msgs::String pointList;
//bool FLAG_GET_A_RESULT=false;
bool FLAG_SENT_A_ORDER=false;
enum BASESTATE{
  excutegoal,waitgoal,waitresult
};
enum RESULTSTATE{
  none,succeeded,aborted
};
RESULTSTATE RESULT_STATE=none;
//=1 SUCCEEDED,=-1 ABORTED
BASESTATE BASE_STATE=waitgoal;


int test_begin=0;


void order_result_callback(const std_msgs::String& result);
ros::Publisher pub_order("/stm_order", &stmOrder);
ros::Publisher pub_test("/stm_test", &stmTest);
ros::Subscriber<std_msgs::String> sub_orderResult("/s_actionlib/s_movetopoint/GoalState",&order_result_callback);
ros::NodeHandle nodehandle;
//point list below
class placeCoordinates
{
public:
  int label;
  float spaceX;
  float spaceY;
  float spaceZ;
  float eulerX;
  float eulerY;
  float eulerZ;
  float eulerW;
  char* discribe;
};
placeCoordinates placecoordinates[]={
  1,    0.22,   2.41,   0,  0,0,1,0,    "A",
  2,    -3.11,  -7.25,  0,  0,0,1,0,    "B",
  3,    8.99,   -1.58,  0,  0,0,1,0,    "C",
  4,    0.03,   0.43,   0,  0,0,1,0,    "D",
  5,    7.16,   -0.73,  0,  0,0,1,0,    "E",
  6,    3.92,   1.85,   0,  0,0,1,0,    "F",
  7,    -0.37,  -7.67,  0,  0,0,1,0,    "G"
};
/*
int strcmp(const char *dest, const char *source)  
{  
   assert((NULL != dest) && (NULL != source));  
   while (*dest && *source && (*dest == *source))  
           {  
                    dest ++;  
                   source ++;  
           }  
   return *dest - *source;  
//如果dest > source,則返回值大於0,如果dest = source,則返回值等於0,如果dest  < source ,則返回值小於0。  

*/
    
void order_result_callback(const std_msgs::String& result)
{
  
  //  stmTest.data=result.data;
    //  pub_test.publish(&stmTest);
      
      /*
  if(result.data=="SUCCEEDED"){
    RESULT_STATE=succeeded;
    BASE_STATE=waitgoal;
  }
  else if(result.data=="ABORTED"){
    RESULT_STATE=aborted;
    BASE_STATE=waitgoal;
  }
  else if(result.data=="WAITGOAL"){
    BASE_STATE=waitgoal;
  }
  else if(result.data=="WAITRESULT"){
    BASE_STATE=waitresult;
  }
  else;
  */
  if(strcmp(result.data,"SUCCEEDED")==0){
    RESULT_STATE=succeeded;
    BASE_STATE=waitgoal;
  }
  else if(strcmp(result.data,"ABORTED")==0){
    RESULT_STATE=aborted;
    BASE_STATE=waitgoal;
  }
  else if(strcmp(result.data,"WAITGOAL")==0){
    BASE_STATE=waitgoal;
  }
  else if(strcmp(result.data,"WAITRESULT")==0){
    BASE_STATE=waitresult;
  }
  else;
  return;
}
void base_pose_callback(const geometry_msgs::Pose& pose)
{
  //pose回撥函式
  robotPose=pose;
  //test
   //stmTest.data="getpose";
   //pub_test.publish(&stmTest);
}
float degree_change(float degree)
{
  return degree*((2.5-0.5)*1000/180)/BASEPWMLEN;
}
float degree_direct(float degree)
{
  return (((degree/180.0)*(2.5-0.5)*1000.0)+0.5*1000.0)/BASEPWMLEN;
}


float pwm_rate_limit(float input)
{
  //0.5ms/20ms<  <2.5ms/20ms
  if(input<=(0.75/20.0)) return 0.75/20.0;
  if(input>=(2.25/20.0)) return 2.25/20.0;
  return input; 
}


//angular-->pwm_1,liner-->pwm_2
//Twist. angular水平,linear垂直,x為直接控制命令,正負決定方向,大小決定速度,y為以當前為基準需旋轉的角度值,正負決定方向,大小決定角度,
void moto_ctrl_callback(const geometry_msgs::Twist& motocmd)
{
  float cur_pwm_1_rate,cur_pwm_2_rate;
  cur_pwm_1_rate=pwm_1.read();
  cur_pwm_2_rate=pwm_2.read();


   if(motocmd.angular.x!=0){
     
     float pl=pwm_rate_limit(cur_pwm_1_rate+motocmd.angular.x);
     char str[100];
     static int num=1;
    sprintf(str,"%f",pl);
    stmTest.data=str;
    pub_test.publish(&stmTest);
    
    pwm_1.write(pwm_rate_limit(cur_pwm_1_rate+motocmd.angular.x));
   }
   if(motocmd.linear.x!=0){
     
     float pl=pwm_rate_limit(cur_pwm_2_rate+motocmd.linear.x);
     char str[100];
     static int num=1;
    sprintf(str,"%f",pl);
    stmTest.data=str;
    pub_test.publish(&stmTest);
     
    pwm_2.write(pwm_rate_limit(cur_pwm_2_rate+motocmd.linear.x));
   }
   if(motocmd.angular.x==0&&motocmd.angular.y!=0){
     
        float pl=pwm_rate_limit(cur_pwm_1_rate+degree_change(motocmd.angular.y));
     char str[100];
     static int num=1;
    sprintf(str,"%f",pl);
    stmTest.data=str;
    pub_test.publish(&stmTest);
    
     pwm_1.write(pwm_rate_limit(cur_pwm_1_rate+degree_change(motocmd.angular.y)));
   }
   if(motocmd.linear.x==0&&motocmd.linear.y!=0){
     
     float pl=pwm_rate_limit(cur_pwm_1_rate+degree_change(motocmd.angular.y));
     char str[100];
     static int num=1;
    sprintf(str,"%f",pl);
    stmTest.data=str;
    pub_test.publish(&stmTest);
     
     pwm_2.write(pwm_rate_limit(cur_pwm_2_rate+degree_change(motocmd.linear.y)));
   }
}


/*
void point_list_callback(const placeCoordinates& pointlist)
{
}
*/
bool button_pressed()
{
  if(myButton==0)
  {
    myled=!myled;
    wait(0.02);
    if(myButton==0)
    {
      while(myButton==0);
      return true;
    }
  }
  else return false;
}
void reset_params()
{
  //FLAG_GET_A_RESULT=false;
  //orderResult.data="";
  FLAG_SENT_A_ORDER=false;
  RESULT_STATE=none;
  stmOrder.data="";
  BASE_STATE=waitgoal;
}
bool arrrive_point()
{
  if(RESULT_STATE==succeeded)
    return true;
  else return false;
}
bool go_point(const char * point)
{
   int trytimes=500;
   while(BASE_STATE!=waitresult&&trytimes>0){
    stmOrder.data = point;
    pub_order.publish(&stmOrder);
    
    /*
     //test
    char str[100];
    static int num=1;
    sprintf(str,"%d",trytimes);
    stmTest.data=str;
    num++;
    pub_test.publish(&stmTest);
    if(BASE_STATE==waitresult){
      stmTest.data="waitresult";
      pub_test.publish(&stmTest);
    }
    if(BASE_STATE==waitgoal){
      stmTest.data="waitgoal";
      pub_test.publish(&stmTest);
    }
    //test
    */
    
    trytimes--;
    nodehandle.spinOnce();
    wait_ms(500);
   }
   if(trytimes==0) return false;
   else return true;
}
void pub_err()
{
  stmOrder.data="ERROR";
  pub_order.publish(&stmOrder);
}




void button_callback()
{
  test_begin=1;
  myled=!myled;
    
    /*
  if(id==0){
  pwm_1.write(0.025);
  pwm_2.write(0.025);
  id++;
  }
  else if(id==1){
  pwm_1.write(0.05);
  pwm_2.write(0.05);
  id++;
  }
  else if(id==2){
  pwm_1.write(0.1);
  pwm_2.write(0.1);
  id=0;
  }
  */
}




int main()
{
    //ros::NodeHandle nodehandle;
    nodehandle.initNode();
    
    //sub_orderResult
    //ros::Subscriber<std_msgs::String> sub_orderResult("/s_actionlib/s_movetopoint/GoalState",&order_result_callback);
    nodehandle.subscribe(sub_orderResult);
    
    //sub_basePose
    ros::Subscriber<geometry_msgs::Pose> sub_basePose("/s_actionlib/s_movetopoint/RobotPose",&base_pose_callback); 
    nodehandle.subscribe(sub_basePose);
    
    //sub_motoCtrl
    ros::Subscriber<geometry_msgs::Twist> sub_motoCtrl("/s_motoCtrl",&moto_ctrl_callback); 
    nodehandle.subscribe(sub_motoCtrl);
    
    //pub_order
    nodehandle.advertise(pub_order);
    nodehandle.advertise(pub_test);
    
    //pub_getList
    //ros::Publisher pub_getList("/stm_order", &getPointList,3212);
    //nodehandle.advertise(pub_getList);
     //sub_pointList
    //ros::Subscriber<placeCoordinates*> sub_pointList("/s_actionlib/s_movetopoint/PointList",&point_list_callback,3203); 
    //nodehandle.advertise(pub_getList);
    
    reset_params();


    
    //CAMERA init
    pwm_1.period_ms(20);
    pwm_1.write(0.1);
    pwm_2.period_ms(20);
    pwm_2.write(0.07);
    
    while (1) {
       
      //example
      //釋出指令給移動底盤,目標點為A
      /*
      if(BASE_STATE==waitgoal&&test_begin==1){
        stmOrder.data="A";x
    pub_order.publish(&stmOrder);
      }
      if(BASE_STATE==waitresult&&test_begin==1){
    test_begin=0;
    FLAG_SENT_A_ORDER=true;
      }
      //example
      //如果底盤到達A點,則原地停留5s
      if(stmOrder.data=="A"&&FLAG_SENT_A_ORDER==true&&RESULT_STATE==succeeded){
    if(test_wait==1) {
      wait_ms(5000);
      test_wait=0;
    }
    if(BASE_STATE==waitgoal){
      stmOrder.data="B";
      pub_order.publish(&stmOrder);
    }
    if(BASE_STATE==waitresult){
      FLAG_SENT_A_ORDER=true;
      reset_params();
    }
      }
      else if(stmOrder.data=="B"&&FLAG_SENT_A_ORDER==true&&FLAG_GET_A_RESULT==true&&RESULT_STATE==1){
    wait_ms(5000);
    reset_params();
    stmOrder.data="C";
    pub_order.publish(&stmOrder);
    FLAG_SENT_A_ORDER=true;
      }
      else if(stmOrder.data=="C"&&FLAG_SENT_A_ORDER==true&&FLAG_GET_A_RESULT==true&&RESULT_STATE==1){
    wait_ms(5000);
    reset_params();
    stmOrder.data="D";
    pub_order.publish(&stmOrder);
    FLAG_SENT_A_ORDER=true;
      }
      else if(stmOrder.data=="D"&&FLAG_SENT_A_ORDER==true&&FLAG_GET_A_RESULT==true&&RESULT_STATE==1){
    wait_ms(5000);
    reset_params();
    stmOrder.data="A";
    pub_order.publish(&stmOrder);
    FLAG_SENT_A_ORDER=true;
      }
      else;
      */
      
      buttonInt.fall(&button_callback);
      
      //camera
     
          
      /*
      pwm_io(1,BASEPWMLEN,0.025);
      pwm_io(2,BASEPWMLEN,0.125);
      if(button_pressed()){
      pwm_io(1,BASEPWMLEN,0.1);
      pwm_io(2,BASEPWMLEN,0.05);
      }
      */
      
      
      //釋出指令給移動底盤,目標點為A
      
    if(test_begin==1){
        if(go_point("A")==false) {
      pub_err();
      return -1;
    }
    test_begin=0;
    FLAG_SENT_A_ORDER=true;
      }
      if(strcmp(stmOrder.data,"A")==0&&arrrive_point()){
    wait_ms(5000);
    go_point("B");
      }
      if(strcmp(stmOrder.data,"B")==0&&arrrive_point()){
    wait_ms(5000);
    go_point("C");
      }
      if(strcmp(stmOrder.data,"C")==0&&arrrive_point()){
    wait_ms(5000);
    go_point("D");
      }
      if(strcmp(stmOrder.data,"D")==0&&arrrive_point()){
    wait_ms(5000);
    go_point("A");
      }
      
      //example
      //重置引數
     // reset_params();
      
      nodehandle.spinOnce();
      //wait_ms(500);
      
    }
}