From dca94b1ed3063024491394a7c3952dd9c26d044d Mon Sep 17 00:00:00 2001 From: billy Date: Mon, 27 Mar 2017 12:04:35 -0500 Subject: [PATCH 1/7] Moved the front kinect --- simulation_2017/urdf/cr17.xacro | 32 ++++++++++++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/simulation_2017/urdf/cr17.xacro b/simulation_2017/urdf/cr17.xacro index 64b85cd..b42c9d5 100644 --- a/simulation_2017/urdf/cr17.xacro +++ b/simulation_2017/urdf/cr17.xacro @@ -17,12 +17,15 @@ + + + @@ -61,6 +64,11 @@ + + + + + @@ -621,6 +629,22 @@ + + + + + + + + + + + + + + + + @@ -630,9 +654,13 @@ - + + + + + - From 66df5e8c9c03cdd4c993f8d214961c5f048eede0 Mon Sep 17 00:00:00 2001 From: billy Date: Mon, 27 Mar 2017 14:30:08 -0500 Subject: [PATCH 2/7] initial setup of nav stack, only test was that the launch file didn't crash and showed the map correctly in rviz --- cr17/launch/move_base.launch | 15 +++++++++++++++ cr17/package.xml | 2 ++ cr17/param/base_local_planner_params.yaml | 11 +++++++++++ cr17/param/costmap_common_params.yaml | 9 +++++++++ cr17/param/global_costmap_params.yaml | 5 +++++ cr17/param/local_costmap_params.yaml | 10 ++++++++++ cr17/param/rmc_field_map.pgm | Bin 0 -> 13153 bytes cr17/param/rmc_field_map.yaml | 6 ++++++ simulation_2017/urdf/cr17.xacro | 6 ++---- 9 files changed, 60 insertions(+), 4 deletions(-) create mode 100644 cr17/launch/move_base.launch create mode 100644 cr17/param/base_local_planner_params.yaml create mode 100644 cr17/param/costmap_common_params.yaml create mode 100644 cr17/param/global_costmap_params.yaml create mode 100644 cr17/param/local_costmap_params.yaml create mode 100644 cr17/param/rmc_field_map.pgm create mode 100644 cr17/param/rmc_field_map.yaml diff --git a/cr17/launch/move_base.launch b/cr17/launch/move_base.launch new file mode 100644 index 0000000..7e93602 --- /dev/null +++ b/cr17/launch/move_base.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/cr17/package.xml b/cr17/package.xml index ae4f8df..9929cf4 100644 --- a/cr17/package.xml +++ b/cr17/package.xml @@ -45,10 +45,12 @@ std_msgs joy tf + move_base roscpp rospy std_msgs joy + move_base diff --git a/cr17/param/base_local_planner_params.yaml b/cr17/param/base_local_planner_params.yaml new file mode 100644 index 0000000..ac9b8c6 --- /dev/null +++ b/cr17/param/base_local_planner_params.yaml @@ -0,0 +1,11 @@ +TrajectoryPlannerROS: + max_vel_x: 0.45 + min_vel_x: 0.1 + max_vel_theta: 1.0 + min_in_place_vel_theta: 0.4 + + acc_lim_theta: 3.2 + acc_lim_x: 2.5 + acc_lim_y: 0 + + holonomic_robot: false \ No newline at end of file diff --git a/cr17/param/costmap_common_params.yaml b/cr17/param/costmap_common_params.yaml new file mode 100644 index 0000000..887c84e --- /dev/null +++ b/cr17/param/costmap_common_params.yaml @@ -0,0 +1,9 @@ +obstacle_range: 3.0 +raytrace_range: 3.5 +footprint: [[0.3301, 1.18], [0.3301, -0.282575], [-0.3301, -0.282575], [-0.3301, 1.18]] #back is 0.282575 backedge from base_link center 1.18m forward sides are 0.3301 +inflation_radius: 0.55 + +observation_sources: front_kinect rear_kinect + +front_kinect: {sensor_frame: front_camera_depth_optical_frame, data_type: PointCloud, topic: /front_camera/depth/points, marking: true, clearing: true} +rear_kinect: {sensor_frame: rear_camera_depth_optical_frame, data_type: PointCloud, topic: /rear_camera/depth/points, marking: true, clearing: true} diff --git a/cr17/param/global_costmap_params.yaml b/cr17/param/global_costmap_params.yaml new file mode 100644 index 0000000..1becb82 --- /dev/null +++ b/cr17/param/global_costmap_params.yaml @@ -0,0 +1,5 @@ +global_costmap: + global_frame: /map + robot_base_frame: base_link + update_frequency: 5.0 + static_map: true \ No newline at end of file diff --git a/cr17/param/local_costmap_params.yaml b/cr17/param/local_costmap_params.yaml new file mode 100644 index 0000000..48f9acc --- /dev/null +++ b/cr17/param/local_costmap_params.yaml @@ -0,0 +1,10 @@ +local_costmap: + global_frame: odom + robot_base_frame: base_link + update_frequency: 5.0 + publish_frequency: 2.0 + static_map: false + rolling_window: false #default in setup is true, but we are in a small set space + width: 4.0 + height: 8.0 + resolution: 0.05 \ No newline at end of file diff --git a/cr17/param/rmc_field_map.pgm b/cr17/param/rmc_field_map.pgm new file mode 100644 index 0000000000000000000000000000000000000000..48a1bc4200651d51142a3c50edfab92f14c30298 GIT binary patch literal 13153 zcmeIzF$;oF6oz5v`zsE#n@K04O~g5oc5!fsxNvEz>HhTw7x&L4e4U!^@bNyK-owG# zp=%!Z;bw74W1L>|yG&oJ%E~Ps)AJLqXFK^N)b63++wlT*Pngd-g33*ZPxdPUqL9N`E@`T{t@ ukzNt^2uC==k-h+qaHLnnJ;D)=aHKDQBOK`!agT6>qknO - + - + @@ -629,8 +629,6 @@ - - From c36895d6b01dcc65095f6eba71c45d214395a9e6 Mon Sep 17 00:00:00 2001 From: bgrayd Date: Mon, 27 Mar 2017 17:00:38 -0500 Subject: [PATCH 3/7] I think the transforms now work --- cr17/param/costmap_common_params.yaml | 4 +- simulation_2017/config/control_cr17.yaml | 2 +- .../launch/control_autonomous.launch | 2 + simulation_2017/nodes/magic_odomFromGazebo.py | 47 ++++++++++++++++++- simulation_2017/urdf/cr17.xacro | 4 +- 5 files changed, 53 insertions(+), 6 deletions(-) diff --git a/cr17/param/costmap_common_params.yaml b/cr17/param/costmap_common_params.yaml index 887c84e..5349ec3 100644 --- a/cr17/param/costmap_common_params.yaml +++ b/cr17/param/costmap_common_params.yaml @@ -5,5 +5,5 @@ inflation_radius: 0.55 observation_sources: front_kinect rear_kinect -front_kinect: {sensor_frame: front_camera_depth_optical_frame, data_type: PointCloud, topic: /front_camera/depth/points, marking: true, clearing: true} -rear_kinect: {sensor_frame: rear_camera_depth_optical_frame, data_type: PointCloud, topic: /rear_camera/depth/points, marking: true, clearing: true} +front_kinect: {sensor_frame: front_camera_depth_optical_frame, data_type: PointCloud2, topic: /front_camera/depth/points, marking: true, clearing: true} +rear_kinect: {sensor_frame: rear_camera_depth_optical_frame, data_type: PointCloud2, topic: /rear_camera/depth/points, marking: true, clearing: true} diff --git a/simulation_2017/config/control_cr17.yaml b/simulation_2017/config/control_cr17.yaml index 6bdea22..db4178e 100644 --- a/simulation_2017/config/control_cr17.yaml +++ b/simulation_2017/config/control_cr17.yaml @@ -24,7 +24,7 @@ cr17_diff_drive_controller: twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] # Top level frame (link) of the robot description - base_frame_id: base_link + base_frame_id: test # Velocity and acceleration limits for the robot linear: diff --git a/simulation_2017/launch/control_autonomous.launch b/simulation_2017/launch/control_autonomous.launch index c339aec..ace9686 100644 --- a/simulation_2017/launch/control_autonomous.launch +++ b/simulation_2017/launch/control_autonomous.launch @@ -32,4 +32,6 @@ + + diff --git a/simulation_2017/nodes/magic_odomFromGazebo.py b/simulation_2017/nodes/magic_odomFromGazebo.py index f3dec77..343ae38 100755 --- a/simulation_2017/nodes/magic_odomFromGazebo.py +++ b/simulation_2017/nodes/magic_odomFromGazebo.py @@ -1,11 +1,12 @@ #!/usr/bin/env python import rospy +import tf from gazebo_msgs.msg import ModelStates from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseWithCovariance, TwistWithCovariance, PoseArray, PoseStamped -MODELPREFIX = rospy.get_param("magic_modelNamePrefix", "r") +MODELPREFIX = rospy.get_param("magic_modelNamePrefix", "robot") class PFieldNavigator(object): @@ -18,6 +19,10 @@ def __init__(self): self.pub_dict2 = {} self.pub_dict3 = {} self.last_poses = {} + + self.br = tf.TransformBroadcaster() + self.firstRobotPose = None + self.robotPose = None ###################################### # Setup ROS publishers @@ -39,6 +44,9 @@ def gazebo_modelstates_callback(self, data): pose = PoseStamped() while i < len(data.name): if (data.name[i]).startswith(MODELPREFIX): + # if self.firstRobotPose == None: + # self.firstRobotPose = data.pose[i] + # self.robotPose = data.pose[i] name = data.name[i] pose.pose = data.pose[i] twist = data.twist[i] @@ -46,6 +54,31 @@ def gazebo_modelstates_callback(self, data): odom.pose.pose = pose odom.twist.twist = twist + self.br.sendTransform( + (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z), + (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w), + rospy.Time.now(), + name, + "odom" + ) + self.br.sendTransform( + (0,0,0), + # (0,0,-0.7071067811865476,0.7071067811865476), + # (0.7071067811865476, -0.7071067811865475,-4.329780281177466e-17, 4.329780281177467e-17 ), + # (4.329780281177467e-17 , -4.329780281177466e-17, -0.7071067811865475, 0.7071067811865476), + (0,0,0,1), + rospy.Time.now(), + "odom", + "map" + ) + self.br.sendTransform( + (0,0,0), + (0,0,0,1), + rospy.Time.now(), + "base_link", + "robot" + ) + self.last_poses[name] = pose poses.poses = self.last_poses.values() poses.poses.remove(pose) @@ -67,6 +100,18 @@ def run(self): ''' rate = rospy.Rate(10) while not rospy.is_shutdown(): + # if self.firstRobotPose != None and self.robotPose != None: + # self.br.sendTransform( + # (self.firstRobotPose.position.x, self.firstRobotPose.position.y, 0), + # #(0,0,-0.7071067811865476,0.7071067811865476), + # #(0.7071067811865476, -0.7071067811865475,-4.329780281177466e-17, 4.329780281177467e-17 ), + # #(4.329780281177467e-17 , -4.329780281177466e-17, -0.7071067811865475, 0.7071067811865476), + # #(0,0,0,1), + # (self.robotPose.orientation.x, self.robotPose.orientation.y, self.robotPose.orientation.z, self.robotPose.orientation.w), + # rospy.Time.now(), + # "odom", + # "map" + # ) rate.sleep() if __name__ == "__main__": diff --git a/simulation_2017/urdf/cr17.xacro b/simulation_2017/urdf/cr17.xacro index 57b1619..b679e6a 100644 --- a/simulation_2017/urdf/cr17.xacro +++ b/simulation_2017/urdf/cr17.xacro @@ -20,11 +20,11 @@ - + - + From 95e82302bab33910849ee40b0e7baf8520add95c Mon Sep 17 00:00:00 2001 From: bgrayd Date: Fri, 31 Mar 2017 13:26:04 -0500 Subject: [PATCH 4/7] The nav stack now runs the robot, but the hlsc needs modifications and the old navigator should be used for docking --- cr17/launch/move_base.launch | 2 ++ .../nodes/cr17/high_level_state_controller.py | 19 +++++++++++++++++++ cr17/nodes/cr17/path_pub.py | 9 +++++++++ cr17/param/costmap_common_params.yaml | 6 +++--- cr17/param/local_costmap_params.yaml | 2 +- .../launch/control_autonomous.launch | 2 +- simulation_2017/param/sim_scoop_config.yaml | 2 +- 7 files changed, 36 insertions(+), 6 deletions(-) diff --git a/cr17/launch/move_base.launch b/cr17/launch/move_base.launch index 7e93602..c383fdd 100644 --- a/cr17/launch/move_base.launch +++ b/cr17/launch/move_base.launch @@ -11,5 +11,7 @@ + + \ No newline at end of file diff --git a/cr17/nodes/cr17/high_level_state_controller.py b/cr17/nodes/cr17/high_level_state_controller.py index 2e5c9d5..b9ec5f0 100755 --- a/cr17/nodes/cr17/high_level_state_controller.py +++ b/cr17/nodes/cr17/high_level_state_controller.py @@ -72,6 +72,8 @@ def pose_sub(self, data): def path_sub(self, data): self.pathPoses = data.poses + for each in self.pathPoses: + each.header.frame_id = "map" def arm_drive_state(self): newMsg = scoopControl() @@ -110,8 +112,13 @@ def arm_postdump_state(self): def calculate_mining_path(self, angle): startPose = PoseStamped() + startPose.header.frame_id = "map" startPose.pose.position.x = self.pose.position.x startPose.pose.position.y =Y_IN_MINING_AREA - 0.4#TODO: make this better, or improve the state transitions, this is so that the mining state will be left + startPose.pose.orientation.x = self.pose.orientation.x + startPose.pose.orientation.y = self.pose.orientation.y + startPose.pose.orientation.z = self.pose.orientation.z + startPose.pose.orientation.w = self.pose.orientation.w start_x = self.pose.position.x start_y = self.pose.position.y @@ -124,9 +131,16 @@ def calculate_mining_path(self, angle): end_x = abs(end_x)/end_x * 1.2 #so it won't run into the wall path = [] + + #this needs to include the orientation if we use the nav_stack, and since the orientation is a quaternion it will need conversion endPose = PoseStamped() + endPose.header.frame_id = "map" endPose.pose.position.x = end_x endPose.pose.position.y = end_y + endPose.pose.orientation.x = 0 + endPose.pose.orientation.y = 0 + endPose.pose.orientation.z = 0 + endPose.pose.orientation.w = 1 path.append(startPose) path.append(endPose) @@ -219,8 +233,13 @@ def run(self): elif(self.autostate == "DOCKING"): self.arm_predump_state() dockPose = PoseStamped() + dockPose.header.frame_id = "map" dockPose.pose.position.x = X_POS_DUMP dockPose.pose.position.y = Y_POS_DUMP + dockPose.pose.orientation.x = 0 + dockPose.pose.orientation.y = 0 + dockPose.pose.orientation.z = -0.7071067811865476 + dockPose.pose.orientation.w = 0.7071067811865476 self.goal_pub.publish(dockPose) elif(self.autostate == "DUMPING"): diff --git a/cr17/nodes/cr17/path_pub.py b/cr17/nodes/cr17/path_pub.py index daf74be..2c24e3e 100755 --- a/cr17/nodes/cr17/path_pub.py +++ b/cr17/nodes/cr17/path_pub.py @@ -23,9 +23,18 @@ def run(self): point1 = PoseStamped() point1.pose.position.x = POINT_1_X point1.pose.position.y = POINT_1_Y + point1.pose.orientation.x = 0 + point1.pose.orientation.y = 0 + point1.pose.orientation.z = -0.7071067811865476 + point1.pose.orientation.w = -0.7071067811865476 + point2 = PoseStamped() point2.pose.position.x = POINT_2_X point2.pose.position.y = POINT_2_Y + point2.pose.orientation.x = 0 + point2.pose.orientation.y = 0 + point2.pose.orientation.z = 0.7071067811865476 + point2.pose.orientation.w = -0.7071067811865476 self.path.poses = [] self.path.poses.append(point1) diff --git a/cr17/param/costmap_common_params.yaml b/cr17/param/costmap_common_params.yaml index 5349ec3..fb366ca 100644 --- a/cr17/param/costmap_common_params.yaml +++ b/cr17/param/costmap_common_params.yaml @@ -1,9 +1,9 @@ obstacle_range: 3.0 raytrace_range: 3.5 -footprint: [[0.3301, 1.18], [0.3301, -0.282575], [-0.3301, -0.282575], [-0.3301, 1.18]] #back is 0.282575 backedge from base_link center 1.18m forward sides are 0.3301 +footprint: [[1.18, 0.3301], [-0.282575, 0.3301], [-0.282575, -0.3301], [1.18,-0.3301]] #back is 0.282575 backedge from base_link center 1.18m forward sides are 0.3301 inflation_radius: 0.55 observation_sources: front_kinect rear_kinect -front_kinect: {sensor_frame: front_camera_depth_optical_frame, data_type: PointCloud2, topic: /front_camera/depth/points, marking: true, clearing: true} -rear_kinect: {sensor_frame: rear_camera_depth_optical_frame, data_type: PointCloud2, topic: /rear_camera/depth/points, marking: true, clearing: true} +front_kinect: {max_obstacle_height: 0.35, min_obstacle_height: 0.06, data_type: PointCloud2, topic: /front_camera/depth/points, marking: true, clearing: true} +rear_kinect: {max_obstacle_height: 0.35, min_obstacle_height: 0.06, data_type: PointCloud2, topic: /rear_camera/depth/points, marking: true, clearing: true} diff --git a/cr17/param/local_costmap_params.yaml b/cr17/param/local_costmap_params.yaml index 48f9acc..903953c 100644 --- a/cr17/param/local_costmap_params.yaml +++ b/cr17/param/local_costmap_params.yaml @@ -4,7 +4,7 @@ local_costmap: update_frequency: 5.0 publish_frequency: 2.0 static_map: false - rolling_window: false #default in setup is true, but we are in a small set space + rolling_window: true #default in setup is true, but we are in a small set space width: 4.0 height: 8.0 resolution: 0.05 \ No newline at end of file diff --git a/simulation_2017/launch/control_autonomous.launch b/simulation_2017/launch/control_autonomous.launch index ace9686..ff972ad 100644 --- a/simulation_2017/launch/control_autonomous.launch +++ b/simulation_2017/launch/control_autonomous.launch @@ -17,7 +17,7 @@ - + diff --git a/simulation_2017/param/sim_scoop_config.yaml b/simulation_2017/param/sim_scoop_config.yaml index 61dc928..8526c85 100644 --- a/simulation_2017/param/sim_scoop_config.yaml +++ b/simulation_2017/param/sim_scoop_config.yaml @@ -1,5 +1,5 @@ scoop_config: - driving_arm_angle: 0 + driving_arm_angle: -50 #0 driving_scoop_angle: -20 pre_dig_arm_angle: 70 pre_dig_scoop_angle: 100 From d2c7c2d149def5e2758ecb46697b0f2656758fc5 Mon Sep 17 00:00:00 2001 From: billy Date: Mon, 3 Apr 2017 11:37:03 -0500 Subject: [PATCH 5/7] Modified the hlsc to use more of the nav_stack --- .../nodes/cr17/high_level_state_controller.py | 91 +++++++------------ cr17/param/base_local_planner_params.yaml | 4 +- .../launch/control_autonomous.launch | 2 - 3 files changed, 36 insertions(+), 61 deletions(-) diff --git a/cr17/nodes/cr17/high_level_state_controller.py b/cr17/nodes/cr17/high_level_state_controller.py index b9ec5f0..25ba730 100755 --- a/cr17/nodes/cr17/high_level_state_controller.py +++ b/cr17/nodes/cr17/high_level_state_controller.py @@ -16,7 +16,7 @@ STATE_TOPIC = rospy.get_param("topics/robot_state", "state") MINING_DISTANCE = rospy.get_param("distance_to_mine", 1.5) -Y_IN_DUMP_RANGE = 0.6 +Y_IN_DUMP_RANGE = 0.4 Y_IN_MINING_AREA = 4.5 #10 cm past edge of mining area Y_IN_DOCKING_AREA = 1.5 @@ -40,25 +40,39 @@ def __init__(self): self.state_pub = rospy.Publisher(STATE_TOPIC, String, queue_size=10) self.pose = Pose() - self.pathPoses = [] #TODO: maybe remove, send entire current path to navigator - self.curPathIndex = 0 #TODO: maybe remove, send entire current path to navigator self.autostate = "INIT" self.miningAngleIndex = 0 - self.miningPathIndex = 0 self.miningReady = False self.miningDone = False - self.minePath = None + self.miningTarget = None self.dumpTimer = None self.autonomyEnabled = True + self.frontMiningGoal = PoseStamped() + self.frontMiningGoal.header.frame_id = "map" + self.frontMiningGoal.pose.position.x = 0 + self.frontMiningGoal.pose.position.y = Y_IN_MINING_AREA+0.4 + self.frontMiningGoal.pose.orientation.x = 0 + self.frontMiningGoal.pose.orientation.y = 0 + self.frontMiningGoal.pose.orientation.z = -0.7071067811865476 + self.frontMiningGoal.pose.orientation.w = -0.7071067811865476 + + self.dumpSetupGoal = PoseStamped() + self.dumpSetupGoal.header.frame_id = "map" + self.dumpSetupGoal.pose.position.x = 0 + self.dumpSetupGoal.pose.position.y = 1.1 + self.dumpSetupGoal.pose.orientation.x = 0 + self.dumpSetupGoal.pose.orientation.y = 0 + self.dumpSetupGoal.pose.orientation.z = -0.7071067811865476 + self.dumpSetupGoal.pose.orientation.w = 0.7071067811865476 + # ROS Publishers self.arm_pub = rospy.Publisher(ARM_STATE_TOPIC, scoopControl, queue_size=10) self.goal_pub = rospy.Publisher(GOAL_TOPIC, PoseStamped, queue_size=10) #TODO: change to a Path #ROS Subscribers rospy.Subscriber(POSE_TOPIC, PoseStamped, self.pose_sub) - rospy.Subscriber(PATH_TOPIC, Path, self.path_sub) # ROS Services self.auto_srv = rospy.Service('autonomousHLSC', autonomousActive, self.set_autonomy) @@ -70,11 +84,6 @@ def set_autonomy(self, data): def pose_sub(self, data): self.pose = data.pose - def path_sub(self, data): - self.pathPoses = data.poses - for each in self.pathPoses: - each.header.frame_id = "map" - def arm_drive_state(self): newMsg = scoopControl() newMsg.desiredState = scoopControl.state_drivingPosition @@ -110,15 +119,7 @@ def arm_postdump_state(self): newMsg.desiredState = scoopControl.state_postDump self.arm_pub.publish(newMsg) - def calculate_mining_path(self, angle): - startPose = PoseStamped() - startPose.header.frame_id = "map" - startPose.pose.position.x = self.pose.position.x - startPose.pose.position.y =Y_IN_MINING_AREA - 0.4#TODO: make this better, or improve the state transitions, this is so that the mining state will be left - startPose.pose.orientation.x = self.pose.orientation.x - startPose.pose.orientation.y = self.pose.orientation.y - startPose.pose.orientation.z = self.pose.orientation.z - startPose.pose.orientation.w = self.pose.orientation.w + def calculate_mining_target(self, angle): start_x = self.pose.position.x start_y = self.pose.position.y @@ -130,8 +131,6 @@ def calculate_mining_path(self, angle): if abs(end_x) > 1.2: end_x = abs(end_x)/end_x * 1.2 #so it won't run into the wall - path = [] - #this needs to include the orientation if we use the nav_stack, and since the orientation is a quaternion it will need conversion endPose = PoseStamped() endPose.header.frame_id = "map" @@ -139,12 +138,10 @@ def calculate_mining_path(self, angle): endPose.pose.position.y = end_y endPose.pose.orientation.x = 0 endPose.pose.orientation.y = 0 - endPose.pose.orientation.z = 0 - endPose.pose.orientation.w = 1 + endPose.pose.orientation.z = -0.7071067811865476 + endPose.pose.orientation.w = -0.7071067811865476 - path.append(startPose) - path.append(endPose) - return path + return endPose def close_to(self, poseChecked): try: @@ -166,17 +163,12 @@ def run(self): if(self.autostate == "INIT"): pass ###################################################################################################### - #TODO: add code to orient the robot properly + #TODO: add code to orient the robot properly, may just set the arm, not sure yet ###################################################################################################### elif(self.autostate == "F_OBSTACLE_FIELD"): - if(len(self.pathPoses) != 0): - if(self.close_to(self.pathPoses[self.curPathIndex])): - self.curPathIndex += 1 - if(self.curPathIndex >= len(self.pathPoses)): - self.curPathIndex = len(self.pathPoses) - 1 - self.goal_pub.publish(self.pathPoses[self.curPathIndex]) + self.goal_pub.publish(self.frontMiningGoal) self.arm_drive_state() elif(self.autostate == "MINING_BEHAVIOR"): @@ -190,31 +182,22 @@ def run(self): elif(self.miningReady and not self.miningDone ): self.arm_dig_state() - if(self.minePath == None): - self.minePath = self.calculate_mining_path(ANGLES_TO_MINE[self.miningAngleIndex]) + if(self.miningTarget == None): + self.miningTarget = self.calculate_mining_target(ANGLES_TO_MINE[self.miningAngleIndex]) self.miningAngleIndex += 1 if(self.miningAngleIndex >= len(ANGLES_TO_MINE)): self.miningAngleIndex = 0 - self.miningPathIndex = min(1, len(self.minePath) - 1) #skip the first point when going forward - if(self.close_to(self.minePath[self.miningPathIndex])): - self.miningPathIndex += 1 - if (self.miningPathIndex >= len(self.minePath)): - self.miningPathIndex = len(self.minePath) - 1 - self.goal_pub.publish(self.minePath[self.miningPathIndex]) + self.goal_pub.publish(self.miningTarget) - if(self.close_to(self.minePath[len(self.minePath) - 1])): + if(self.close_to(self.miningTarget)): self.miningDone = True self.arm_postdig_state() #once the end of the path has been reached, close the hand and iterate back across path elif self.miningDone: self.arm_postdig_state() - if (self.close_to(self.minePath[self.miningPathIndex])): - self.miningPathIndex -= 1 - if (self.miningPathIndex < 0): - self.miningPathIndex = 0 - self.goal_pub.publish(self.minePath[self.miningPathIndex]) + self.goal_pub.publish(self.dumpSetupGoal) #once the beginning has been reached again @@ -223,12 +206,8 @@ def run(self): ###################################################################################################### elif(self.autostate == "B_OBSTACLE_FIELD"): + self.goal_pub.publish(self.dumpSetupGoal) self.arm_drive_state() - if(self.close_to(self.pathPoses[self.curPathIndex])): - self.curPathIndex -= 1 - if(self.curPathIndex < 0 ): - self.curPathIndex = 0 - self.goal_pub.publish(self.pathPoses[self.curPathIndex]) elif(self.autostate == "DOCKING"): self.arm_predump_state() @@ -245,7 +224,7 @@ def run(self): elif(self.autostate == "DUMPING"): dumping_complete = False if(self.dumpTimer == None): - self.dumpTimer = rospy.Time.now() + rospy.Duration(TIME_DUMP_SEC) + self.dumpTimer = rospy.Time.now() + rospy.Duration(secs = TIME_DUMP_SEC) elif(self.dumpTimer >= rospy.Time.now() ): dumping_complete = True self.arm_postdump_state() @@ -264,16 +243,14 @@ def run(self): elif(self.autostate == "F_OBSTACLE_FIELD"): if(self.pose.position.y >= Y_IN_MINING_AREA): self.autostate = "MINING_BEHAVIOR" - self.curPathIndex = len(self.pathPoses) - 1 elif(self.autostate == "MINING_BEHAVIOR"): if( not self.autonomyEnabled or (self.pose.position.y < Y_IN_MINING_AREA - .3) ): self.autostate = "B_OBSTACLE_FIELD" - self.minePath = None + self.miningTarget = None self.miningDone = False elif(self.autostate == "B_OBSTACLE_FIELD"): if(self.pose.position.y <= Y_IN_DOCKING_AREA): self.autostate = "DOCKING" - self.curPathIndex = 0 elif(self.autostate == "DOCKING"): if(self.pose.position.y < Y_IN_DUMP_RANGE): self.autostate = "DUMPING" diff --git a/cr17/param/base_local_planner_params.yaml b/cr17/param/base_local_planner_params.yaml index ac9b8c6..fcd6c67 100644 --- a/cr17/param/base_local_planner_params.yaml +++ b/cr17/param/base_local_planner_params.yaml @@ -1,8 +1,8 @@ TrajectoryPlannerROS: - max_vel_x: 0.45 + max_vel_x: 4 min_vel_x: 0.1 max_vel_theta: 1.0 - min_in_place_vel_theta: 0.4 + min_in_place_vel_theta: 0.2 acc_lim_theta: 3.2 acc_lim_x: 2.5 diff --git a/simulation_2017/launch/control_autonomous.launch b/simulation_2017/launch/control_autonomous.launch index ff972ad..7ae4a0e 100644 --- a/simulation_2017/launch/control_autonomous.launch +++ b/simulation_2017/launch/control_autonomous.launch @@ -28,8 +28,6 @@ - - From c4ab30676cc92163c7de784acc26efb2800af9e8 Mon Sep 17 00:00:00 2001 From: billy Date: Mon, 3 Apr 2017 14:46:24 -0500 Subject: [PATCH 6/7] Added a docker node that will dock the robot, the navstack needs to learn to go in reverse --- cr17/CMakeLists.txt | 2 + cr17/msg/dockerStatus.msg | 2 + cr17/nodes/cr17/docker.py | 83 +++++++++++++++++++ .../nodes/cr17/high_level_state_controller.py | 37 +++++---- cr17/param/costmap_common_params.yaml | 4 +- cr17/param/docker.yaml | 7 ++ cr17/param/topics.yaml | 1 + cr17/srv/dockerState.srv | 3 + .../launch/control_autonomous.launch | 6 ++ simulation_2017/param/sim_scoop_config.yaml | 2 +- 10 files changed, 130 insertions(+), 17 deletions(-) create mode 100644 cr17/msg/dockerStatus.msg create mode 100755 cr17/nodes/cr17/docker.py create mode 100644 cr17/param/docker.yaml create mode 100644 cr17/srv/dockerState.srv diff --git a/cr17/CMakeLists.txt b/cr17/CMakeLists.txt index 4c6312c..533d73d 100644 --- a/cr17/CMakeLists.txt +++ b/cr17/CMakeLists.txt @@ -54,12 +54,14 @@ catkin_python_setup() localizationPoints.msg wheelData.msg scoopControl.msg + dockerStatus.msg ) ## Generate services in the 'srv' folder add_service_files( FILES autonomousActive.srv + dockerState.srv ) ## Generate actions in the 'action' folder diff --git a/cr17/msg/dockerStatus.msg b/cr17/msg/dockerStatus.msg new file mode 100644 index 0000000..766d9b2 --- /dev/null +++ b/cr17/msg/dockerStatus.msg @@ -0,0 +1,2 @@ +bool dockerActive +bool dockingComplete \ No newline at end of file diff --git a/cr17/nodes/cr17/docker.py b/cr17/nodes/cr17/docker.py new file mode 100755 index 0000000..276e757 --- /dev/null +++ b/cr17/nodes/cr17/docker.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python2 +import rospy +from cr17.msg import dockerStatus +from cr17.srv import dockerState, dockerStateResponse +from geometry_msgs.msg import PoseStamped, Twist, Pose +from tf.transformations import euler_from_quaternion, quaternion_from_euler + + +POSE_TOPIC = rospy.get_param("topics/localization_pose") +DOCKER_TOPIC = rospy.get_param("topics/docker_status") +DRIVE_TOPIC = rospy.get_param("topics/drive_cmds") + +Y_TARGET = rospy.get_param("docker/y_target") +Y_RANGE = rospy.get_param("docker/y_range") +Y_SCALAR = rospy.get_param("docker/y_scalar") +YAW_TARGET = rospy.get_param("docker/yaw_target") +YAW_RANGE = rospy.get_param("docker/yaw_range") +YAW_SCALAR = rospy.get_param("docker/yaw_scalar") + + +class Docker(object): + def __init__(self): + rospy.init_node('docker') + + self.dockerActive = False + self.pose = Pose() + + #ROS Publishers + self.drivePub = rospy.Publisher(DRIVE_TOPIC, Twist, queue_size = 10) + self.dockerStatusPub = rospy.Publisher(DOCKER_TOPIC, dockerStatus, queue_size = 10) + + #ROS Subscribers + rospy.Subscriber(POSE_TOPIC, PoseStamped, self.pose_sub) + + # ROS Services + rospy.Service('/dockerState', dockerState, self.set_docker_state) + + def pose_sub(self, data): + self.pose = data.pose + + def set_docker_state(self, data): + self.dockerActive = data.newDockerState + return dockerStateResponse(self.dockerActive) + + def run(self): + rate = rospy.Rate(10) + + while not rospy.is_shutdown(): + currentStatus = dockerStatus() + currentStatus.dockerActive = self.dockerActive + + if self.dockerActive: + currentY = self.pose.position.y + currentOrientation = euler_from_quaternion([self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w]) + currentYaw = currentOrientation[2] + + driveTwist = Twist() + driveTwist.linear.x = (currentY - Y_TARGET) * Y_SCALAR + driveTwist.angular.z = (currentYaw - YAW_TARGET) * YAW_SCALAR + + rospy.logwarn(currentYaw) + + currentStatus.dockingComplete = False + + if abs(currentY - Y_TARGET) < Y_RANGE: + if abs(currentYaw - YAW_TARGET) < YAW_RANGE: + currentStatus.dockingComplete = True + driveTwist.linear.x = 0 + driveTwist.angular.z = 0 + + self.drivePub.publish(driveTwist) + + else: + currentStatus.dockingComplete = False + + self.dockerStatusPub.publish(currentStatus) + rate.sleep() + + + +if __name__ == "__main__": + docker = Docker() + docker.run() diff --git a/cr17/nodes/cr17/high_level_state_controller.py b/cr17/nodes/cr17/high_level_state_controller.py index 25ba730..77c51a5 100755 --- a/cr17/nodes/cr17/high_level_state_controller.py +++ b/cr17/nodes/cr17/high_level_state_controller.py @@ -5,8 +5,8 @@ from geometry_msgs.msg import PoseStamped, Twist, Pose from std_msgs.msg import Float64, Bool, String from nav_msgs.msg import Path -from cr17.msg import scoopControl -from cr17.srv import autonomousActive, autonomousActiveResponse +from cr17.msg import scoopControl, dockerStatus +from cr17.srv import autonomousActive, autonomousActiveResponse, dockerState ARM_STATE_TOPIC = rospy.get_param("topics/scoop_state_cmds", "scoop_commands") @@ -14,6 +14,7 @@ PATH_TOPIC = rospy.get_param("topics/path", "/obstacle_path") GOAL_TOPIC = rospy.get_param("topics/navigation_goals", "nav_goal") #TODO: might need to be renamed STATE_TOPIC = rospy.get_param("topics/robot_state", "state") +DOCKER_TOPIC = rospy.get_param("topics/docker_status") MINING_DISTANCE = rospy.get_param("distance_to_mine", 1.5) Y_IN_DUMP_RANGE = 0.4 @@ -52,7 +53,7 @@ def __init__(self): self.frontMiningGoal = PoseStamped() self.frontMiningGoal.header.frame_id = "map" self.frontMiningGoal.pose.position.x = 0 - self.frontMiningGoal.pose.position.y = Y_IN_MINING_AREA+0.4 + self.frontMiningGoal.pose.position.y = Y_IN_MINING_AREA + 0.4 self.frontMiningGoal.pose.orientation.x = 0 self.frontMiningGoal.pose.orientation.y = 0 self.frontMiningGoal.pose.orientation.z = -0.7071067811865476 @@ -61,18 +62,21 @@ def __init__(self): self.dumpSetupGoal = PoseStamped() self.dumpSetupGoal.header.frame_id = "map" self.dumpSetupGoal.pose.position.x = 0 - self.dumpSetupGoal.pose.position.y = 1.1 + self.dumpSetupGoal.pose.position.y = Y_IN_DOCKING_AREA - 0.1 self.dumpSetupGoal.pose.orientation.x = 0 self.dumpSetupGoal.pose.orientation.y = 0 self.dumpSetupGoal.pose.orientation.z = -0.7071067811865476 self.dumpSetupGoal.pose.orientation.w = 0.7071067811865476 + self.dockerStatus = dockerStatus() + # ROS Publishers self.arm_pub = rospy.Publisher(ARM_STATE_TOPIC, scoopControl, queue_size=10) self.goal_pub = rospy.Publisher(GOAL_TOPIC, PoseStamped, queue_size=10) #TODO: change to a Path #ROS Subscribers rospy.Subscriber(POSE_TOPIC, PoseStamped, self.pose_sub) + rospy.Subscriber(DOCKER_TOPIC, dockerStatus, self.docker_sub) # ROS Services self.auto_srv = rospy.Service('autonomousHLSC', autonomousActive, self.set_autonomy) @@ -84,6 +88,9 @@ def set_autonomy(self, data): def pose_sub(self, data): self.pose = data.pose + def docker_sub(self, data): + self.dockerStatus = data + def arm_drive_state(self): newMsg = scoopControl() newMsg.desiredState = scoopControl.state_drivingPosition @@ -211,17 +218,19 @@ def run(self): elif(self.autostate == "DOCKING"): self.arm_predump_state() - dockPose = PoseStamped() - dockPose.header.frame_id = "map" - dockPose.pose.position.x = X_POS_DUMP - dockPose.pose.position.y = Y_POS_DUMP - dockPose.pose.orientation.x = 0 - dockPose.pose.orientation.y = 0 - dockPose.pose.orientation.z = -0.7071067811865476 - dockPose.pose.orientation.w = 0.7071067811865476 - self.goal_pub.publish(dockPose) + try: + dockerService = rospy.ServiceProxy("/dockerState", dockerState) + dockerService(True) + except rospy.ServiceException, e: + rospy.logerr("/dockerState service call failed: %s",e) elif(self.autostate == "DUMPING"): + if self.dockerStatus.dockerActive: + try: + dockerService = rospy.ServiceProxy("/dockerState", dockerState) + dockerService(False) + except rospy.ServiceException, e: + rospy.logerr("/dockerState service call failed: %s",e) dumping_complete = False if(self.dumpTimer == None): self.dumpTimer = rospy.Time.now() + rospy.Duration(secs = TIME_DUMP_SEC) @@ -252,7 +261,7 @@ def run(self): if(self.pose.position.y <= Y_IN_DOCKING_AREA): self.autostate = "DOCKING" elif(self.autostate == "DOCKING"): - if(self.pose.position.y < Y_IN_DUMP_RANGE): + if (self.pose.position.y < Y_IN_DUMP_RANGE) or self.dockerStatus.dockingComplete: self.autostate = "DUMPING" elif(self.autostate == "DUMPING"): if(dumping_complete or (self.pose.position.y >= Y_IN_DUMP_RANGE)): diff --git a/cr17/param/costmap_common_params.yaml b/cr17/param/costmap_common_params.yaml index fb366ca..f106a05 100644 --- a/cr17/param/costmap_common_params.yaml +++ b/cr17/param/costmap_common_params.yaml @@ -5,5 +5,5 @@ inflation_radius: 0.55 observation_sources: front_kinect rear_kinect -front_kinect: {max_obstacle_height: 0.35, min_obstacle_height: 0.06, data_type: PointCloud2, topic: /front_camera/depth/points, marking: true, clearing: true} -rear_kinect: {max_obstacle_height: 0.35, min_obstacle_height: 0.06, data_type: PointCloud2, topic: /rear_camera/depth/points, marking: true, clearing: true} +front_kinect: {max_obstacle_height: 0.31, min_obstacle_height: 0.06, data_type: PointCloud2, topic: /front_camera/depth/points, marking: true, clearing: true} +rear_kinect: {max_obstacle_height: 0.31, min_obstacle_height: 0.06, data_type: PointCloud2, topic: /rear_camera/depth/points, marking: true, clearing: true} diff --git a/cr17/param/docker.yaml b/cr17/param/docker.yaml new file mode 100644 index 0000000..2c77e83 --- /dev/null +++ b/cr17/param/docker.yaml @@ -0,0 +1,7 @@ +docker: + y_target: 0.7 + y_range: 0.08 + y_scalar: 1.5 + yaw_target: -1.57079632679 + yaw_range: 0.2 + yaw_scalar: -1.5 \ No newline at end of file diff --git a/cr17/param/topics.yaml b/cr17/param/topics.yaml index e8595cf..cf29afb 100644 --- a/cr17/param/topics.yaml +++ b/cr17/param/topics.yaml @@ -12,6 +12,7 @@ topics: filtered_pose: filtered_pose kinect_beacon_points: kinect_beacon_points aruco_pose: "/ar_multi_board/transform" + docker_status: "/docker_status" # op_mode: operation_mode # ROS String msgs, operation mode for robot diff --git a/cr17/srv/dockerState.srv b/cr17/srv/dockerState.srv new file mode 100644 index 0000000..abd01d8 --- /dev/null +++ b/cr17/srv/dockerState.srv @@ -0,0 +1,3 @@ +bool newDockerState +--- +bool setDockerState diff --git a/simulation_2017/launch/control_autonomous.launch b/simulation_2017/launch/control_autonomous.launch index 7ae4a0e..a9f0476 100644 --- a/simulation_2017/launch/control_autonomous.launch +++ b/simulation_2017/launch/control_autonomous.launch @@ -15,12 +15,18 @@ + + + + + + diff --git a/simulation_2017/param/sim_scoop_config.yaml b/simulation_2017/param/sim_scoop_config.yaml index 8526c85..bd2cfaf 100644 --- a/simulation_2017/param/sim_scoop_config.yaml +++ b/simulation_2017/param/sim_scoop_config.yaml @@ -5,7 +5,7 @@ scoop_config: pre_dig_scoop_angle: 100 digging_arm_angle: 70 digging_scoop_angle: 100 - post_dig_arm_angle: 50 + post_dig_arm_angle: 0 post_dig_scoop_angle: -30 pre_dump_arm_angle: -45 pre_dump_scoop_angle: -30 From 6721f614dfd71e24043dd8fa7ae2d38014d62bfe Mon Sep 17 00:00:00 2001 From: bgrayd Date: Tue, 11 Apr 2017 23:42:13 -0500 Subject: [PATCH 7/7] got the teb local planner and the docker working --- cr17/launch/move_base.launch | 2 + cr17/nodes/cr17/docker.py | 14 +++- .../nodes/cr17/high_level_state_controller.py | 22 +++--- cr17/param/base_local_planner_params.yaml | 78 ++++++++++++++++++- cr17/param/costmap_common_params.yaml | 2 +- 5 files changed, 105 insertions(+), 13 deletions(-) diff --git a/cr17/launch/move_base.launch b/cr17/launch/move_base.launch index c383fdd..3a31ecd 100644 --- a/cr17/launch/move_base.launch +++ b/cr17/launch/move_base.launch @@ -13,5 +13,7 @@ + + \ No newline at end of file diff --git a/cr17/nodes/cr17/docker.py b/cr17/nodes/cr17/docker.py index 276e757..7029bcc 100755 --- a/cr17/nodes/cr17/docker.py +++ b/cr17/nodes/cr17/docker.py @@ -25,6 +25,8 @@ def __init__(self): self.dockerActive = False self.pose = Pose() + self.dockingReverse = False + #ROS Publishers self.drivePub = rospy.Publisher(DRIVE_TOPIC, Twist, queue_size = 10) self.dockerStatusPub = rospy.Publisher(DOCKER_TOPIC, dockerStatus, queue_size = 10) @@ -39,6 +41,8 @@ def pose_sub(self, data): self.pose = data.pose def set_docker_state(self, data): + if (self.dockerActive != data.newDockerState) and (data.newDockerState): + self.dockingReverse = True self.dockerActive = data.newDockerState return dockerStateResponse(self.dockerActive) @@ -58,8 +62,6 @@ def run(self): driveTwist.linear.x = (currentY - Y_TARGET) * Y_SCALAR driveTwist.angular.z = (currentYaw - YAW_TARGET) * YAW_SCALAR - rospy.logwarn(currentYaw) - currentStatus.dockingComplete = False if abs(currentY - Y_TARGET) < Y_RANGE: @@ -70,6 +72,14 @@ def run(self): self.drivePub.publish(driveTwist) + elif self.dockingReverse: + driveTwist = Twist() + driveTwist.linear.x = -20 + for each in range(0, 30): + self.drivePub.publish(driveTwist) + rate.sleep() + self.dockingReverse = False + else: currentStatus.dockingComplete = False diff --git a/cr17/nodes/cr17/high_level_state_controller.py b/cr17/nodes/cr17/high_level_state_controller.py index 77c51a5..48ebee8 100755 --- a/cr17/nodes/cr17/high_level_state_controller.py +++ b/cr17/nodes/cr17/high_level_state_controller.py @@ -19,7 +19,7 @@ Y_IN_DUMP_RANGE = 0.4 Y_IN_MINING_AREA = 4.5 #10 cm past edge of mining area -Y_IN_DOCKING_AREA = 1.5 +Y_IN_DOCKING_AREA = 1.7 TIME_DUMP_SEC = 10 @@ -62,7 +62,7 @@ def __init__(self): self.dumpSetupGoal = PoseStamped() self.dumpSetupGoal.header.frame_id = "map" self.dumpSetupGoal.pose.position.x = 0 - self.dumpSetupGoal.pose.position.y = Y_IN_DOCKING_AREA - 0.1 + self.dumpSetupGoal.pose.position.y = Y_IN_DOCKING_AREA - 0.2 self.dumpSetupGoal.pose.orientation.x = 0 self.dumpSetupGoal.pose.orientation.y = 0 self.dumpSetupGoal.pose.orientation.z = -0.7071067811865476 @@ -225,12 +225,6 @@ def run(self): rospy.logerr("/dockerState service call failed: %s",e) elif(self.autostate == "DUMPING"): - if self.dockerStatus.dockerActive: - try: - dockerService = rospy.ServiceProxy("/dockerState", dockerState) - dockerService(False) - except rospy.ServiceException, e: - rospy.logerr("/dockerState service call failed: %s",e) dumping_complete = False if(self.dumpTimer == None): self.dumpTimer = rospy.Time.now() + rospy.Duration(secs = TIME_DUMP_SEC) @@ -240,6 +234,14 @@ def run(self): else: self.arm_dump_state() + elif(self.autostate == "POST_DUMP"): + if self.dockerStatus.dockerActive: + try: + dockerService = rospy.ServiceProxy("/dockerState", dockerState) + dockerService(False) + except rospy.ServiceException, e: + rospy.logerr("/dockerState service call failed: %s",e) + ###################################################################################################### #TODO: add code to turn the robot around ###################################################################################################### @@ -265,8 +267,10 @@ def run(self): self.autostate = "DUMPING" elif(self.autostate == "DUMPING"): if(dumping_complete or (self.pose.position.y >= Y_IN_DUMP_RANGE)): - self.autostate = "F_OBSTACLE_FIELD" + self.autostate = "POST_DUMP" self.dumpTimer = None + elif(self.autostate == "POST_DUMP"): + self.autostate = "F_OBSTACLE_FIELD" else: self.autostate = "INIT" diff --git a/cr17/param/base_local_planner_params.yaml b/cr17/param/base_local_planner_params.yaml index fcd6c67..a8c2004 100644 --- a/cr17/param/base_local_planner_params.yaml +++ b/cr17/param/base_local_planner_params.yaml @@ -8,4 +8,80 @@ TrajectoryPlannerROS: acc_lim_x: 2.5 acc_lim_y: 0 - holonomic_robot: false \ No newline at end of file + holonomic_robot: false + + +TebLocalPlannerROS: + + odom_topic: odom + map_frame: /odom + + # Trajectory + + teb_autosize: True + dt_ref: 0.3 + dt_hysteresis: 0.1 + global_plan_overwrite_orientation: True + max_global_plan_lookahead_dist: 3.0 + feasibility_check_no_poses: 5 + + # Robot + + max_vel_x: 4 + max_vel_x_backwards: 4 + max_vel_theta: 1 + acc_lim_x: 1.0 + acc_lim_theta: 0.9 + min_turning_radius: 0.1 + footprint_model: + type: "polygon" + vertices: [[1.18, 0.3301], [-0.282575, 0.3301], [-0.282575, -0.3301], [1.18,-0.3301]] + + # GoalTolerance + + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.4 + free_goal_vel: False + + # Obstacles + + min_obstacle_dist: 0.3 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.0 + obstacle_poses_affected: 30 + costmap_converter_plugin: "" + costmap_converter_spin_thread: True + costmap_converter_rate: 5 + + # Optimization + + no_inner_iterations: 4 + no_outer_iterations: 3 + optimization_activate: True + optimization_verbose: False + penalty_epsilon: 0.1 + weight_max_vel_x: 2 + weight_max_vel_theta: 1 + weight_acc_lim_x: 1 + weight_acc_lim_theta: 1 + weight_kinematics_nh: 1000 + weight_kinematics_forward_drive: 1 + weight_kinematics_turning_radius: 0.9 + weight_optimaltime: 1 + weight_obstacle: 50 + weight_dynamic_obstacle: 10 # not in use yet + selection_alternative_time_cost: False # not in use yet + + # Homotopy Class Planner + + enable_homotopy_class_planning: True + enable_multithreading: True + simple_exploration: False + max_number_classes: 4 + roadmap_graph_no_samples: 15 + roadmap_graph_area_width: 5 + h_signature_prescaler: 0.5 + h_signature_threshold: 0.1 + obstacle_keypoint_offset: 0.1 + obstacle_heading_threshold: 0.45 + visualize_hc_graph: False \ No newline at end of file diff --git a/cr17/param/costmap_common_params.yaml b/cr17/param/costmap_common_params.yaml index f106a05..63cbc53 100644 --- a/cr17/param/costmap_common_params.yaml +++ b/cr17/param/costmap_common_params.yaml @@ -1,7 +1,7 @@ obstacle_range: 3.0 raytrace_range: 3.5 footprint: [[1.18, 0.3301], [-0.282575, 0.3301], [-0.282575, -0.3301], [1.18,-0.3301]] #back is 0.282575 backedge from base_link center 1.18m forward sides are 0.3301 -inflation_radius: 0.55 +inflation_radius: 0.3 observation_sources: front_kinect rear_kinect