Hello.
I’m currently trying to publish my robot’s encoder values as Odometry and see how it behaves on RViz, based on the assignment in Chapter 3 of Basic Kinematic Class to read the encoder values and output the robot’s Odometry.
However, I have encountered a problem where /odom is not displayed as a topic in the program I created referring to the assignment on encoders and odometry.
I have created a program that receives the encoder value (already converted to distance) from a robot with an arduino type microcontroller and publishes the data to /odom as an odometry message.
The following is the program that receives the encoder from the robot and publishes the Odometry message. The github for them is also posted below.
arduino_serial.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include <string>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
int open_serial(const char *device_name){
int fd1=open(device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
//ファイルディスクリプタfd1を読み書き可の状態で開く
//ファイルディスクリプタ：プログラムからファイルを操作する際に，操作対象のファイルを識別・同定するために割り当てられる番号
fcntl(fd1, F_SETFL,0);
//fd1に対して操作を行う．第２引数は実行するコマンド，F_SEFTLは
//設定の読み込み
struct termios conf_tio; //構造体(変数専用のクラスみたいな)の宣言
tcgetattr(fd1,&conf_tio); //termios構造体(シリアルポートの属性)の取得のために用いられる
// | +------第２引数：ファイルディスクプタの情報を補完するメモリ位置
// +--------------第１引数：呼び出されるファイルディスクリプタ
//set baudrate
speed_t BAUDRATE = B115200;
cfsetispeed(&conf_tio, BAUDRATE); //入力ボーレート設定
cfsetospeed(&conf_tio, BAUDRATE); //出力ボーレート設定
//non canonical, non echo back
conf_tio.c_lflag &= ~(ECHO | ICANON);
//ECHOまたはICANONのメモリが解放された際に働くデコンストラクタ ~(チルダ)はその宣言
//non blocking
conf_tio.c_cc[VMIN]=0;
conf_tio.c_cc[VTIME]=0;
//設定の保存
tcsetattr(fd1,TCSANOW,&conf_tio); //パラメータを設定する
// | +-----termiosの情報位置
// +----------------即時に変更を行う
return fd1;
}
int fd1;
int main(int argc, char **argv)
{
ros::init(argc, argv, "s4_comport_serialport");
ros::NodeHandle n;
//Publisher
ros::Publisher serial_pub = n.advertise<std_msgs::String>("Serial_in", 1000);
char device_name[]="/dev/ttyUSB0";
fd1=open_serial(device_name);
if(fd1<0){
ROS_ERROR("Serial Fail: cound not open %s", device_name);
ROS_ERROR("Serial Fail\n");
ros::shutdown();
}
ros::Rate loop_rate(10);
while (ros::ok()){
char buf[256]={0};
int recv_data=read(fd1, buf, sizeof(buf)); //ファイルからのデータ読み込み
// | | +--------データの大きさ
// | +-----------------データを格納するアドレス
// +----------------------ファイルのディスクリプタ（ファイルの識別番号）
if(recv_data>0){
ROS_INFO("recv:%03d %s\n",recv_data,buf);
std_msgs::String serial_msg;
serial_msg.data=buf;
serial_pub.publish(serial_msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
nexus_odometry.py
#!/usr/bin/env python
import rospy
from math import sin,cos,pi
from geometry_msgs.msg import Twist,Pose,Point,Quaternion,Vector3
from nav_msgs.msg import Odometry
from rosgraph_msgs.msg import Clock
import tf
from std_msgs.msg import String
from tf.broadcaster import TransformBroadcaster
class OdometryClass():
def __init__(self):
self.odom_sub = rospy.Subscriber("/Serial_in", String,self.callback)
self.odom_pub = rospy.Publisher("/odom",Odometry,queue_size=1)
self.odom = Odometry()
self.odom_broadcaster = TransformBroadcaster()
self.rate = rospy.Rate(10)
self.current_odometry = 0
self.last_odometry = 0
self.current_time = rospy.Time.now()
self.last_time = rospy.Time.now()
self.L = 0.3
self.x = 0
self.y = 0
self.th = 0
rospy.wait_for_message("/clock",Clock)
self.UpdatePose()
def callback(self,msg):
self.current_odometry = msg.data
def UpdatePose(self):
while not rospy.is_shutdown():
d_odom = float(self.current_odometry)
self.current_time = rospy.Time.now()
dt = (self.current_time - self.last_time).to_sec()
self.last_odometry = d_odom
v = d_odom / dt
delta_x = v*cos(self.th)
delta_y = v*sin(self.th)
self.x += delta_x
self.y += delta_y
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th)
self.odom_broadcaster.sendTransform(
(self.x,self.y,0.),
odom_quat,
self.current_time,
"base_link",
"odom"
)
odom = Odometry()
odom.header.stamp = self.current_time
odom.header.frame_id = "odom"
odom.pose.pose = Pose(Point(self.x,self.y,0.), Quaternion(*odom_quat))
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(v,0,0), Vector3(0,0,0))
self.odom_pub.publish(odom)
self.last_time = self.current_time
self.rate.sleep()
if __name__ == "main":
rospy.init_node("pub_odom")
oc = OdometryClass()
#rospy.loginfo(oc.current_odometry)
rospy.spin()
As for the program that publishes Odometry, the main idea is almost the same, although there are some differences in kinematics calculations.
The environment we are using now is ROS melodic.
I have no idea why /odom doesn’t show up in the topics and why I can’t publish Odometry.
I would really appreciate your help. Thank you for your advice.
Thank you very much.
