2024年校内赛要求：要求参赛选手在指定的比赛设备上建立ROS包，包的名字规则为pkgxxx（xxx为选手名字拼音缩写）。选手自行开发的代码需放置在其包下面，通过各自包下的launch文件启动。

参赛设备1：spark-7797，热点为spark-7797，安全密钥连接，密码为：12345678，连接热点后，

通过VNC Viewer连接到该主机，IP地址为：10.42.0.1

工作区:ws_2024

在ws_2024工作区下建立包，包的名字规则为pkgxxx（xxx为选手名字拼音缩写）。

步骤1、创建工作区

注：如果该步骤之前已完成，则可以省略

（注：其中ws_2024为工作区的名字)

$ mkdir  -p  ~/ws_2024/src
$ cd  ~/ws_2024/src
$ catkin_init_workspace
$ cd  ~/ws_2024
$ catkin_make
$ source  ~/ws_2024/devel/setup.bash
$ rospack  profile
$ echo   "source  ~/ws_2024/devel/setup.bash"   >>  ~/.bashrc

步骤2、创建包（如果该步骤之前已经完成，则可省略）

（注：其中pkgduan为包的名字）

$ cd  ~/ws_2024/src
$catkin_create_pkg  pkgduan  std_msgs  geometry_msgs rospy roscpp
$ cd  ~/ws_2024
$ catkin_make

步骤3、建立launch目录、scripts目录：

$roscd pkgduan
$mkdir launch
$mkdir scripts
$cp ~/spark_noetic/src/3rd_app/move2grasp/launch/teleop2grasp.launch launch
$cp ~/spark_noetic/src/3rd_app/move2grasp/scripts/teleop.py scripts
$cp ~/spark_noetic/src/3rd_app/move2grasp/scripts/grasp_pro_new.py scripts

将~/spark_noetic/src/3rd_app/move2grasp/launch/teleop2grasp.launch文件拷贝到launch目录，

将~/spark_noetic/src/3rd_app/move2grasp/scripts/teleop.py文件拷贝到scripts目录

将~/spark_noetic/src/3rd_app/move2grasp/scripts/grasp_pro_new.py文件拷贝到scripts目录

步骤4、修改launch目录下的teleop2grasp.launch文件,修改后存盘退出

$roscd pkgduan/launch
$gedit  teleop2grasp.launch

原来的文件内容包括：

<node pkg="move2grasp" type="teleop.py" name="teleop" launch-prefix="xterm -e" />
	<group if="$(eval arg('camera_type_tel')=='d435')">
		<node pkg="move2grasp" type="grasp_d435.py" name="grasp" output="screen">
			<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
		</node>
	</group>
	<group if="$(eval arg('camera_type_tel')=='astrapro')">
		<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" output="screen"/>
	</group>

将其中的 pkg="move2grasp"  改为 pkg="pkgxxx"  (其中pkgxxx为你的包的名字，有三个地方需要改，也就是启动你自己包下的节点)，如下：

<node pkg="pkgxxx" type="teleop.py" name="teleop" launch-prefix="xterm -e" />
	<group if="$(eval arg('camera_type_tel')=='d435')">
		<node pkg="pkgxxx" type="grasp_d435.py" name="grasp" output="screen">
			<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
		</node>
	</group>
	<group if="$(eval arg('camera_type_tel')=='astrapro')">
		<node pkg="pkgxxx" type="grasp_pro_new.py" name="grasp" output="screen"/>
	</group>

步骤5、启动launch文件并实现机器人控制、抓取,新开一个终端，执行下面指令：

$roslaunch  pkgduan  teleop2grasp.launch

启动后，控制界面如下：

点击图标，将遥控界面至于活动界面，通过WASD控制机器人移动和旋转，通过G抓取其前方的分米立方块，通过H放下立方块。

步骤6、如何扩展，基本思路和方法：

通过rosrun rqt_graph rqt_graph可以查看的目前ROS的计算图如下（部分）：

其中teloop节点对应的代码就是：teleop.py

grasp节点对应的代码就是：grasp_pro_new.py

修改上述两个节点，就可以实现更为复杂的功能。

例如，在teleop.py中增加更多的功能键，实现不同高度的放置（参考其中H功能键的代码扩展），

grasp_pub = rospy.Publisher('/grasp', String, queue_size=1)

...                
                
                
        # ch为按键
        if ch == 'g':
            if can_grasp:
                msg=String()
                msg.data='1'
                grasp_pub.publish(msg)
                can_grasp=False
                speed = 0
                turn = 0
        elif ch == 'h':
            if can_release:
                msg=String()
                msg.data='0'
                grasp_pub.publish(msg)
                can_release=False
                speed = 0
                turn = 0

上面的代码中，msg.data向/grasp话题发布的消息，该消息被grasp节点订阅，实现不同的功能

目前发布的消息只有"0","1"；

例如，可以设置一个按键'k',发布消息"2"，表示要放到第二层的高度。

这就交给各位同学，各显神通了！

再来看一下grasp_pro_new.py的核心代码（grasp节点)：

self.sub2 = rospy.Subscriber(
            '/grasp', String, self.grasp_cp, queue_size=1)

这说明，该节点从/grasp话题订阅消息，回调函数为self.grasp_cp，只要改该函数就可以增加功能，

    def grasp_cp(self, msg):
        if msg.data == '1':
            # \u8ba2\u9605\u6444\u50cf\u5934\u8bdd\u9898,\u5bf9\u56fe\u50cf\u4fe1\u606f\u8fdb\u884c\u5904\u7406
            self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
            self.is_found_object = False
            rate = rospy.Rate(10)
            times=0
            steps=0
            while not self.is_found_object:
                rate.sleep()
                times+=1
                # \u8f6c\u4e00\u5708\u6ca1\u6709\u53d1\u73b0\u53ef\u6293\u53d6\u7269\u4f53,\u9000\u51fa\u6293\u53d6
                if steps>=5:
                    self.sub.unregister()
                    print("stop grasp\n")
                    status=String()
                    status.data='-1'
                    self.grasp_status_pub.publish(status)
                    return
                # \u65cb\u8f6c\u4e00\u5b9a\u89d2\u5ea6\u626b\u63cf\u662f\u5426\u6709\u53ef\u4f9b\u6293\u53d6\u7684\u7269\u4f53
                if times>=30:
                    times=0
                    steps+=1
                    self.turn_body()
                    print("not found\n")
            print("unregisting sub\n")
            self.sub.unregister()
            print("unregisted sub\n")
            # \u6293\u53d6\u68c0\u6d4b\u5230\u7684\u7269\u4f53    
            self.grasp()
            status=String()
            status.data='1'
            self.grasp_status_pub.publish(status) 
        if msg.data=='0':
            # \u653e\u4e0b\u7269\u4f53
            self.is_found_object = False
            self.release_object()
            status=String()
            status.data='0'
            self.grasp_status_pub.publish(status) 

附文件源码：

视频讲解1：(如何扩展功能界面)

视频讲解2：（扩展功能）

。