JSHOP2与ROS通信

写在前面

因为项目的原因，需要建立JSHOP2与ROS之间的通信。百度了一堆，也没找到相关的教程，老师建议使用ROSJAVA，不知道什么原因，ROSJAVA一直安装失败。我觉得很大的原因是我菜。。。所以需要想办法绕过，又想到很久之前学习Python课程的时候，老师提到过Python又是一门胶水语言，因此决定使用Python解决问题，通过ROSPY实现相关的功能。写下这篇博客，记录这中间的想法与实现过程。

桥梁的一端——JSHOP2

建立规划域与问题域之后，我们就可以使用make命令得到HTN的规划结果，但是make指令的具体内容是什么，对我们来说一无所知。想到之前写JSHOP2环境配置的时候，REDME文件中介绍过各个文件的作用，因此打开README文件，发现如下内容：

打开Makefile，发现make指令的具体内容：

我们可以看到，make命令在目录“examples/propagation”下执行Java指令：

java JSHOP2.InternalDomain propagation
java JSHOP2.InternalDomain -r problem
javac problem.java
java problem

最后调用系统指令rm删除Java编译过程中的中间文件

rm propagation.java; rm propagation.txt; rm problem.java; rm *.class

在系统终端中输入make指令的详细内容，我们也可以得到HTN规划的结果。

通过以上分析，我们得到了JSHOP2具体的编译与运行命令，也就是说，我们可以使用这些命令得到HTN规划的结果。

桥梁的另一端——ROS

简单起见，我们介绍ROS通信机制中的话题，通过使用消息的发布与订阅来完成节点间的通信。

创建Publish
Publish的主要作用是针对指定话题发布特定数据类型的消息。我们使用Publish来发布HTN规划的结果。下面的代码来源于胡春旭老师的《ROS机器人开发实践》一书，我们也可以在ROS的小乌龟例程中看到。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('velocity_publisher', anonymous=True)

	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

	#设置循环的频率
    rate = rospy.Rate(10) 

    while not rospy.is_shutdown():
		# 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

		# 发布消息
        turtle_vel_pub.publish(vel_msg)
    	rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z)

		# 按照循环频率延时
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

实现一个Publish的流程

1. 初始化ROS节点
2. 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
3. 按照一定的频率发布消息

创建Subscriber
创建一个Subscriber以订阅Publish节点发布的“/turtle1/pose”话题，具体内容如下：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose

import rospy
from turtlesim.msg import Pose

def poseCallback(msg):
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)

def pose_subscriber():
	# ROS节点初始化
    rospy.init_node('pose_subscriber', anonymous=True)

	# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

实现Subscriber的简要流程

1. 初始化ROS节点
2. 订阅需要的话题
3. 循环等待话题消息，接收到消息后进入回调函数
4. 在回调函数中完成消息处理

搭建桥梁

了解以上内容之后，我们可以使用Python的os模块调用系统命令，然后将结果保存在result变量中。同样的，我们使用ROSPY来调用Python代码。完成通信桥梁的搭建。
核心代码内容如下：

import os
import random
#在Python中配置JSHOP2环境
CLASSPATH = '${JAVA_HOME}/lib:/home/jason/JSHOP2/JSHOP-SourceCode/JSHOP2/bin/antlr.jar:/home/jason/JSHOP2/JSHOP-SourceCode/JSHOP2/bin/JSHOP2.jar:.'
os.environ['CLASSPATH'] = CLASSPATH

#修改当前的工作目录到指定的路径
os.chdir('/home/jason/JSHOP2/JSHOP-SourceCode/JSHOP2/examples/abc')
#使用Java命令获取HTN的规划结果
os.system('java JSHOP2.InternalDomain basic')
os.system('java JSHOP2.InternalDomain -r problem')
os.system('javac problem.java')
#将返回的结果赋于result变量
result = os.popen('java problem').readlines()

将以上代码放在Publisher中，完成对消息的初始化。然后就可以使用相应的ROS知识完成消息的发布。
同样的，我们也可以编写Subscriber来订阅Publisher发布的HTN规划结果result