diff --git a/.gitignore b/.gitignore index 973bb23ac0d1bbbb30ce1830439fd18f905056fe..fff456dd32a3f4a4a37026f4074909ec1312fe4b 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,8 @@ CMakeLists.txt !*/CMakeLists.txt ./.catkin_workspace + +__pycache__ +*.pyc + +*venv diff --git a/navigaion_signal_sender/CMakeLists.txt b/goal_sender/CMakeLists.txt similarity index 96% rename from navigaion_signal_sender/CMakeLists.txt rename to goal_sender/CMakeLists.txt index 5a27dc4cf4f3dada0ace5961b8ec9ec13315dec4..5a822e325507aab9d35e9aed63e6abdbf34eff6a 100644 --- a/navigaion_signal_sender/CMakeLists.txt +++ b/goal_sender/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(navigaion_signal_sender) +project(goal_sender) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES navigaion_signal_sender +# LIBRARIES goal_sender # CATKIN_DEPENDS actionlib move_base_msgs rospy # DEPENDS system_lib ) @@ -122,7 +122,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/navigaion_signal_sender.cpp +# src/${PROJECT_NAME}/goal_sender.cpp # ) ## Add cmake target dependencies of the library @@ -133,7 +133,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/navigaion_signal_sender_node.cpp) +# add_executable(${PROJECT_NAME}_node src/goal_sender_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -197,7 +197,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_navigaion_signal_sender.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_goal_sender.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/navigaion_signal_sender/package.xml b/goal_sender/package.xml similarity index 93% rename from navigaion_signal_sender/package.xml rename to goal_sender/package.xml index b67968e211573840d781f173502c3d2afabb5e27..d739126d6f519e2bc46742f9ab129a12c3082519 100644 --- a/navigaion_signal_sender/package.xml +++ b/goal_sender/package.xml @@ -1,8 +1,8 @@ - navigaion_signal_sender + goal_sender 0.0.0 - The navigaion_signal_sender package + The goal_sender package @@ -19,7 +19,7 @@ - + diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py new file mode 100755 index 0000000000000000000000000000000000000000..879b2bdf8c7a11ec795d972c0b18518041934406 --- /dev/null +++ b/goal_sender/scripts/Navigation.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python + +# Navigation.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +import sys +# import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import Queue as queue + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + pass + + +class Command(RoIS_Common.Command): + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + +class Event(RoIS_Common.Event): + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + # def poll_event(self): + # msg = self.event_queue.get() + # return msg + +from goal_sender_ros import GoalSenderROS + +class Navigation(Event, Command, Query): + def __init__(self, c): + super(Navigation, self).__init__(c) + self._component = c + self._goal_sender = GoalSenderROS() + self._component.Target_Position = [""] + self._component.Time_Limit = 10 + self._component.Routing_Policy = "" + + def set_parameter(self, target_position, time_limit, routing_policy): + self._goal_sender.send_goal(target_position) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = self._goal_sender.get_goalsender_state().value + return (status, c_status) + + +def event_dispatch(n): + # n.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + # n.person_detected(datetime.now().isoformat(), 1) + + +class Component(object): + pass + +import rospy + +def example_n(port): + print("Starting node...") + component = Component() + n = Navigation(component) + print("Navigation constructed") + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(n,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(n) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + rospy.init_node('navigation_component') + example_n(8001) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py new file mode 100644 index 0000000000000000000000000000000000000000..fc616a489b251b996f217c27261883fc8672c39c --- /dev/null +++ b/goal_sender/scripts/Navigation_client.py @@ -0,0 +1,107 @@ +# Navigation_Client.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Engine + +"""Navigation_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + def set_parameter(self, target_position, time_limit, routing_policy): + status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + status = RoIS_HRI.ReturnCode_t(status) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + status, c_status = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), target_position, time_limit, routing_policy) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # if logger is not None: + # self.logger = logger + + # def start_th(self): + # self.th = threading.Thread(target=self.event_loop) + # self.th.start() + + # def event_loop(self): + # """event_loop + # """ + # while True: + # try: + # self.poll_event() + # except ConnectionRefusedError: + # break + + # def poll_event(self): + # """poll_event + # """ + # msg = self._e_proxy.poll_event() + # (params, methodname) = xmlrpc.client.loads(msg) + # #self.logger.debug('poll_event: '+methodname) + # print(params,methodname) + # if methodname == 'speech_recognized': + # self.speech_recognized(params[0], params[1]) + + +class Navigation_Client(Command, Query, Event): + """Navigation_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # self.start_th() diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py new file mode 100644 index 0000000000000000000000000000000000000000..c6058e48db97d7733ca4d534cd5fa807cad86d65 --- /dev/null +++ b/goal_sender/scripts/goal_sender_ros.py @@ -0,0 +1,56 @@ +#/usr/bin/env python + +import rospy +import actionlib + +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +from geometry_msgs.msg import PoseStamped + +from actionlib_msgs.msg import GoalStatus as g_state +from pyrois.RoIS_Common import Component_Status as c_state + +def pose_to_goal(pose): + goal = MoveBaseGoal() + goal.target_pose.header.frame_id = 'map' + goal.target_pose.pose.position.x = pose[0][0] + goal.target_pose.pose.position.y = pose[0][1] + goal.target_pose.pose.position.z = pose[0][2] + goal.target_pose.pose.orientation.x = pose[1][0] + goal.target_pose.pose.orientation.y = pose[1][1] + goal.target_pose.pose.orientation.z = pose[1][2] + goal.target_pose.pose.orientation.w = pose[1][3] + + return goal + + +class GoalSenderROS(object): + # Table for conversion from GoalStatus to Component_Status + # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html + GoalSenderState = { + g_state.PENDING: c_state.READY, + g_state.ACTIVE: c_state.BUSY, + g_state.PREEMPTED: c_state.READY, + g_state.SUCCEEDED: c_state.READY, + g_state.ABORTED: c_state.ERROR, + g_state.PREEMPTING: c_state.BUSY, + g_state.RECALLING: c_state.BUSY, + g_state.RECALLED: c_state.READY, + g_state.LOST: c_state.ERROR + } + + def __init__(self): + self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + self.action_client.wait_for_server() + + def send_goal(self, pose_xy): + pose_xy.append(0) + pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)] + goal = pose_to_goal(pose) + self.action_client.send_goal(goal) + + def get_goalsender_state(self): + state = self.action_client.get_state() + return GoalSenderROS.GoalSenderState[state] + +# If you remove this comment out, it does not work. +# rospy.init_node('goal_sender') diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..335976b1be46521ba9339e197cf0cfa2410f4106 --- /dev/null +++ b/goal_sender/scripts/route_guidance_engine.py @@ -0,0 +1,228 @@ +# route_guidance_engine.py +# +# Copyright 2017 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python 3 +# For HRI Engine + +"""route_guidance_engine +""" + +from __future__ import print_function + +import os +import sys +import threading + +from pyrois import RoIS_HRI, RoIS_Service + +if sys.version_info.major == 2: + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass +else: + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class SystemIF(RoIS_HRI.SystemIF): + """SystemIF + """ + def __init__(self, Engine): + self._engine = Engine + + def connect(self): + # print("connect") + if self._engine.state is False: + self._engine.state = True + status = RoIS_HRI.ReturnCode_t.OK.value + else: + status = RoIS_HRI.ReturnCode_t.ERROR.value + # time.sleep(30) + return status + + def disconnect(self): + if self._engine.state is True: + self._engine.state = False + status = RoIS_HRI.ReturnCode_t.OK.value + else: + status = RoIS_HRI.ReturnCode_t.ERROR.value + #status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_profile(self, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + # RoIS_HRI_Profile + profile = "Unsupported" + return (status, profile) + + def get_error_detail(self, error_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + # ResultLists + results = "None" + return (status, results) + + +class CommandIF(RoIS_HRI.CommandIF): + """CommandIF + """ + def __init__(self, Engine): + self._engine = Engine + + def search(self, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + # List< RoIS_Identifier> + component_ref_list = ["None"] + return (status, component_ref_list) + + def bind(self, component_ref): + # print("state:", self._engine.state) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def bind_any(self, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + component_ref_list = ["None"] + return (status, component_ref_list) + + def release(self, component_ref): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_parameter(self, component_ref): + status = RoIS_HRI.ReturnCode_t.OK.value + parameter_list = ["None"] + return (status, parameter_list) + + def set_parameter(self, component_ref, parameters): + status = RoIS_HRI.ReturnCode_t.OK.value + command_id = "0" + return (status, command_id) + + def execute(self, command_unit_list): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_command_result(self, command_id, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + results = ["None"] + return (status, results) + + +class QueryIF(RoIS_HRI.QueryIF): + """ + class QueryIF(object): + """ + + def __init__(self, Engine): + self._engine = Engine + + def query(self, query_type, condition): + status = RoIS_HRI.ReturnCode_t.OK.value + results = ["None"] + return (status, results) + + +class EventIF(RoIS_HRI.EventIF): + """ + class QueryIF(object): + """ + + def __init__(self, Engine): + self._engine = Engine + + def subscribe(self, event_type, condition): + """ + subscribe(self, event_type, condition) -> (status,subscribe_id) + """ + status = RoIS_HRI.ReturnCode_t.OK.value + subscribe_id = "0" + return (status, subscribe_id) + + def unsubscribe(self, subscribe_id): + """ + unsubscribe(self,subscribe_id) -> status + """ + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def get_event_detail(self, event_id, condition): + """ + get_event_detail(self,event_id,condition) -> (status,results) + """ + status = RoIS_HRI.ReturnCode_t.OK.value + results = ["None"] + return (status, results) + + +from Navigation_client import Navigation_Client as NavClient + +class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): + """IF + """ + def __init__(self, Engine): + super().__init__(Engine) + self.command_id = 0 + self.compoent_clients = { + 'Navigation': NavClient('http://localhost:8001') + } + + def set_parameter(self, component_ref, parameters): + status = None + if not component_ref in self.compoent_clients: + status = RoIS_HRI.ReturnCode_t.ERROR.value + else: + target_component_client = self.compoent_clients[component_ref] + status = target_component_client.set_parameter(*parameters).value + self.command_id += 1 + return (status, str(self.command_id)) + + ### Remove ### + def get_navi_status(self): + status, c_status = self.compoent_clients['Navigation'].component_status() + return (status.value, c_status.value) + ############## + +class IF_server: + """IF_Server + """ + def __init__(self, port): + self._addr = os.getenv("HRIENGINE") + self._server = ThreadingXMLRPCServer( + ("0.0.0.0", port), logRequests=False) + + def run(self, _IF): + """IF_Server + """ + self._server.register_instance(_IF) + self._server.register_introspection_functions() + # print("server running") + self._server.serve_forever() + + +class RouteGuidanceEngine: + """IF_Server + """ + def __init__(self): + self.state = False + + +def test_engine(port): + """test_engine + """ + IF_server(port).run(IF(RouteGuidanceEngine())) + + +if __name__ == "__main__": + test_engine(8000) diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py new file mode 100644 index 0000000000000000000000000000000000000000..6abe2d974ee306c75e95e5758a6bb5afe95077d9 --- /dev/null +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -0,0 +1,141 @@ +# HRI_Engine_client.py +# +# Copyright 2017 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python 3 +# For Service Application + +"""HRI_Engine_client +""" +import time +import xmlrpc.client +# from pyrois import Service_Application_Base_example + +from pyrois import RoIS_HRI, RoIS_Common, RoIS_Service +# import Service_Application_IF_test + +class SystemIF(RoIS_HRI.SystemIF): + """SystemIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def connect(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.connect()) + return status + + def disconnect(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.disconnect()) + return status + + def get_profile(self, condition): + (s, profile) = self._proxy.get_profile(condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, profile) + + def get_error_detail(self, error_id, condition): + (s, results) = self._proxy.get_error_detail(error_id, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class CommandIF(RoIS_HRI.CommandIF): + """CommandIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def search(self, condition): + (s, component_ref_list) = self._proxy.search(condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, component_ref_list) + + def bind(self, component_ref): + status = RoIS_HRI.ReturnCode_t(self._proxy.bind(component_ref)) + return status + + def bind_any(self, condition): + (s, component_ref_list) = self._proxy.search(condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, component_ref_list) + + def release(self, component_ref): + status = RoIS_HRI.ReturnCode_t(self._proxy.release(component_ref)) + return status + + def get_parameter(self, component_ref): + (s, parameter_list) = self._proxy.get_parameter(component_ref) + status = RoIS_HRI.ReturnCode_t(s) + return (status, parameter_list) + + def set_parameter(self, component_ref, parameters): + (status, command_id) = self._proxy.set_parameter(component_ref, parameters) + status = RoIS_HRI.ReturnCode_t(status) + return (status, command_id) + + def execute(self, command_unit_list): + s = self._proxy.execute(command_unit_list) + status = RoIS_HRI.ReturnCode_t(s) + return status + + def get_command_result(self, command_id, condition): + (s, results) = self._proxy.get_command_result(command_id, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class QueryIF(RoIS_HRI.QueryIF): + """QueryIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def query(self, query_type, condition): + (s, results) = self._proxy.query(query_type, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class EventIF(RoIS_HRI.EventIF): + """EventIF + """ + def __init__(self, proxy_obj): + self._proxy = proxy_obj + + def subscribe(self, event_type, condition): + (s, subscribe_id) = self._proxy.subscribe(event_type, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, subscribe_id) + + def unsubscribe(self, subscribe_id): + s = self._proxy.unsubscribe(subscribe_id) + status = RoIS_HRI.ReturnCode_t(s) + return status + + def get_event_detail(self, event_id, condition): + (s, results) = self._proxy.get_event_detail(event_id, condition) + status = RoIS_HRI.ReturnCode_t(s) + return (status, results) + + +class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): + """IF + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + # self.a = Service_Application_Base_example.Service_Application_Base() + # Service_Application_IF_test.Service_Application_IF.__init__(self,uri) + + def analysis_c_status(self): + s = self._proxy.analysis_c_status() + status = RoIS_HRI.ReturnCode_t(s) + return status + + ### Remove ### + def get_navi_status(self): + status, c_status = self._proxy.get_navi_status() + return RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status) + ############## diff --git a/goal_sender/scripts/test_component.py b/goal_sender/scripts/test_component.py new file mode 100644 index 0000000000000000000000000000000000000000..73fdab2689dd6c5dc9e0f693649818ba57619401 --- /dev/null +++ b/goal_sender/scripts/test_component.py @@ -0,0 +1,9 @@ +import time +from Navigation_client import Navigation_Client + +uri = "http://localhost:8001" +client = Navigation_Client(uri) + +print(client.start()) +time.sleep(3) +print(client.set_parameter("ahi", "chuni", "dwa")) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..006b76c90de2c4dc538cb73e9ae699af283ca7ee --- /dev/null +++ b/goal_sender/scripts/test_engine.py @@ -0,0 +1,30 @@ +# 1. launch navigation. +# 2. execute "rosrun (this_package) Navigation.py". +# 3. run route_guidance_engine server. +# 4. run this test. + + +from route_guidance_engine_client import IF +import time +from pyrois.RoIS_Common import Component_Status + +s = 'http://127.0.0.1:8000' +i = IF(s) +i.connect() +i.search("engine_profile_test") +print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')) +#i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis') +destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] +for dest in destinations: + print(i.set_parameter('Navigation',[dest, "", ""])) + navi_status = Component_Status.BUSY + print("TEST: status = {}".format(navi_status)) + while navi_status != Component_Status.READY: + server_status, navi_status = i.get_navi_status() + print("TEST: status = {}".format(navi_status)) + time.sleep(1) + time.sleep(3) + +#print(i.analysis_c_status()) +i.disconnect() +print("finish") diff --git a/navigaion_signal_sender/scripts/nagigation_signal_sender.py b/navigaion_signal_sender/scripts/nagigation_signal_sender.py deleted file mode 100755 index 0f24f5ac5036ffdf549d0ec125bbf52fb73e783c..0000000000000000000000000000000000000000 --- a/navigaion_signal_sender/scripts/nagigation_signal_sender.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -import rospy -import actionlib -from actionlib_msgs.msg import GoalStatus - -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PoseStamped - -def pose_to_goal(pose): - goal = MoveBaseGoal() - goal.target_pose.header.frame_id = 'map' - goal.target_pose.pose.position.x = pose[0][0] - goal.target_pose.pose.position.y = pose[0][1] - goal.target_pose.pose.position.z = pose[0][2] - goal.target_pose.pose.orientation.x = pose[1][0] - goal.target_pose.pose.orientation.y = pose[1][1] - goal.target_pose.pose.orientation.z = pose[1][2] - goal.target_pose.pose.orientation.w = pose[1][3] - - return goal - - -def main(): - rospy.init_node('navigation_signal_sender') - - action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - action_client.wait_for_server() - - pose = [(10.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)] - goal = pose_to_goal(pose) - state = action_client.send_goal_and_wait(goal) - if state == GoalStatus.SUCCEEDED: - rospy.loginfo("state: SUCCEEDED!!") - elif state == GoalStatus.ABORTED: - rospy.loginfo("state: ABORTED...") - else: - rospy.loginfo("state: {}".format(state)) - - -if __name__ == '__main__': - main()