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/launch/move_base.launch b/cr17/launch/move_base.launch
new file mode 100644
index 0000000..3a31ecd
--- /dev/null
+++ b/cr17/launch/move_base.launch
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
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..7029bcc
--- /dev/null
+++ b/cr17/nodes/cr17/docker.py
@@ -0,0 +1,93 @@
+#!/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()
+
+ 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)
+
+ #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):
+ if (self.dockerActive != data.newDockerState) and (data.newDockerState):
+ self.dockingReverse = True
+ 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
+
+ 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)
+
+ 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
+
+ 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 2e5c9d5..48ebee8 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,11 +14,12 @@
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.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
+Y_IN_DOCKING_AREA = 1.7
TIME_DUMP_SEC = 10
@@ -40,25 +41,42 @@ 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 = 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
+ 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(PATH_TOPIC, Path, self.path_sub)
+ rospy.Subscriber(DOCKER_TOPIC, dockerStatus, self.docker_sub)
# ROS Services
self.auto_srv = rospy.Service('autonomousHLSC', autonomousActive, self.set_autonomy)
@@ -70,8 +88,8 @@ def set_autonomy(self, data):
def pose_sub(self, data):
self.pose = data.pose
- def path_sub(self, data):
- self.pathPoses = data.poses
+ def docker_sub(self, data):
+ self.dockerStatus = data
def arm_drive_state(self):
newMsg = scoopControl()
@@ -108,10 +126,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.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
+ def calculate_mining_target(self, angle):
start_x = self.pose.position.x
start_y = self.pose.position.y
@@ -123,14 +138,17 @@ 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"
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.7071067811865476
+ endPose.pose.orientation.w = -0.7071067811865476
- path.append(startPose)
- path.append(endPose)
- return path
+ return endPose
def close_to(self, poseChecked):
try:
@@ -152,17 +170,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"):
@@ -176,31 +189,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
@@ -209,30 +213,35 @@ 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()
- dockPose = PoseStamped()
- dockPose.pose.position.x = X_POS_DUMP
- dockPose.pose.position.y = Y_POS_DUMP
- 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"):
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()
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
######################################################################################################
@@ -245,23 +254,23 @@ 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):
+ 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)):
- 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/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/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..a8c2004
--- /dev/null
+++ b/cr17/param/base_local_planner_params.yaml
@@ -0,0 +1,87 @@
+TrajectoryPlannerROS:
+ max_vel_x: 4
+ min_vel_x: 0.1
+ max_vel_theta: 1.0
+ min_in_place_vel_theta: 0.2
+
+ acc_lim_theta: 3.2
+ acc_lim_x: 2.5
+ acc_lim_y: 0
+
+ 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
new file mode 100644
index 0000000..63cbc53
--- /dev/null
+++ b/cr17/param/costmap_common_params.yaml
@@ -0,0 +1,9 @@
+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.3
+
+observation_sources: front_kinect rear_kinect
+
+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/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..903953c
--- /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: 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/cr17/param/rmc_field_map.pgm b/cr17/param/rmc_field_map.pgm
new file mode 100644
index 0000000..48a1bc4
Binary files /dev/null and b/cr17/param/rmc_field_map.pgm differ
diff --git a/cr17/param/rmc_field_map.yaml b/cr17/param/rmc_field_map.yaml
new file mode 100644
index 0000000..18e576a
--- /dev/null
+++ b/cr17/param/rmc_field_map.yaml
@@ -0,0 +1,6 @@
+image: rmc_field_map.pgm
+resolution: 0.05
+origin: [-2.09, -0.2, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
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/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..a9f0476 100644
--- a/simulation_2017/launch/control_autonomous.launch
+++ b/simulation_2017/launch/control_autonomous.launch
@@ -15,8 +15,14 @@
+
+
+
+
+
+
@@ -28,8 +34,8 @@
-
-
+
+
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/param/sim_scoop_config.yaml b/simulation_2017/param/sim_scoop_config.yaml
index 61dc928..bd2cfaf 100644
--- a/simulation_2017/param/sim_scoop_config.yaml
+++ b/simulation_2017/param/sim_scoop_config.yaml
@@ -1,11 +1,11 @@
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
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
diff --git a/simulation_2017/urdf/cr17.xacro b/simulation_2017/urdf/cr17.xacro
index 64b85cd..b679e6a 100644
--- a/simulation_2017/urdf/cr17.xacro
+++ b/simulation_2017/urdf/cr17.xacro
@@ -17,12 +17,15 @@
+
+
+
@@ -61,6 +64,11 @@
+
+
+
+
+
@@ -621,6 +629,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -630,9 +652,13 @@
-
+
+
+
+
+
-