【ROS】ROS机器人学习（catkin海龟测试）含源码

一、ros环境配置

1. 创建catkin空间

 $ mkdir -p ~/catkin_ws/src
 $ cd ~/catkin_ws/
 $ catkin_make  
 $ source devel/setup.bash  #catkin_make工具处理后，在devel文件夹里，source其中setup.*sh环境配置文件可设置在ROS工作环境的最顶层，打开一个新终端会重置环境

2. 查看ROS_PACKAGE_PATH环境变量的工作空间目录

 $ echo $ROS_PACKAGE_PATH

若不在/home/...上，则需添加到工作空间目录

 $ export ROS_PACKAGE_PATH=~/catkin_ws/src:$ROS_PACKAGE_PATH

---

附ros基本命令:

• 查看软件包路径

$ rospack find [包名]

• 直接切换到工作目录的某个软件包

$ roscd

• 直接罗列目录

$ rosls

---

二、ros简单运行

catkin程序包最起码由
　- package.xml #提供有关程序包的元信息
　- CMakelists.txt
组成。

1. 创建一个程序包

 $ cd ~/catkin_ws/src
 $ catkin_create_pkg beginner_tutorials std_msgs rospy roscpp #catkin_create_pkg命令创建新程序包,名为"beginner_tutorials",依赖于std_msgs rospy roscpp,这些包保存在package.xml中（关于package中的描述、维护者、许可、依赖项标签见wiki.ros.org）
    -这些包也有其依赖的包，如rospy也有其依赖的包
 $ rospack depends rospy #能查看所有依赖包

2. 编辑程序包

 $ cd ~/catkin_ws/
 $ catkin_make #编译beginner_tutorials程序包

3. ROS节点（nodes）
   是一个可执行文件。可发布、接受、提供服务。

 $ rosrun turtlesim turtlesim_node #运行turtlesim包中的
 $ rosnode

三、海龟测试

1. 在catkin环境下为此项目建ros包learning_tf

$ cd %YOUR_CATKIN_WORKSPACE_HOME%/src
$ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
( build环境
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/
$ catkin_make
$ source ./devel/setup.bash )

2. 在nodes中写一个广播：向tf广播 机器人的状态（在该包内编写 向tf发布海龟状态 的代码）

$ roscd learning_tf
$ gedit nodes/turtle_tf_broadcaster.py

代码如下：
#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin() 

赋予运行权限：
$ chmod -x nodes/turtle_tf_broadcaster.py

3. 在launch中建立启动文件运行广播

$ gedit launch/start_demo.launch

代码如下：

    
    
    

    
      
    
    
       
    

  

启动：
$ roslaunch learning_tf start_demo.launch 

4. 在nodes中写一个监听器：使用tf访问框架转换（在该包内编写 监听器 的代码）

$ roscd learning_tf
$ gedit nodes/turtle_tf_listener.py

代码如下：
#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

赋予运行权限：
$ chmod -x nodes/turtle_tf_listener.py

5. 在launch中编辑启动文件

$ gedit launch/start_demo.launch

代码如下：

    ...
    


启动：
$ roslaunch learning_tf start_demo.launch

6. 使用tf_echo工具来检查乌龟的状态

$ rosrun tf tf_echo /world /turtle1

---

附TF（ROS接口的函数库）简介及安装
一、TF简介
TF是处理机器人不同位置坐标系的一个包。
机器人不同部位和世界的坐标系以 tree structure 的形式存储起来，TF可以使任何两个坐标系之间的点向量相互转化。
机器人系统是有很多坐标系系统，并且每一个坐标系系统都随时间变化，tf 可以帮助你解决这些问题：头 坐标系跟世界坐标系是什么关系，栅格图中物体的位置跟我地盘有什么关系？在地图坐标系中，底盘的位置跟现在有什么关系？
附tf案例测试：

#coding:utf-8
#0导入模块，生成模拟数据集
import tensorflow as tf
import numpy as np
BATCH_SIZE = 8
seed = 23455

#基于seed产生随机数
rng = np.random.RandomState(seed)
#随机数返回32
X = rng.rand(32,2)
Y = [[int(x0 + x1 < 1)] for (x0,x1) in X]
print "X:\n",X
print "Y:\n",Y

x = tf.placeholder(tf.floate32, shape=(None, 2))
y = tf.placeholder(tf.floate32, shape=(None, 1))

w1 = tf.Variable(tf.random_normal([2, 3], stddev=1, seed=1))
w2 = tf.Variable(tf.random_normal([2, 3], stddev=1, seed=1))

a = tf.matmul(x, w1)
y = tf.matmul(a, w2)

loss = tf.reduce_mean(tf.square(y-y_))
train_step = tf.train.GradientDescentOptimizer(0.001).minimize(loss)

with tf.Session() as sess:
	init_op = tf.global_variables_initializer()
	sess.run(init_op)
	print "w1:\n", sess.run(w1)
	print "w2:\n", sess.run(w2)
	print "\n"
	
	STEPS = 3000
	for i in range(STEPS):
		start = (i*BATCH_SIZE) % 32
		end = start + BATCH_SIZE
		sess.run(train_step, feed_dict={x: X[start:end], y_:Y[start:end]})
		if i % 500 == 0:
			total_loss = sess.run(loss, feed_dict={x: X,y_: Y})
	
	print "\n"
	print "w1:\n", sess.run(w1)
	print "w2:\n", sess.run(w2)
	print "\n"
	
	STEPS = 3000
	for i in range(STEPS):
		start = (i*BATCH_SIZE) % 32
		end = start + BATCH_SIZE
		sess.run(train_step, feed_dict={x: X[start:end], y_: Y[start:end]})
		if i % 500 == 0:
			total_loss = sess.run(loss, feed_dict={x: X,y_: Y})
			print("After %d training step(s), loss on all data is %g" % (i, total_loss))
	
	print "\n"
	print "w1:\n", sess.run(w1)
	print "w2:\n", sess.run(w2)

二、配置tf环境（本章在indigo版本的ubuntu环境中配置）

1. 安装tf相应包

$ sudo apt-get install ros-melodic-ros-tutorials ros-melodic-geometry-tutorials ros-melodic-rviz ros-melodic-rosbash ros-melodic-rqt-tf-tree

2. tf工具

($ roslaunch turtle_tf turtle_tf_demo.launch体验)
$ rosrun tf view_frames  #创建由TF通过ros广播的帧的图
$ evince frames.pdf  #TF侦听器正在监听通过ROS广播的帧，并绘制帧其之间如何连接的树
$ rosrun rqt_tf_tree rqt_tf_tree  #rqt_tf_tree是一个运行时工具，用于可视化正在ROS上广播的帧树
$ rosrun tf tf_echo turtle1 turtle2  #打印通过ROS广播的任意两个帧之间进行转换数据,即2相对于1的坐标
$ rosrun rviz rviz -d 'rospack find turtle_tf'/rviz/turtle_rviz.rviz #可视化工具检查TF帧。

---

个人代码行已存储在github：https://github.com/onlyhyl/turtle