<img src=”https://www.guyuehome.com/Uploads/wp/2020/10/微信截图_20201021181131.png” alt=”微信截图_20201021181131″ width=”321″ height=”141″ />
工控机与底盘通信正常,但底盘注册订阅的话题均查询不到。
STM32部分代码:
#include &lt;stdio.h&gt;
#include “hardwareserial.h”
#include “Kinematics.h”
#include &lt;ros.h&gt;
#include &lt;riki_msgs/Velocities.h&gt;
#include &lt;geometry_msgs/Twist.h&gt;
#include &lt;geometry_msgs/Vector3.h&gt;
#include &lt;ros/time.h&gt;
double required_angular_vel = 0.0;
double required_linear_vel = 0.0;
uint32_t previous_command_time = 0;
void command_callback( const geometry_msgs::Twist&amp; cmd_msg);
ros::NodeHandle nh;
riki_msgs::Velocities raw_vel_msg;
ros::Subscriber&lt;geometry_msgs::Twist&gt; cmd_sub(“cmd_vel”, command_callback);
ros::Publisher raw_vel_pub(“raw_vel”, &amp;raw_vel_msg);
void move_base();
void command_callback( const geometry_msgs::Twist&amp; cmd_msg)
{
required_linear_vel = cmd_msg.linear.x;//*40.64048351520634;//45.055968420371;//41.98
required_angular_vel = 10 * cmd_msg.angular.z;//*20.37179682044995;//22.77576019211;
previous_command_time = millis();
}
int main(void)
{
uint32_t previous_control_time = 0;
char vel_buffer1[]= ” enter move base1 “;
SystemInit();
initialise();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//???????????2
delay_init(168);
nh.initNode();
USART_ITConfig(USART2, USART_IT_IDLE, ENABLE); // ¿ªÆô´®¿Ú2IDLEÖжÏ
DMA_Use_USART2_Rx_Init();//ÅäÖô®¿Ú2µÄDMA½ÓÊÕ
nh.advertise(raw_vel_pub);
nh.subscribe(cmd_sub);
#if 1
while (!nh.connected()){
nh.spinOnce();
}
#endif
delay(1000);
nh.loginfo(“Rikibase Connected!”);
delay(1000);
while(1){
if (((millis() – previous_control_time) &gt;= (1000 / COMMAND_RATE))){
nh.logwarn(vel_buffer1);
move_base();
previous_control_time = millis();
}
if ((millis() – previous_command_time) &gt;= 400){
stop_base();
}
nh.spinOnce();
}
}
&nbsp;
第三方账号登入
QQ 微博 微信