ROSでのpythonのプログラムからロボットのゴールを指定する。

はじめに

rosで目的地まで自律移動させる際に使っているrvizの「2D Nav Goal」をプログラム上から指定することを目的としています。個人的にpythonで処理をしてから目的地の決定をしたいと思ったのでpythonでやる方法を調査しました。

コード

以下ソースコードです。

sample.py

#!/usr/bin/env python

import rospy
import actionlib
import tf
from nav_msgs.msg import Odometry
import math
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

def goal_pose(pose):
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]

    return goal_pose


if __name__ == '__main__':
    rospy.init_node('patrol')
    listener = tf.TransformListener()

    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    listener.waitForTransform("map", "base_link", rospy.Time(), rospy.Duration(4.0))

    pose = [(-24.1430511475,-1.39947879314,0.0),(0.0,0.0,0.712479235583,0.701693194255)]
    goal = goal_pose(pose)
    client.send_goal(goal)

参考

https://github.com/HotBlackRobotics/hotblackrobotics.github.io/blob/master/en/blog/_posts/2018-01-29-action-client-py.md
https://hotblackrobotics.github.io/en/blog/2018/01/29/action-client-py/
https://www.robotech-note.com/entry/2018/04/16/080816