ROS與stm32L476RG nucleo 基於mbed開發,使用rosserial_python包通訊
阿新 • • 發佈：2019-02-08
使用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);
}
}