def _simulate(self, planner, start=None, finish=None):
# set goal
msg = PodiMoveBaseActionGoal()
msg.goal.target_poses.header.frame_id = "map"
c = Convert()
end_goals = PoseArray()
for goal in finish:
if planner == "robot_only":
new_xy = c.human_to_robot(goal["x"], goal["y"], goal["angle"])
new_pose = Pose()
new_pose.position.x = new_xy[0]
new_pose.position.y = new_xy[1]
new_pose.position.z = 0
elif planner == "coupled":
new_pose = Pose()
new_pose.position.x = goal["x"] #+ 0.32 * math.cos(finish["x"] * math.pi / 180)
new_pose.position.y = goal["y"] #+ 0.32 * math.sin(finish["y"] * math.pi / 180)
new_pose.position.z = 0
else:
rospy.logerr("Invalid planner input to simulation function")
self._kill()
fangle = goal["angle"] * math.pi / 180
qfinish = qstart = tf.transformations.quaternion_from_euler(0, 0, fangle)
new_pose.orientation.x = qfinish[0]
new_pose.orientation.y = qfinish[1]
new_pose.orientation.z = qfinish[2]
new_pose.orientation.w = qfinish[3]
msg.goal.target_poses.poses.append(new_pose)
end_goals.header.frame_id = "map"
end_goals.poses.append(new_pose)
time.sleep(3)
rospy.logerr(msg)
for j in range (0, 10):
self._endgoal_pub.publish(end_goals)
self._goal_pub.publish(msg)
quaternion = (g["goal"][0]["orientation"]["x"],
g["goal"][0]["orientation"]["y"],
g["goal"][0]["orientation"]["z"],
g["goal"][0]["orientation"]["w"])
c = Convert()
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
theta = yaw * 180 / math.pi
if g["planner"] == "robot_only":
#new_xy = c.robot_to_human(g["goal"][0]["goal"]["target_poses"]["poses"][0]["position"]["x"], g["goal"][0]["goal"]["target_poses"]["poses"][0]["position"]["y"], theta)
new_xy = c.robot_to_human(g["goal"][0]["position"]["x"],
g["goal"][0]["position"]["y"], theta)
else:
#new_xy = c.human_to_robot(g["goal"][0]["goal"]["target_poses"]["poses"][0]["position"]["x"], g["goal"][0]["goal"]["target_poses"]["poses"][0]["position"]["y"], theta)
new_xy = c.human_to_robot(g["goal"][0]["position"]["x"],
g["goal"][0]["position"]["y"], theta)
g2_marker.pose.position.x = new_xy[0]
g2_marker.pose.position.y = new_xy[1]
g2_marker.pose.position.z = g["goal"][0]["position"][
"z"] #g["goal"][0]["goal"]["target_poses"]["poses"][0]["position"]["z"]
g2_marker.pose.orientation.x = g["goal"][0]["orientation"][
"x"] #g["goal"][0]["goal"]["target_poses"]["poses"][0]["orientation"]["x"]
g2_marker.pose.orientation.y = g["goal"][0]["orientation"][
"y"] #g["goal"][0]["goal"]["target_poses"]["poses"][0]["orientation"]["y"]
g2_marker.pose.orientation.z = g["goal"][0]["orientation"][
"z"] #g["goal"][0]["goal"]["target_poses"]["poses"][0]["orientation"]["z"]
g2_marker.pose.orientation.w = g["goal"][0]["orientation"][
"w"] #g["goal"][0]["goal"]["target_poses"]["poses"][0]["orientation"]["w"]
g2_marker.scale.x = 0.07
g2_marker.scale.y = 0.02
rospy.logerr("Error on calling service: %s", str(e))
for i in range(0, 100000):
self._rviz_pub.publish(initial)
time.sleep(1)
# set goal
msg = PodiMoveBaseActionGoal()
msg.goal.target_poses.header.frame_id = "map"
end_goals = PoseArray()
for goal in finish:
self._description_pub.publish(goal["description"])
if planner == "robot_only":
new_xy = c.human_to_robot(goal["x"], goal["y"], goal["angle"])
new_pose = Pose()
new_pose.position.x = new_xy[0]
new_pose.position.y = new_xy[1]
new_pose.position.z = 0
elif planner == "coupled":
new_pose = Pose()
new_pose.position.x = goal["x"]
new_pose.position.y = goal["y"]
new_pose.position.z = 0
else:
rospy.logerr("Invalid planner input to simulation function")
self._kill()
fangle = goal["angle"] * math.pi / 180
qfinish = qstart = tf.transformations.quaternion_from_euler(