def ready_state(self):
"""Actions performed in ready state"""
# Docking
dock_goal = MoveActionGoal()
dock_goal.goal.dock_frame = self.pregoal_link
dock_goal.goal.robot_dock_frame = self.robot_link
#dock_goal.goal.dock_frame = self.goal_link
#dock_goal.goal.dock_offset.x = -0.5
self.dock_action_client.send_goal(dock_goal)
self.dock_action_client.wait_for_result()
if self.dock_action_client.get_result() == True:
# Execute move
if self.tf.frameExists(self.robot_link) and self.tf.frameExists(
self.goal_link):
# Get relative goal position
t = self.tf.getLatestCommonTime(self.robot_link,
self.goal_link)
(position,
quaternion) = self.tf.lookupTransform(self.robot_link,
self.goal_link, t)
(_, _, orientation) = euler_from_quaternion(quaternion)
# Orientate robot
move_goal = MoveActionGoal()
move_goal.goal.goal.theta = orientation
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
if self.move_action_client.get_result() == True:
# Get relative goal position
t = self.tf.getLatestCommonTime(self.robot_link,
self.goal_link)
(position, quaternion) = self.tf.lookupTransform(
self.robot_link, self.goal_link, t)
(_, _, orientation) = euler_from_quaternion(quaternion)
# Move forward
move_goal = MoveActionGoal()
move_goal.goal.goal.x = position[0]
self.move_action_client.send_goal(move_goal)
self.move_action_client.wait_for_result()
else:
rospy.ERROR("Move failed. TF are not available")
else:
rospy.ERROR("Docking failed")
switch_to_state(State.SHUTDOWN_STATE)
def navigation_callback(message):
# type: (String) -> None
print(message.data)
rospy.wait_for_service("location/request")
try:
request = rospy.ServiceProxy('location/request',
Request_Location)
response = request(
message.data) # type: Request_LocationResponse
client = actionlib.SimpleActionClient("/move_base",
MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose = response.location.pose
client.send_goal(goal)
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
print("SUCCEEDED")
if client.get_state() == GoalStatus.ABORTED:
print("ABORTED")
except rospy.ServiceException as e:
rospy.ERROR(e)
return
'''
def init_params(self):
self.remote_vin = rospy.get_param("~remote_vin")
self.host = rospy.get_param("~host")
self.port = rospy.get_param("~port")
self.send_rate = rospy.get_param("~send_rate")
if self.remote_vin is not None and self.host is not None and self.port is not None:
self.client = Connector(self.event_bus, self.host, self.port)
else:
rospy.ERROR("[remote_control_g29]: config unset, exit.")
exit(1)
def _initial_(self):
storage = rospy.get_param("~file_path", default="/data")
if not os.path.exists(storage):
try:
os.mkdir(storage)
except Exception as e:
rospy.ERROR(
"could not make dataSet dir: {} , please check it".format(
storage))
rospy.ERROR(e)
exit(1)
dir_name = time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime())
self.full_path = os.path.join(storage, dir_name)
try:
os.mkdir(self.full_path)
except Exception as e:
rospy.ERROR(
"could not make dataSet dir: {} , please check it".format(
self.full_path))
rospy.ERROR(e)
exit(1)
def navigation_callback(self, message):
# type: (Float64) -> None
"""
移動命令を受け取って実際にmove_baseに移動命令をアクションで送る
:param message: 場所情報
:return: なし
"""
try:
transform = None
for i in range(10):
if self.transform is not None:
transform = self.transform
break
rospy.sleep(0.5)
# 接続要求
# gmapping か amcl が立ち上げっていれば繋がる
client = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
print("wait move_base server")
client.wait_for_server()
print("callback move_base")
# データの作成
# ただしなぜかは不明だが orientation(型はOrientation)の初期値は(0,0,0,0)だがこれだと動かない (原因は不明)
# 必ず w の値を0以外に設定する
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose = self.calc(transform, message.data)
self.rviz.register(goal.target_pose.pose)
# データの送信
print("send place msg @navigation")
client.send_goal(goal)
# データ送信後アクションサーバーからの返答待ち
# 失敗にしろ成功にしろ結果は返ってくるのでタイムアウトの必要はなし
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
print("SUCCEEDED")
self.result_publisher.publish(True)
elif client.get_state() == GoalStatus.ABORTED:
# orientationのw値が0だとこっちが即返ってくる
print("ABORTED")
self.result_publisher.publish(False)
except rospy.ServiceException as e:
rospy.ERROR(e)
def navigation_callback(self, message):
# type: (Location) -> None
"""
移動命令を受け取って実際にmove_baseに移動命令をアクションで送る
:param message: 場所情報
:return: なし
"""
print(message, "@navigation_callback")
try:
# 接続要求
# gmapping か amcl が立ち上げっていれば繋がる
client = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
print("wait move_base server")
client.wait_for_server()
print("callback move_base")
# データの作成
# ただしなぜかは不明だが orientation(型はOrientation)の初期値は(0,0,0,0)だがこれだと動かない (原因は不明)
# 必ず w の値を0以外に設定する
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position = Point(message.x, message.y,
message.z)
goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1)
# データの送信
print("send place msg @navigation")
client.send_goal(goal)
# データ送信後アクションサーバーからの返答待ち
# 失敗にしろ成功にしろ結果は返ってくるのでタイムアウトの必要はなし
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
print("SUCCEEDED")
answer = "I arrived at the target point"
rospy.wait_for_service(self.speak_topic)
rospy.ServiceProxy(self.speak_topic, StringService)(answer)
self.result_publisher.publish(True)
elif client.get_state() == GoalStatus.ABORTED:
# orientationのw値が0だとこっちが即返ってくる
print("ABORTED")
answer = "Sorry, I can't arrived at the target point"
rospy.wait_for_service(self.speak_topic)
rospy.ServiceProxy(self.speak_topic, StringService)(answer)
self.result_publisher.publish(False)
except rospy.ServiceException as e:
rospy.ERROR(e)
def set_gripper_state(self, state='close'):
"""
set gripper state (binary): open or close
input: string ('close' or 'open')
output: goal result
"""
# self.__getCurrentFingerPosition(self._prefix)
positions = [0.0, 0.0]
if state == 'open':
positions = [0.0, 0.0]
elif state == 'close':
positions = [self._finger_maxTurn, self._finger_maxTurn]
else:
rospy.ERROR('Invalid gripper state. Options: close OR open')
return None
return self.__gripper_client(positions)
def get_linkable_vars(self):
return list(
set(self.controlled_vars.keys()) | set(self.fixed_vars.keys()))
def delete_dyn_reconfig_var(self, var):
self.set_dyn_reconfig_var(var)
if var in self.get_linkable_vars():
if var in self.controlled_vars:
del self.controlled_vars[var]
elif var in self.fixed_vars:
del self.fixed_vars[var]
if var in self.linked_vars:
for var2 in self.linked_vars[var]:
self.set_dyn_reconfig_var(var2)
del self.links[var2]
del self.linked_vars[var]
elif var in self.links:
self.linked_vars[self.links[var]].remove(var)
del self.links[var]
def refresh_current_config(self):
self.current_config = self.dyn_reconfig_client.get_configuration()
if __name__ == '__main__':
try:
learning_client = LearningClient()
learning_gui = LearningGUI(learning_client)
except rospy.ROSInterruptException:
rospy.ERROR("program interrupted before completion")
