Vehicle-cpp/navigation_hingecar_ws/install_dev/actionClient/pubGoalSlagpotUp.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
# from std_msgs.msg import String
# from hinge_car_nav_msgs.msg import HingeCarNavActionGoal
from hinge_car_nav_msgs.msg import hinge_car_nav_msgs
import geometry_msgs.msg
# import sys
# import tf
from tf.transformations import quaternion_from_euler
# import numpy as np
# import sys
# Brings in the SimpleActionClient
# import yaml
import actionlib

# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
# from tf.transformations import quaternion_from_euler


def callback(data):
    # rospy.loginfo(rospy.get_caller_id() + 'I heard %s', )
    rospy.loginfo('received move base action goal')
    # rospy.loginfo(data.goal_id)


def done_cb(data, data1):
    print("action done cb:%d,%s" % (data, data1))


def active_cb():
    print("active_cb")


def feedback_cb(data):
    pass

    # print("feedback_cb x:%f,y%f", data.pose.position.x,data.pose.position.y)
    # print("feedback_cb ", data.base_position.pose.position.x )


if __name__ == '__main__':
    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('action_client', anonymous=True)

    # rospy.Subscriber('/move_base/goal',MoveBaseActionGoal, callback)
    # goal_pub = rospy.Publisher('/move_base/goal',MoveBaseActionGoal)

    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient(
        'hinge_car_nav', hinge_car_nav_msgs.msg.HingeCarNavAction)

    # Waits until the action server has started up and started
    # listening for goals.
    # 192.653706,205.496984
    p0 = geometry_msgs.msg.PoseStamped()
    #p0.pose.position.x = 165.79   # 开始点位 165.797971,196.436970
    #p0.pose.position.y = 96.436
    # x = 192.65
    # y = 205.49
    # yaw = -1.526
    #1.537860),fork_pose:(196.880965,183.836425
    #虚拟包位
    #x = 196.88
    #y = 183.83
    #yaw = 1.537
	#156.042740,208.596165 -1.543    155.733523,208.609741 -1.560797
	
	#对包成功
    #x = 155.73304
    #y = 208.6096
    #yaw = -1.5607


    #178.045984,209.219726,-1.608619   N30
    #(78.512528,210.467081,-1.603034),
    x = 178.045984
    y = 209.219726
    yaw = -1.608619
	
	
	
	
    # q = quaternion_from_euler(0.0 , 0.0 , yaw)
    # p0.pose.orientation.x = q[0]
    # p0.pose.orientation.y = q[1]
    # p0.pose.orientation.z = q[2]
    # p0.pose.orientation.w = q[3]
    # 定义欧拉角
    roll, pitch = 0.0, 0.0  # 用实际值替换
    q = quaternion_from_euler(roll, pitch, yaw)
    # 创建geometry_msgs/Quaternion类型的四元数
    quaternion_msg = geometry_msgs.msg.Quaternion(*q)
    p0.pose.orientation = quaternion_msg
    p0.pose.position.x = x
    p0.pose.position.y = y
    print("before client connected")
    client.wait_for_server()
    print("client connected")
    goal = hinge_car_nav_msgs.msg.HingeCarNavGoal()
    path = hinge_car_nav_msgs.msg.HingeCarPath()
    goal.goal_type = goal.LOAD_SLAGPOT_IN
    goal.path.slagpot_pose = p0
    client.send_goal(goal, done_cb, active_cb, feedback_cb)
    # Sends the goal to the action server.
    # Waits for the server to finish performing the action.

    client.wait_for_result()
    client.get_result()
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        result = client.get_result()
    except rospy.ROSInterruptException:
        print("program interrupted before completion")
    rospy.sleep(5.0)