Mentions légales du service

Skip to content
Snippets Groups Projects
Commit 24053f58 authored by AUTERNAUD Alex's avatar AUTERNAUD Alex
Browse files

new frame as parameters

parent 4938f2b6
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/robot_behavior/launch/behavior_planning_manager.launch
+ 4
− 0
View file @ 24053f58
......@@ -22,6 +22,8 @@
<arg name="planning_manager_loop_time" default="2"/>
<arg name="timer_error_max_time" default="5"/>
<arg name="final_goal_frame" default="final_goal"/>
<arg name="pan_goal_frame" default="pan_goal"/>
<arg name="human_goal_frame" default="human_goal"/>
<node pkg="robot_behavior" type="behavior_planning_manager_main.py" name="planning_manager" output="screen">
......@@ -47,5 +49,7 @@
<param name="planning_manager_loop_time" type="int" value="$(arg planning_manager_loop_time)"/>
<param name="timer_error_max_time" type="int" value="$(arg timer_error_max_time)"/>
<param name="final_goal_frame" type="str" value="$(arg final_goal_frame)"/>
<param name="pan_goal_frame" type="str" value="$(arg pan_goal_frame)"/>
<param name="human_goal_frame" type="str" value="$(arg human_goal_frame)"/>
</node>
</launch>
src/robot_behavior/launch/new_main.launch
+ 4
− 0
View file @ 24053f58
......@@ -60,6 +60,8 @@
<arg name="goal_finder_loop_time" default="2"/>
<arg name="final_goal_frame" default="final_goal"/>
<arg name="intermediate_waypoint_goal_frame" default="intermediate_waypoint_goal"/>
<arg name="pan_goal_frame" default="pan_goal"/>
<arg name="human_goal_frame" default="human_goal"/>
<group ns="$(arg namespace)" if="$(arg action_servers)">
......@@ -129,6 +131,8 @@
<arg name="planning_manager_loop_time" value="$(arg planning_manager_loop_time)"/>
<arg name="timer_error_max_time" value="$(arg timer_error_max_time)"/>
<arg name="final_goal_frame" value="$(arg final_goal_frame)"/>
<arg name="pan_goal_frame" value="$(arg pan_goal_frame)"/>
<arg name="human_goal_frame" value="$(arg human_goal_frame)"/>
</include>
</group>
......
src/robot_behavior/src/robot_behavior/behavior_planning_manager_node.py
+ 17
− 15
View file @ 24053f58
......@@ -59,6 +59,8 @@ class PlanningManagerNode:
self.timer_error_max_time = rospy.get_param('~timer_error_max_time', 5) # seconds
self.parent_frame = self.map_frame # by default
self.final_goal_frame = rospy.get_param('~final_goal_frame', 'final_goal')
self.pan_goal_frame = rospy.get_param('~pan_goal_frame', 'pan_goal')
self.human_goal_frame = rospy.get_param('~human_goal_frame', 'human_goal')
self.planning_manager_loop_time = rospy.get_param('~planning_manager_loop_time', 2)
self.read_robot_config(filename=self.robot_config_path)
......@@ -415,7 +417,7 @@ class PlanningManagerNode:
(self.x_pan_target, self.y_pan_target, 0),
quat_pan_goal,
current_time,
"pan goal",
self.pan_goal_frame,
self.parent_frame
)
......@@ -427,7 +429,7 @@ class PlanningManagerNode:
(self.x_human_features, self.y_human_features, 0),
quat,
current_time,
"human_goal",
self.human_goal_frame,
self.parent_frame
)
......@@ -634,14 +636,14 @@ class PlanningManagerNode:
goal.timeout = self.default_timeout
self.global_path_planner_action_server(goal=goal)
while self._global_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = LocalPlannerGoal()
# goal.intermediate_waypoint =
goal.timeout = self.default_timeout
self.local_path_planner_action_server(goal=goal)
while self._local_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
elif go_to_position_target[-2] == 0:
# global frame
self.parent_frame = self.map_frame
......@@ -652,14 +654,14 @@ class PlanningManagerNode:
goal.timeout = self.default_timeout
self.global_path_planner_action_server(goal=goal)
while self._global_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = LocalPlannerGoal()
# goal.intermediate_waypoint =
goal.timeout = self.default_timeout
self.local_path_planner_action_server(goal=goal)
while self._local_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
else:
# reset done above
rospy.logwarn('Wrong position, go_to position with second last flag == {} is not a valid value'.format(go_to_position_target[-2]))
......@@ -698,21 +700,21 @@ class PlanningManagerNode:
goal.timeout = self.default_timeout
self.goal_finder_action_server(goal=goal)
while self._goal_finder_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = GlobalPlannerGoal()
# goal.final_goal =
goal.timeout = self.default_timeout
self.global_path_planner_action_server(goal=goal)
while self._global_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = LocalPlannerGoal()
# goal.intermediate_waypoint =
goal.timeout = self.default_timeout
self.local_path_planner_action_server(goal=goal)
while self._local_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
self.setting_goals = False
......@@ -736,21 +738,21 @@ class PlanningManagerNode:
goal.timeout = self.default_timeout
self.goal_finder_action_server(goal=goal)
while self._goal_finder_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = GlobalPlannerGoal()
# goal.final_goal =
goal.timeout = self.default_timeout
self.global_path_planner_action_server(goal=goal)
while self._global_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = LocalPlannerGoal()
# goal.intermediate_waypoint =
goal.timeout = self.default_timeout
self.local_path_planner_action_server(goal=goal)
while self._local_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
self.setting_goals = False
......@@ -774,21 +776,21 @@ class PlanningManagerNode:
goal.timeout = self.default_timeout
self.goal_finder_action_server(goal=goal)
while self._goal_finder_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = GlobalPlannerGoal()
# goal.final_goal =
goal.timeout = self.default_timeout
self.global_path_planner_action_server(goal=goal)
while self._global_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
goal = LocalPlannerGoal()
# goal.intermediate_waypoint =
goal.timeout = self.default_timeout
self.local_path_planner_action_server(goal=goal)
while self._local_path_planner_action_client.get_state() != GoalStatus.ACTIVE:
pass
rospy.sleep(0.02)
self.setting_goals = False
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment