使用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);
}
}