def goToMouth(robotConfig=None, binNum=0, isExecute=True, withPause=False):
## robotConfig: current time robot configuration
joint_topic = '/joint_states'
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
# plan store
plans = []
## initial variable and tcp definitions
# set tcp
l2 = 0.47
tip_hand_transform = [
0, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
pubFrame(br,
pose=tip_hand_transform,
frame_id='tip',
parent_frame_id='link_6',
npub=5)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
# set home orientation
gripperOri = [0, 0.7071, 0, 0.7071]
# move to bin mouth
distFromShelf = 0.15
mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
targetPosition = [
mouthPt.pose.position.x, mouthPt.pose.position.y,
mouthPt.pose.position.z
]
q_initial = robotConfig
planner = IK(q0=q_initial,
target_tip_pos=targetPosition,
target_tip_ori=gripperOri,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
s = plan.success()
if s:
print '[goToMouth] move to bin mouth successful'
plan.visualize()
plans.append(plan)
if isExecute:
pauseFunc(withPause)
plan.execute()
else:
print '[goToMouth] move to bin mouth fail'
return None
qf = plan.q_traj[-1]
## open gripper fully
openGripper()
return plan
def __init__(self,
tf_listener=None,
advertise_service=False,
advertise_action=True):
#init a TF transform listener
if tf_listener == None:
self.tf_listener = tf.TransformListener()
else:
self.tf_listener = tf_listener
#point cloud processing action
self._container_client = actionlib.SimpleActionClient(
'find_container_action', FindContainerAction)
rospy.loginfo("Waiting for find_container_action...")
self._container_client.wait_for_server()
#cluster grasp planning service
service_name = "plan_point_cluster_grasp"
rospy.loginfo("waiting for plan_point_cluster_grasp service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
self._cluster_grasp_srv = rospy.ServiceProxy(service_name,
GraspPlanning)
#cluster grasp planning params
service_name = "set_point_cluster_grasp_params"
rospy.loginfo("waiting for set_point_cluster_grasp_params service")
rospy.wait_for_service(service_name)
rospy.loginfo("service found")
self._cluster_grasp_params_srv = rospy.ServiceProxy(
service_name, SetPointClusterGraspParams)
#planning scene setting service
planning_scene_srv_name = "environment_server/set_planning_scene_diff"
rospy.loginfo("waiting for %s service" % planning_scene_srv_name)
rospy.wait_for_service(planning_scene_srv_name)
self._planning_scene_srv = rospy.ServiceProxy(planning_scene_srv_name,
SetPlanningSceneDiff)
#what approximate directions should the grasps be pointing into (as with a container)?
self._opening_dir_list = rospy.get_param("~opening_dir_list",
[[0, 0, 1], [-1, 0, 0]])
#inserted for HRI studies--if the task number is set, change the opening_dir_list
task_number = rospy.get_param("interactive_grasping/task_number", 0)
if task_number == 3:
rospy.loginfo("Task number is 3, doing front grasps only.")
self._opening_dir_list = [[-1, 0, 0]]
elif task_number > 0:
rospy.loginfo("Task number is 1 or 2, doing overhead grasps only.")
self._opening_dir_list = [[0, 0, 1]]
#whether to make the pregrasp just outside the cluster bounding box, or let it be the default (10 cm)
self._pregrasp_just_outside_box = rospy.get_param(
"~pregrasp_just_outside_box", False)
#name of the cluster planner service topic
self._cluster_planner_name = rospy.get_param(
"cluster_planner_name", "point_cluster_grasp_planner")
#advertise the grasp planning service
if advertise_service:
rospy.Service('plan_segmented_clutter_grasps', GraspPlanning,
self.plan_segmented_clutter_grasps_callback)
rospy.loginfo(
"segmented clutter grasp planner is ready for queries")
#advertise the grasp planning action
if advertise_action:
self._as = actionlib.SimpleActionServer(
rospy.get_name(),
GraspPlanningAction,
execute_cb=self.segmented_clutter_grasps_execute_cb)
self._as.start()
rospy.loginfo(rospy.get_name() + " is ready for queries")
#!/usr/bin/env python
import roslib
roslib.load_manifest('project1_ws')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('tf_listener2')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(3, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if not (math.sqrt(trans[0] ** 2 + trans[1] ** 2))<0.1:
angular = 0.7 * math.atan2(trans[1], trans[0])
linear = 0.1 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
def node():
global mapData
rospy.Subscriber("/robot_1/map", OccupancyGrid, mapCallBack)
pub = rospy.Publisher('shapes', Marker, queue_size=10)
rospy.init_node('RRTexplorer', anonymous=False)
#Actionlib client
client1 = actionlib.SimpleActionClient('/robot_1/move_base', MoveBaseAction)
client1.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.stamp=rospy.Time.now()
goal.target_pose.header.frame_id="/robot_1/map"
rate = rospy.Rate(100)
listener = tf.TransformListener()
listener.waitForTransform('/robot_1/map', '/robot_1/base_link', rospy.Time(0),rospy.Duration(10.0))
try:
(trans,rot) = listener.lookupTransform('/robot_1/map', '/robot_1/base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
trans=[0,0]
xinx=trans[0]
xiny=trans[1]
goal.target_pose.pose.position.x=xinx-0.5
goal.target_pose.pose.position.y=0
goal.target_pose.pose.position.z=0
goal.target_pose.pose.orientation.w = 1.0
client1.send_goal(goal)
client1.wait_for_result()
client1.get_result()
x_init=array([xinx,xiny])
V=array([x_init])
i=1.0
E=concatenate((x_init,x_init))
points=Marker()
line=Marker()
#Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id=line.header.frame_id="/robot_1/map"
points.header.stamp=line.header.stamp=rospy.Time.now()
points.ns=line.ns = "markers"
points.id = 0
line.id =1
points.type = Marker.POINTS
line.type=Marker.LINE_LIST
#Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = line.action = Marker.ADD;
points.pose.orientation.w = line.pose.orientation.w = 1.0;
line.scale.x = 0.02;
points.scale.x=0.2;
line.scale.y= 0.02;
points.scale.y=0.2;
line.color.r =9.0/255.0
line.color.g= 91.0/255.0
line.color.b =236.0/255.0
points.color.r = 255.0/255.0
points.color.g = 0.0/255.0
points.color.b = 0.0/255.0
points.color.a=1;
line.color.a = 0.6;
points.lifetime =line.lifetime = rospy.Duration();
p=Point()
p.x = x_init[0] ;
p.y = x_init[0] ;
p.z = 0;
pp=[]
pl=[]
pp.append(copy(p))
frontiers=[]
xdim=mapData.info.width
ydim=mapData.info.height
resolution=mapData.info.resolution
Xstartx=mapData.info.origin.position.x
Xstarty=mapData.info.origin.position.y
#-------------------------------RRT------------------------------------------
while not rospy.is_shutdown():
# Sample free
xr=(random()*20.0)-10.0
yr=(random()*20.0)-10.0
x_rand = array([xr,yr])
# Nearest
x_nearest=V[Nearest(V,x_rand),:]
# Steer
x_new=Steer(x_nearest,x_rand,param.eta)
# ObstacleFree
checking=ObstacleFree2(x_nearest,x_new,mapData)
if checking==-1:
if len(frontiers)>0:
frontiers=vstack((frontiers,x_new))
else:
frontiers=[x_new]
(trans,rot) = listener.lookupTransform('/robot_1/map', '/robot_1/base_link', rospy.Time(0))
xinx=trans[0]
xiny=trans[1]
x_init=array([xinx,xiny])
V=array([x_init])
E=concatenate((x_init,x_init))
pp=[]
pl=[]
elif checking==1:
V=vstack((V,x_new))
temp=concatenate((x_nearest,x_new))
E=vstack((E,temp))
z=0
while z<len(frontiers):
if gridValue(mapData,frontiers[z])!=-1:
frontiers=delete(frontiers, (z), axis=0)
z=z-1
z+=1
frontiers=assigner1rrtfront(goal,frontiers,client1,listener)
print rospy.Time.now()," ",len(frontiers)
pp=[]
for q in range(0,len(frontiers)):
p.x=frontiers[q][0]
p.y=frontiers[q][1]
pp.append(copy(p))
#Plotting
points.points=pp
pl=prepEdges(E)
line.points=pl
pub.publish(line)
pub.publish(points)
rate.sleep()
def __init__(self):
"""Initialize this _AutoRallyCtrlr."""
rospy.init_node("autoRally_controller")
# Parameters
# Wheels
(left_steer_link_name, left_steer_ctrlr_name,
left_front_axle_ctrlr_name, self._left_front_inv_circ) = \
self._get_front_wheel_params("left")
(right_steer_link_name, right_steer_ctrlr_name,
right_front_axle_ctrlr_name, self._right_front_inv_circ) = \
self._get_front_wheel_params("right")
(left_rear_link_name, left_rear_axle_ctrlr_name,
self._left_rear_inv_circ) = \
self._get_rear_wheel_params("left")
(self._right_rear_link_name, right_rear_axle_ctrlr_name,
self._right_rear_inv_circ) = \
self._get_rear_wheel_params("right")
list_ctrlrs = rospy.ServiceProxy("controller_manager/list_controllers",
ListControllers)
list_ctrlrs.wait_for_service()
# Shock absorbers
shock_param_list = rospy.get_param("~shock_absorbers", [])
self._shock_pubs = []
try:
for shock_params in shock_param_list:
try:
ctrlr_name = shock_params["controller_name"]
try:
eq_pos = shock_params["equilibrium_position"]
except:
eq_pos = self._DEF_EQ_POS
eq_pos = float(eq_pos)
except:
rospy.logwarn("An invalid parameter was specified for a "
"shock absorber. The shock absorber will "
"not be used.")
continue
pub = rospy.Publisher(ctrlr_name + "/command",
Float64,
latch=True,
queue_size=1)
_wait_for_ctrlr(list_ctrlrs, ctrlr_name)
pub.publish(eq_pos)
self._shock_pubs.append(pub)
except:
rospy.logwarn("The specified list of shock absorbers is invalid. "
"No shock absorbers will be used.")
# Command timeout
try:
self._cmd_timeout = float(
rospy.get_param("~cmd_timeout", self._DEF_CMD_TIMEOUT))
except:
rospy.logwarn("The specified command timeout value is invalid. "
"The default timeout value will be used instead.")
self._cmd_timeout = self._DEF_CMD_TIMEOUT
# Publishing frequency
try:
pub_freq = float(
rospy.get_param("~publishing_frequency", self._DEF_PUB_FREQ))
if pub_freq <= 0.0:
raise ValueError()
except:
rospy.logwarn("The specified publishing frequency is invalid. "
"The default frequency will be used instead.")
pub_freq = self._DEF_PUB_FREQ
self._sleep_timer = rospy.Rate(pub_freq)
# _last_cmd_time is the time at which the most recent Ackermann
# driving command was received.
self._last_cmd_time = rospy.get_time()
# _ackermann_cmd_lock is used to control access to _steer_ang,
# _steer_ang_vel, _speed, and _accel.
# self._ackermann_cmd_lock = threading.Lock()
# self._steer_ang = 0.0 # Steering angle
# self._steer_ang_vel = 0.0 # Steering angle velocity
# self._speed = 0.0
# self._accel = 0.0 # Acceleration
self.lastCmdTime = rospy.get_time()
self.servoCmds = dict()
self.servoCmdLock = threading.Lock()
self._last_steer_ang = 0.0 # Last steering angle
self._theta_left = 0.0 # Left steering joint angle
self._theta_right = 0.0 # Right steering joint angle
self._last_speed = 0.0
self._last_accel_limit = 0.0 # Last acceleration limit
# Axle angular velocities
self._left_front_ang_vel = 0.0
self._right_front_ang_vel = 0.0
self._left_rear_ang_vel = 0.0
self._right_rear_ang_vel = 0.0
# _joint_dist_div_2 is the distance between the steering joints,
# divided by two.
tfl = tf.TransformListener()
ls_pos = self._get_link_pos(tfl, left_steer_link_name)
rs_pos = self._get_link_pos(tfl, right_steer_link_name)
self._joint_dist_div_2 = numpy.linalg.norm(ls_pos - rs_pos) / 2
lrw_pos = self._get_link_pos(tfl, left_rear_link_name)
rrw_pos = numpy.array([0.0] * 3)
front_cent_pos = (ls_pos + rs_pos) / 2 # Front center position
rear_cent_pos = (lrw_pos + rrw_pos) / 2 # Rear center position
self._wheelbase = numpy.linalg.norm(front_cent_pos - rear_cent_pos)
self._inv_wheelbase = 1 / self._wheelbase # Inverse of _wheelbase
self._wheelbase_sqr = self._wheelbase**2
#load servo commander priorities
self.commandPriorities = rospy.get_param("~servoCommandProirities", [])
self.commandPriorities = sorted(self.commandPriorities.items(),
key=operator.itemgetter(1))
rospy.loginfo("AutoRallyGazeboController: Loaded %d servo commanders",
len(self.commandPriorities))
# Publishers and subscribers
self._left_steer_cmd_pub = \
_create_cmd_pub(list_ctrlrs, left_steer_ctrlr_name)
self._right_steer_cmd_pub = \
_create_cmd_pub(list_ctrlrs, right_steer_ctrlr_name)
self._left_front_axle_cmd_pub = \
_create_axle_cmd_pub(list_ctrlrs, left_front_axle_ctrlr_name)
self._right_front_axle_cmd_pub = \
_create_axle_cmd_pub(list_ctrlrs, right_front_axle_ctrlr_name)
self._left_rear_axle_cmd_pub = \
_create_axle_cmd_pub(list_ctrlrs, left_rear_axle_ctrlr_name)
self._right_rear_axle_cmd_pub = \
_create_axle_cmd_pub(list_ctrlrs, right_rear_axle_ctrlr_name)
self.servoCmdSub = \
rospy.Subscriber("servoCommand", servoMSG,
self.servoCmdCb, queue_size=1)
def __init__(self, robot_name, robot_config, robot_is_marsupial):
rospy.init_node('cloudsim2osgar', log_level=rospy.DEBUG)
self.bus = Bus()
self.robot_name = robot_name
self.robot_config = robot_config
self.prev_gas_detected = None # report on change including the first reading
self.mapping_info_provider = MappingInformationProvider()
# common topics
topics = [
('/' + robot_name + '/battery_state', BatteryState, self.battery_state, ('battery_state',)),
('/subt/score', Int32, self.score, ('score',)),
('/' + robot_name + '/gas_detected', Bool, self.gas_detected, ('gas_detected', )),
]
publishers = {}
publishers['cmd_vel'] = (rospy.Publisher('/' + robot_name + '/cmd_vel', Twist, queue_size=1), twist)
self.tf = tf.TransformListener()
# configuration specific topics
robot_description = rospy.get_param("/{}/robot_description".format(robot_name))
if robot_config == "ROBOTIKA_X2_SENSOR_CONFIG_1":
rospy.loginfo("robotika x2")
elif robot_config == "SSCI_X4_SENSOR_CONFIG_2":
rospy.loginfo("ssci drone")
topics.append(('/' + robot_name + '/top_scan', LaserScan, self.top_scan, ('top_scan',)))
topics.append(('/' + robot_name + '/bottom_scan', LaserScan, self.bottom_scan, ('bottom_scan',)))
topics.append(('/' + robot_name + '/odom_fused', Odometry, self.odom_fused, ('pose3d',)))
topics.append(('/' + robot_name + '/air_pressure', FluidPressure, self.air_pressure, ('air_pressure',)))
topics.append(('/' + robot_name + '/front_scan', LaserScan, self.scan360, ('scan360',)))
topics.append(('/rtabmap/rgbd/compressed', RGBDImage, self.rgbd_front, ('rgbd_front',)))
if robot_name.endswith('XM'):
topics.append(('/mapping/octomap_binary', Octomap, self.octomap, ('octomap',)))
if robot_is_marsupial == 'true':
rospy.loginfo("X4 is marsupial")
publishers['detach'] = (rospy.Publisher('/' + robot_name + '/detach', Empty, queue_size=1), empty)
elif robot_config == "CORO_PAM_SENSOR_CONFIG_1":
rospy.loginfo("CoRo Pam drone")
topics.append(('/' + robot_name + '/local_map/output/up', LaserScan, self.top_scan, ('top_scan',)))
topics.append(('/' + robot_name + '/local_map/output/down', LaserScan, self.bottom_scan, ('bottom_scan',)))
topics.append(('/' + robot_name + '/odom_fused', Odometry, self.odom_fused, ('pose3d',)))
topics.append(('/' + robot_name + '/air_pressure', FluidPressure, self.air_pressure, ('air_pressure',)))
topics.append(('/rtabmap/rgbd/front/compressed', RGBDImage, self.rgbd_front, ('rgbd_front',)))
topics.append(('/rtabmap/rgbd/left/compressed_rotated', RGBDImage, self.rgbd_left, ('rgbd_left',)))
topics.append(('/rtabmap/rgbd/right/compressed_rotated', RGBDImage, self.rgbd_right, ('rgbd_right',)))
topics.append(('/' + robot_name + '/local_map/output/scan', LaserScan, self.scan360, ('scan360',)))
if robot_name.endswith('XM'):
topics.append(('/mapping/octomap_binary', Octomap, self.octomap, ('octomap',)))
if robot_is_marsupial == 'true':
rospy.loginfo("CoRo Pam is marsupial")
publishers['detach'] = (rospy.Publisher('/' + robot_name + '/detach', Empty, queue_size=1), empty)
elif robot_config == "TEAMBASE":
rospy.loginfo("teambase")
elif robot_config.startswith("ROBOTIKA_FREYJA_SENSOR_CONFIG"):
if robot_config.endswith("_2"):
rospy.loginfo("freya 2 (with comms beacons)")
publishers['deploy'] = (rospy.Publisher('/' + robot_name + '/breadcrumb/deploy', Empty, queue_size=1), empty)
else:
rospy.loginfo("freya 1 (basic)")
topics.append(('/' + robot_name + '/odom_fused', Odometry, self.odom_fused, ('pose3d',)))
topics.append(('/' + robot_name + '/scan_front', LaserScan, self.scan_front, ('scan_front',)))
topics.append(('/' + robot_name + '/scan_rear', LaserScan, self.scan_rear, ('scan_rear',)))
topics.append(('/' + robot_name + '/local_map/output/scan', LaserScan, self.scan360, ('scan360',)))
topics.append(('/rtabmap/rgbd/front/compressed', RGBDImage, self.rgbd_front, ('rgbd_front',)))
topics.append(('/rtabmap/rgbd/rear/compressed', RGBDImage, self.rgbd_rear, ('rgbd_rear',)))
topics.append(('/' + robot_name + '/camera_left/image_raw/compressed', CompressedImage, self.camera_left, ('camera_left',)))
topics.append(('/' + robot_name + '/camera_right/image_raw/compressed', CompressedImage, self.camera_right, ('camera_right',)))
if robot_name.endswith('XM'):
topics.append(('/mapping/octomap_binary', Octomap, self.octomap, ('octomap',)))
elif robot_config.startswith("ROBOTIKA_KLOUBAK_SENSOR_CONFIG"):
if robot_config.endswith("_2"):
rospy.loginfo("k2 2 (with comms beacons)")
publishers['deploy'] = (rospy.Publisher('/' + robot_name + '/breadcrumb/deploy', Empty, queue_size=1), empty)
else:
rospy.loginfo("k2 1 (basic)")
topics.append(('/' + robot_name + '/odom_fused', Odometry, self.odom_fused, ('pose3d',)))
topics.append(('/' + robot_name + '/scan_front', LaserScan, self.scan_front, ('scan_front',)))
topics.append(('/' + robot_name + '/scan_rear', LaserScan, self.scan_rear, ('scan_rear',)))
topics.append(('/' + robot_name + '/local_map/output/scan', LaserScan, self.scan360, ('scan360',)))
topics.append(('/rtabmap/rgbd/front/compressed', RGBDImage, self.rgbd_front, ('rgbd_front',)))
topics.append(('/rtabmap/rgbd/rear/compressed', RGBDImage, self.rgbd_rear, ('rgbd_rear',)))
else:
rospy.logerr("unknown configuration")
return
if robot_config.startswith("ROBOTIKA_KLOUBAK_SENSOR_CONFIG"):
topics.append(('/' + robot_name + '/imu/front/data', Imu, self.imu, ('acc',)))
else:
topics.append(('/' + robot_name + '/imu/data', Imu, self.imu, ('acc',)))
outputs = functools.reduce(operator.add, (t[-1] for t in topics)) + ('robot_name', 'joint_angle')
self.bus.register(outputs)
for name, type, handler, _ in topics:
rospy.loginfo("waiting for {}".format(name))
rospy.wait_for_message(name, type)
setattr(self, handler.__name__+"_count", 0)
rospy.Subscriber(name, type, handler)
self.bus.publish('robot_name', robot_name)
# main thread receives data from osgar and sends it to ROS
while True:
channel, data = self.bus.listen()
# switch on channel to feed different ROS publishers
if channel in publishers:
publisher, handle = publishers[channel]
publisher.publish(handle(data))
if channel != 'cmd_vel':
# for debugging breadcrumbs deployment and marsupial detachment
rospy.logdebug("receiving: {} {}".format(channel, data))
elif channel == 'artf_xyz':
for artf_type, artf_xyz, _, _ in data:
self.mapping_info_provider.add_artifact(artf_type, artf_xyz)
else:
rospy.loginfo("ignoring: {} {}".format(channel, data))
def __init__(self,
map_topic='/map',
paths=[
'/move_base/SBPLLatticePlanner/plan',
'/move_base/TrajectoryPlannerROS/local_plan'
],
polygons=['/move_base/local_costmap/robot_footprint'],
tf=None,
parent=None):
super(NavView, self).__init__()
self._parent = parent
self._pose_mode = False
self._goal_mode = False
self.last_path = None
self.map_changed.connect(self._update)
self.destroyed.connect(self.close)
#ScrollHandDrag
self.setDragMode(QGraphicsView.ScrollHandDrag)
self._map = None
self._map_item = None
self.w = 0
self.h = 0
self._paths = {}
self._polygons = {}
self.path_changed.connect(self._update_path)
self.polygon_changed.connect(self._update_polygon)
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46),
(102, 224, 18), (242, 156, 6), (240, 64, 10),
(196, 30, 250)]
self._scene = QGraphicsScene()
if tf is None:
self._tf = tf.TransformListener()
else:
self._tf = tf
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
for path in paths:
self.add_path(path)
for poly in polygons:
self.add_polygon(poly)
try:
self._pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=100)
self._goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=100)
except TypeError:
self._pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped)
self._goal_pub = rospy.Publisher('/move_base_simple/goal',
PoseStamped)
self.setScene(self._scene)
def __init__(self):
# Real robot new positions
# self.joint_positions_p1 = 1*np.array([0.313771815355006, 0.7874442301146876, -0.35008257875556276, -1.605427980371526, 0.39355274195809514, 0.7924178032543101, -1.4842731591265332])
# self.joint_positions_p2 = 1*np.array([0.5628616530365289, 0.8266034640114479, -0.343280991826965, -1.548290132212003, 0.36961525944794194, 0.787609672383395, -1.4840846472048561])
# self.joint_positions_p3 = 1*np.array([0.7087980537375754, 0.9364973682354464, -0.3175523589760338, -1.3048212555591447, 0.32881175402628704, 0.9519498877320673, -1.483692343658399])
# self.joint_positions_home = 1*np.array([0.5569787767738359, 0.2807509613788765, -0.2521163338340084, -1.4935594952802898, 0.06308649381713503, 1.3422390616692874, -1.528839315708223])
# FOR VIDEOS WITH SIMULATION
# self.joint_positions_p1 = 1*np.array([0.313771815355006, 0.7874442301146876, -0.35008257875556276, -1.605427980371526, 0.39355274195809514, 0.7924178032543101, -1.4842731591265332])
# self.joint_positions_p2 = 1*np.array([0.5628616530365289, 0.8266034640114479, -0.343280991826965, -1.548290132212003, 0.36961525944794194, 0.787609672383395, -1.4840846472048561])
# self.joint_positions_p3 = 1*np.array([0.9087980537375754, 0.9364973682354464, -0.3175523589760338, -1.3048212555591447, 0.32881175402628704, 0.9519498877320673, -1.483692343658399])
# self.joint_positions_home = 1*np.array([0.5569787767738359, 0.2807509613788765, -0.2521163338340084, -1.4935594952802898, 0.06308649381713503, 1.3422390616692874, -1.528839315708223])
# self.joint_positions_home = 1*np.array([0.5569787767738359, 0.2807509613788765, -0.2521163338340084, -1.4935594952802898, 0.06308649381713503, 1.3422390616692874, -1.528839315708223])
# self.joint_positions_p1 = 1*np.array([0.27099370506783615, 0.7950941203635832, -0.18290599687813244, -1.545624301259042, 0.19252302584488534, 0.8620693993694969, -0.023598616669335572])
# self.joint_positions_p2 = 1*np.array([0.6446756195683863, 0.9211181322779567, -0.4819512971273034, -1.4284985165235762, 0.5240945070507964, 0.9734429360283877, 0.024398803334734404])
# self.joint_positions_p3 = 1*np.array([1.0506115772018698, 1.2189853831695028, -1.0281253363138159, -1.1760928235711319, 0.9755587628795142, 1.2917956014430403, 0.11816475720439637])
# self.joint_positions_handover = 1*np.array([0.5569787767738359, 0.2807509613788765, -0.2521163338340084, -1.4935594952802898, 0.06308649381713503, 0.2022390616692874, -1.528839315708223])
self.joint_positions_home = 1 * np.array([
0.20024075874350197, 0.3457841368224398, -0.39772640410707416,
-1.5429807609967892, 0.1143476257008436, 1.2734894582565162,
-5.7464311123443805e-05
])
self.joint_positions_p1 = 1 * np.array([
-0.24520071701864543, 0.8216742323795524, -0.2692404729649888,
-1.497127918129015, 0.3290061859738872, 0.875714937494543,
0.027934688991962303
])
self.joint_positions_p2 = 1 * np.array([
0.005690829067532814, 0.7855678600152904, -0.21038864090325585,
-1.56908060519925, 0.14593324885458367, 0.8493683380970197,
-0.2437001721577387
])
self.joint_positions_p3 = 1 * np.array([
0.15672118448444747, 0.7813835079646257, -0.12592376380188047,
-1.560340569973171, 0.14926112433896319, 0.8510714761960086,
-0.025095686034286177
])
self.pub = rospy.Publisher('action_state', String, queue_size=10)
# self.pub_for_gripper = rospy.Publisher('gripper_command', String, queue_size = 10)
self.ctrl_freq = 200
self.q_send = 1 * np.array([
-0.2561309292711379, 0.48341762688080325, -0.07932016919260708,
-1.0536090715454243, -0.017271089745345972, 1.5119789493870104,
-1.5704001283280475
])
self.q_old = 1 * np.array([
-0.2561309292711379, 0.48341762688080325, -0.07932016919260708,
-1.0536090715454243, -0.017271089745345972, 1.5119789493870104,
-1.5704001283280475
])
self.old_q_send = 1 * np.array([
-0.2561309292711379, 0.48341762688080325, -0.07932016919260708,
-1.0536090715454243, -0.017271089745345972, 1.5119789493870104,
-1.5704001283280475
])
self.joint_names = [
"iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4",
"iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7"
]
self.joint_cmd_pub = rospy.Publisher(
'/iiwa/PositionController/command',
numpy_msg(Float64MultiArray),
queue_size=10)
self.joint_state_sub = rospy.Subscriber('iiwa/joint_states',
numpy_msg(JointState),
self.get_joint_state)
self.joint_current_states = self.joint_positions_home
self.position_reached = 0
self.current_action = "home"
self.current_target = self.joint_positions_home
self.gripper_service = rospy.ServiceProxy('gripper_command',
ControlGripper)
#PLOTS
self.commandJoints = [[] for _ in np.arange(7)]
self.errorJoints = [[] for _ in np.arange(7)]
self.speedJoints = [[] for _ in np.arange(7)]
self.endEffxyz = [[] for _ in np.arange(3)]
self.timeStart = 0
self.timeVector = []
self.listenerEndEff = tf.TransformListener()
self.time = []
self.subplotID = [331, 332, 333, 334, 335, 336, 337]
self.titles = [
'Joint 1', 'Joint 2', 'Joint 3', 'Joint 4', 'Joint 5', 'Joint 6',
'Joint 7'
]
#!/usr/bin/env python
# import roslib
# roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('turtle_tf_listener') #create node
listener = tf.TransformListener() #create TransformListener instance
rospy.wait_for_service('spawn') #Blocks until service is available
spawner = rospy.ServiceProxy(
'spawn', turtlesim.srv.Spawn) #Create a handle to a ROS service
#for invoking calls
spawner(4, 2, 0, 'turtle2') # create 2th turtle
#create Publisher
turtle_vel = rospy.Publisher('turtle2/cmd_vel',
geometry_msgs.msg.Twist,
queue_size=1)
rate = rospy.Rate(10.0) #Publish rate =10 Hz
while not rospy.is_shutdown():
try:
#get us the latest available transform from turtle1 to turtle2 frame
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_angular', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
self.rate = 30
r = rospy.Rate(self.rate)
# The test angle is 360 degrees
self.test_angle = radians(rospy.get_param('~test_angle', 360.0))
self.speed = rospy.get_param('~speed', 0.7) # radians per second
self.tolerance = rospy.get_param(
'tolerance', radians(5)) # degrees converted to radians
self.odom_angular_scale_correction = rospy.get_param(
'~odom_angular_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
# Fire up the dynamic_reconfigure server
dyn_server = Server(CalibrateAngularConfig,
self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_angular",
timeout=60)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(60.0))
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
reverse = 1
while not rospy.is_shutdown():
# Execute the rotation
if self.start_test:
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
last_angle = self.odom_angle
turn_angle = 0
# Alternate directions between tests
reverse = -reverse
angular_speed = reverse * self.speed
while abs(turn_angle) < abs(self.test_angle):
if rospy.is_shutdown():
return
move_cmd = Twist()
move_cmd.angular.z = angular_speed
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
# Compute how far we have gone since the last measurement
delta_angle = self.odom_angular_scale_correction * normalize_angle(
self.odom_angle - last_angle)
# Add to our total angle so far
turn_angle += delta_angle
last_angle = self.odom_angle
# Stop the robot
self.cmd_vel.publish(Twist())
# Update the status flag
self.start_test = False
params = {'start_test': False}
dyn_client.update_configuration(params)
rospy.sleep(0.5)
# Stop the robot
self.cmd_vel.publish(Twist())
def __init__(self, sim):
"""
Subscribe to the configuration message
"""
if (False == sim):
self.config_updated = False
rospy.Subscriber("/segway/feedback/configuration", Configuration,
self._update_configuration_limits)
rospy.sleep(1.0)
if (False == self.config_updated):
rospy.logerr(
"Timed out waiting for RMP feedback topics make sure the driver is running"
)
sys.exit(0)
return
else:
self.vel_limit_mps = 0.5
self.yaw_rate_limit_rps = 0.5
self.accel_lim = 1.0
self.yaw_accel_lim = 1.0
# create an interactive marker server on the topic namespace simple_marker
self._server = InteractiveMarkerServer("segway_marker_ctrl")
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.last_marker_update = rospy.get_time()
robot_name = rospy.get_param('~robot_name', "RMP_110")
if ("RMP_OMNI" == robot_name):
self.include_y = True
else:
self.include_y = False
self.linear_scale = rospy.get_param('~linear_scale', 1.0)
self.angular_scale = rospy.get_param('~angular_scale', 2.2)
self.motion_cmd = Twist()
self.motion_pub = rospy.Publisher('/segway/int_marker/cmd_vel',
Twist,
queue_size=10)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
int_marker.name = "segway_twist_ctrl"
int_marker.description = "Segway Control Marker"
control = InteractiveMarkerControl()
control.orientation_mode = InteractiveMarkerControl.FIXED
control.orientation.w = 0.707106781
control.orientation.x = 0.707106781
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
if (True == self.include_y):
control = InteractiveMarkerControl()
control.orientation_mode = InteractiveMarkerControl.FIXED
control.orientation.w = 0.707106781
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 0.707106781
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation_mode = InteractiveMarkerControl.FIXED
control.orientation.w = 0.707106781
control.orientation.x = 0
control.orientation.y = 0.707106781
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
self._server.insert(int_marker, self.processFeedback)
# 'commit' changes and send to all clients
self._server.applyChanges()
SegwayMarkerMenu(self._server, sim)
pub1.publish(cloud)
else:
pub2.publish(cloud)
return
def transform_cloud(cloud_array, transformation_matrix=np.eye(4)):
'''
Transforms all points in a cloud given a transformation matrix
Input: Nx3 array of points in cloud
Output: Nx3 array of transformed points
'''
if cloud_array.shape[0] > cloud_array.shape[1]:
homogenious_coordinates = np.vstack(
(cloud_array.T, np.ones(cloud_array.shape[0])))
else:
homogenious_coordinates = np.vstack(
(cloud_array, np.ones(cloud_array.shape[1])))
transformed_cloud = np.matmul(transformation_matrix,
homogenious_coordinates)
return transformed_cloud[:3, :].T
if __name__ == '__main__':
rospy.init_node('cloud_processing_node')
tf_tree = tf.TransformListener()
rospy.Subscriber('/sensor/side_camera/depth/color/points', PointCloud2,
cloud_sub_callback)
rospy.spin()
def __init__(self):
self._cv_bridge = CvBridge()
self._laser_projector = LaserProjection()
# # Camera rectification?? To be improved: read from .yaml file
# Put here the calibration of the camera
# self.DIM = (1920, 1208)
# self.K = np.array([[693.506921, 0.000000, 955.729444], [0.000000, 694.129349, 641.732500], [0.0, 0.0, 1.0]])
# self.D = np.array([[-0.022636], [ -0.033855], [0.013493], [-0.001831]])
# self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(self.K, self.D, np.eye(3), self.K, self.DIM, cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix
# OR load it from a yaml file
self.cameraModel = PinholeCameraModel()
# See https://github.com/ros-perception/camera_info_manager_py/tree/master/tests
camera_infomanager = CameraInfoManager(
cname='truefisheye',
url='package://ros_camera_lidar_calib/cfg/truefisheye800x503.yaml'
) # Select the calibration file
camera_infomanager.loadCameraInfo()
self.cameraInfo = camera_infomanager.getCameraInfo()
# Crete a camera from camera info
self.cameraModel.fromCameraInfo(self.cameraInfo) # Create camera model
self.DIM = (self.cameraInfo.width, self.cameraInfo.height)
# Get rectification maps
self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(
self.cameraModel.intrinsicMatrix(),
self.cameraModel.distortionCoeffs(), np.eye(3),
self.cameraModel.intrinsicMatrix(),
(self.cameraInfo.width, self.cameraInfo.height),
cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix
# # Declare subscribers to get the latest data
cam0_subs_topic = '/gmsl_camera/port_0/cam_0/image_raw'
cam1_subs_topic = '/gmsl_camera/port_0/cam_1/image_raw'
cam2_subs_topic = '/gmsl_camera/port_0/cam_2/image_raw'
#cam3_subs_topic = '/gmsl_camera/port_0/cam_3/image_raw/compressed'
lidar_subs_topic = '/Sensor/points'
#self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , Image, self.cam0_img_callback, queue_size=1)
#self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , Image, self.cam1_img_callback, queue_size=1)
self.cam2_img_sub = rospy.Subscriber(cam2_subs_topic,
Image,
self.cam2_img_callback,
queue_size=1)
#self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , CompressedImage, self.cam0_img_compre_callback, queue_size=1)
#self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , CompressedImage, self.cam1_img_compre_callback, queue_size=1)
#self.cam2_img_sub = rospy.Subscriber( cam2_subs_topic , CompressedImage, self.cam2_img_compre_callback, queue_size=1)
#self.cam3_img_sub = rospy.Subscriber( cam3_subs_topic , CompressedImage, self.cam3_img_compre_callback, queue_size=1)
self.lidar_sub = rospy.Subscriber(lidar_subs_topic,
PointCloud2,
self.lidar_callback,
queue_size=1)
# Get the tfs
self.tf_listener = tf.TransformListener()
#self.lidar_time = rospy.Subscriber(lidar_subs_topic , PointCloud2, self.readtimestamp)
#self.img0_time = rospy.Subscriber(cam0_subs_topic , CompressedImage, self.readtimestamp)
# # Declare the global variables we will use to get the latest info
self.cam0_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam0_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam1_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam1_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam2_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam2_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam3_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam3_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.pcl_cloud = np.empty(
(500, 4)
) # Do not know the width of a normal scan. might be variable too......
self.now = rospy.Time.now()
# # Main loop: Data projections and alignment on real time
self.lidar_angle_range_interest = [
0, 180
] # From -180 to 180. Front is 0deg. Put the range of angles we may want to get points from. Depending of camera etc
thread.start_new_thread(self.projection_calibrate())
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoints_tree = None
self.has_image = None
self.camera_image = None
self.lights = []
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic
light in 3D map space and helps you acquire an accurate ground truth
data source for the traffic light classifier by sending the current
color state of all traffic lights in the simulator. When testing on
the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
# TODO CAEd: consider other image formats to feed into classifier
rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.upcoming_red_light_pub =\
rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.bridge = CvBridge()
# CAEd: set up classifier
# full_param_name = rospy.search_param('model_location')
# print("Full_Param Name: %s " % full_param_name)
if IS_SIM:
model_location = rospy.get_param(
"/sim_model_path",
'../../../models/ssd_sim/frozen_inference_graph.pb')
else:
model_location = rospy.get_param(
"/real_model_path",
'../../../models/ssd_real/frozen_inference_graph.pb')
model_filter = rospy.get_param("/model_filter", 10)
min_score = rospy.get_param("/min_score", 0.5)
width = rospy.get_param("/width", 800)
height = rospy.get_param("/height", 600)
self.light_classifier =\
TLClassifier(model_location, model_filter,
min_score, width, height)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.loop()
def __init__(self):
### MoveIt!
moveit_commander.roscpp_initialize(sys.argv)
br = tf.TransformBroadcaster()
#rospy.init_node('move_group_planner',
# anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_left = moveit_commander.MoveGroupCommander("panda_left")
self.group_right = moveit_commander.MoveGroupCommander("panda_right")
self.hand_left = moveit_commander.MoveGroupCommander("hand_left")
self.hand_right = moveit_commander.MoveGroupCommander("hand_right")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_left.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_left.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_right.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_right.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get a list of all the groups in the robot:
self.group_names = self.robot.get_group_names()
print("============ Robot Groups:", self.robot.get_group_names())
rospy.sleep(1)
stefan_dir = "/home/jiyeong/STEFAN/stl/"
self.stefan = SceneObject()
self.scene.remove_attached_object(
self.group_left.get_end_effector_link())
self.scene.remove_attached_object(
self.group_right.get_end_effector_link())
self.scene.remove_world_object()
for key, value in self.stefan.list.items():
self.scene.add_mesh(key, value, stefan_dir + key + ".stl")
print("============ Add Mesh:")
### Franka Collision
self.set_collision_behavior = rospy.ServiceProxy(
'franka_control/set_force_torque_collision_behavior',
franka_control.srv.SetForceTorqueCollisionBehavior)
#self.set_collision_behavior.wait_for_service()
self.active_controllers = []
self.listener = tf.TransformListener()
self.tr = TransformerROS()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") ]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") ]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral") ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.04)
arm.set_goal_orientation_tolerance(1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 1
rospy.loginfo("Scaling for MoveIt timeout=" + str(rospy.get_param('/move_group/trajectory_execution/allowed_execution_duration_scaling')))
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the closed position
rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed ) )
gripper.set_joint_value_target(self.gripper_closed)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the neutral position
rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral) )
gripper.set_joint_value_target(self.gripper_neutral)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.4
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.07, 0.007, 0.10]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.36
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = (table_ground + table_size[2] / 2.0)-0.08
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
box1_pose.pose.position.y = 0.0
box1_pose.pose.position.z = (table_ground + table_size[2] + box1_size[2] / 2.0)-0.08
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
box2_pose.pose.position.y = 0.2
box2_pose.pose.position.z = (table_ground + table_size[2] + box2_size[2] / 2.0)-0.08
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = table_pose.pose.position.x - 0.03
target_pose.pose.position.y = 0.1
target_pose.pose.position.z = (table_ground + table_size[2] + target_size[2] / 2.0)-0.075
target_pose.pose.orientation.w = 0.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = table_pose.pose.position.x - 0.15
place_pose.pose.position.y = -0.23
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 0.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] + self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
success = False
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts:
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
rospy.loginfo("Initializing %s" % rospy.get_name())
## parameters
self.map_frame = rospy.get_param("~map_frame", 'odom')
self.bot_frame = 'base_link'
self.switch_thred = 1.5 # change to next lane if reach next
self.pursuit = PurePursuit()
self.lka = rospy.get_param("~lookahead", 0.5)
self.pursuit.set_look_ahead_distance(self.lka)
## node path
while not rospy.has_param("/guid_path") and not rospy.is_shutdown():
rospy.loginfo("Wait for /guid_path")
rospy.sleep(0.5)
self.guid_path = rospy.get_param("/guid_path")
self.tag_ang_init = rospy.get_param("/guid_path_ang_init")
self.last_node = -1
self.next_node_id = None
self.all_anchor_ids = rospy.get_param("/all_anchor_ids")
self.joy_lock = False
self.get_goal = True
self.joint_ang = None
## set first tracking lane
self.pub_robot_speech = rospy.Publisher("speech_case",
String,
queue_size=1)
self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1)
rospy.sleep(1) # wait for the publisher to be set well
self.set_lane(True)
# variable
self.target_global = None
self.listener = tf.TransformListener()
# markers
self.pub_goal_marker = rospy.Publisher("goal_marker",
Marker,
queue_size=1)
self.pub_target_marker = rospy.Publisher("target_marker",
Marker,
queue_size=1)
# publisher, subscriber
self.pub_pid_goal = rospy.Publisher("pid_control/goal",
PoseStamped,
queue_size=1)
self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan)
self.sub_box = rospy.Subscriber("anchor_position",
PoseDirectional,
self.cb_goal,
queue_size=1)
sub_joy = rospy.Subscriber('joy_teleop/joy',
Joy,
self.cb_joy,
queue_size=1)
sub_fr = rospy.Subscriber('front_right/ranges',
DeviceRangeArray,
self.cb_range,
queue_size=1)
sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states',
JointState,
self.cb_joint,
queue_size=1)
#Don't update goal too often
self.last_update_goal = None
self.goal_update_thred = 0.001
self.hist_goal = np.array([])
self.normal = True
self.get_goal = True
self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking)
# print("init done")
# prevent sudden stop
self.last_plan = None
# keep searching until find path or reach search end
self.search_angle = 10 * math.pi / 180
self.search_max = 5
### stupid range drawing
# for using tf to draw range
br1 = tf2_ros.StaticTransformBroadcaster()
br2 = tf2_ros.StaticTransformBroadcaster()
# stf.header.frame_id = "uwb_back_left"
# stf.child_frame_id = "base_link"
# stf.transform.translation.x = -1*r_tag_points[0, 0]
# stf.transform.translation.y = -1*r_tag_points[0, 1]
# br1.sendTransform(stf)
# stf2 = stf
# stf2.header.stamp = rospy.Time.now()
# stf2.header.frame_id = "uwb_front_right"
# stf2.transform.translation.x = -1*r_tag_points[1, 0]
# stf2.transform.translation.y = -1*r_tag_points[1, 1]
# br2.sendTransform(stf2)
stf = TransformStamped()
stf.header.stamp = rospy.Time.now()
stf.transform.rotation.w = 1
stf.header.frame_id = "base_link"
stf.child_frame_id = "uwb_back_left"
stf.transform.translation.x = r_tag_points[0, 0]
stf.transform.translation.y = r_tag_points[0, 1]
stf2 = TransformStamped()
stf2.header.stamp = rospy.Time.now()
stf2.transform.rotation.w = 1
stf2.header.frame_id = "base_link"
stf2.child_frame_id = "uwb_front_right"
stf2.transform.translation.x = r_tag_points[1, 0]
stf2.transform.translation.y = r_tag_points[1, 1]
def __init__(self):
rospy.init_node('tl_detector')
self.base_waypoints = None
self.curr_pose = None
self.camera_image = None
self.tree = None
self.lights = []
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.datasize = 0
self.ignore_state = False
# Subscribe to topic '/current_pose' to get the current position of the car
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
# Subscribe to topic '/image_color' to get the image from the cars front camera
rospy.Subscriber('/image_color', Image, self.image_cb)
# Subscribe to topic '/vehicle/traffic_lights' to get the location and state of the traffic lights
# IMPORTANT: The state will not be available in real life testing
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_lights_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# Fetch info from '/base_waypoints', which provide a complete list of waypoints the car will be following
wp = rospy.wait_for_message('/base_waypoints', Lane)
# Set the waypoints only once
if not self.base_waypoints:
self.base_waypoints = wp.waypoints
self.datasize = len(self.base_waypoints)
if PRINT_DEBUG:
rospy.logwarn('Got the base points for tl_detector.')
# Get the x/y coordinates of the base_waypoints
b_xcor = []
b_ycor = []
for pt in self.base_waypoints:
b_xcor.append(pt.pose.pose.position.x)
b_ycor.append(pt.pose.pose.position.y)
self.tree = KDTree(zip(b_xcor, b_ycor))
# Create the publisher to write messages to topic '/traffic_waypoint'
# The index of the waypoint which is closest to the next red traffic light has to be published
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
# Create the publisher to write messages to topic '/traffic_waypoint_state'
# The state of the closest red traffic light has to be published
self.upcoming_red_light_state_pub = rospy.Publisher('/traffic_waypoint_state', Int32, queue_size=1)
# Block until shutdown -> Tasks are handled with callbacks
rospy.spin()
def follower_node():
rospy.init_node('follower')
global robotX
global robotY
global robotZ
global motor_command_publisher
motor_command_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size = 10)
waypoint_subscriber = rospy.Subscriber("/waypoint_cmd", Transform, waypoint_callback)
action_subscriber = rospy.Subscriber("/incoming", Transform, avoid_object)
map_subscriber = rospy.Subscriber("/map", OccupancyGrid, map_callback, queue_size = 1000)
listener = tf.TransformListener()
step=1
size=0.7
dx=0
dy=0
dz=0
print "Starting, dont forget to enable motors"
delay = rospy.Rate(50.0);
while not rospy.is_shutdown():
motor_command = Twist()
objectDetected=False
error = False
stopped = False
if control:
try:
#grab the latest available transform from the odometry frame (robot's original location - usually the same as the map unless the odometry becomes inaccurate) to the robot's base.
(translationZ,orientationZ) = listener.lookupTransform("/base_link", "/base_footprint", rospy.Time(0));
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print("EXCEPTION:",e)
#if something goes wrong with this just go to bed for a second or so and wake up hopefully refreshed.
delay.sleep()
continue
try:
#grab the latest available transform from the odometry frame (robot's original location - usually the same as the map unless the odometry becomes inaccurate) to the robot's base.
(translation,orientation) = listener.lookupTransform("/world", "/base_footprint", rospy.Time(0));
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print("EXCEPTION:",e)
delay.sleep()
continue
#print "\n---------------------\ntranslation: ", translation, "\norientation: ", orientation, "\nwaypoint:\n", waypoint
robotX=translation[0]
robotY=translation[1]
robotZ=translationZ[2]
print "Robot coordinates: ", robotX, "-", robotY, "-", robotZ
if slamMap is not None:
xMax = round((robotX-slamMap.info.origin.position.x+size)/slamMap.info.resolution)
yMax = round((robotY-slamMap.info.origin.position.y+size)/slamMap.info.resolution)
xMin = round((robotX-slamMap.info.origin.position.x-size)/slamMap.info.resolution)
yMin = round((robotY-slamMap.info.origin.position.y-size)/slamMap.info.resolution)
xRob = round((robotX-slamMap.info.origin.position.x)/slamMap.info.resolution)
yRob = round((robotY-slamMap.info.origin.position.y)/slamMap.info.resolution)
print "Map data: ", slamMap.info.width, "-", slamMap.info.height, "-", slamMap.info.resolution, "-", slamMap.info.origin.position.x, "-", slamMap.info.origin.position.y
print "Checking map: ", xMax, "-", xMin, "-", yMax, "-", yMin, " Robot on map: ", int(xRob), "-", int(yRob)
#sys.stdout.write('\n')
for y in range(int(yMin),int(yMax),1):
for x in range(int(xMin),int(xMax),1):
index = x+y*slamMap.info.width
if abs(xRob-x)<1 and abs(yRob-y)<1:
if slamMap.data[index] > 90:
error = True
#sys.stdout.write('R')
elif slamMap.data[index] > 90:
## This square is occupied
objectDetected = True
#sys.stdout.write('X')
#elif slamMap.data[index] >= 0:
## This square is unoccupied
#sys.stdout.write(" ")
#else:
#sys.stdout.write('?')
#sys.stdout.write('\n')
else:
print "Map data not found! Check if slam works"
if not error and objectDetected:
print("Object detected, stop!")
if not stopped and dx!=0 and dy!=0 and dz!=0:
motor_command.linear.x = -dx
motor_command.linear.y = -dy
motor_command.linear.z = -dz
#print "command!\n", motor_command.linear.x, "-", motor_command.linear.y, "-", motor_command.linear.z, "-k: ", k
motor_command_publisher.publish(motor_command)
stopped=True
else:
if(abs(robotZ)<0.3): #prevent collisions
motor_command.linear.z = 0.2
motor_command_publisher.publish(motor_command)
rospy.sleep(0.2)
dx=waypoint.translation.x-robotX
dy=waypoint.translation.y-robotY
dz=waypoint.translation.z+robotZ#base link has negative z
#print "moving!\n", dx, "-", dy, "-", dz
k=1
if(abs(dx)>step or abs(dy)>step or abs(dz)>step):
maxd = max(abs(dx),abs(dy),abs(dz))
k = step / maxd
dx = dx*k
dy = dy*k
dz = dz*k
motor_command.linear.x = dx
motor_command.linear.y = dy
motor_command.linear.z = dz
#print "command!\n", motor_command.linear.x, "-", motor_command.linear.y, "-", motor_command.linear.z, "-k: ", k
motor_command_publisher.publish(motor_command)
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.lmove_group = MoveGroupInterface("left_arm", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
self.move_group.setPlannerId("RRTConnectkConfigDefault")
self.lmove_group.setPlannerId("RRTConnectkConfigDefault")
self.rmove_group.setPlannerId("RRTConnectkConfigDefault")
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint",
"left_shoulder_lift_joint", "left_shoulder_pan_joint",
"left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint",
"linear_joint", "pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.pickplace = [None] * 2
self.pickplace[0] = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace[0].planner_id = "RRTConnectkConfigDefault"
self.pickplace[1] = PickPlaceInterface("right_side",
"right_gripper",
verbose=True)
self.pickplace[1].planner_id = "RRTConnectkConfigDefault"
self.pick_result = [None] * 2
self._last_gripper_picked = 0
self.place_result = [None] * 2
self._last_gripper_placed = 0
self._objs_to_keep = []
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
self._rgripper = GripperActionClient('right')
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
self.scene.clear()
# This is a simulation so need to adjust gripper parameters
if sim:
self._gripper_closed = 0.96
self._gripper_open = 0.0
else:
self._gripper_closed = 0.0
self._gripper_open = 0.165
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.is_site = self.config['is_site']
#TODO Remove hack to force site mode or ground_truth for testing
# self.is_site = True
self.ground_truth = False
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.waypoints_2d = None
self.waypoint_tree = None
self.vgg_model = None
self.graph = None
self.sess = None
self.initialized = False
if self.is_site:
# Detector Stuff
self.model_image_size = None
model_path = os.path.expanduser('./weights/parking_lot.h5')
anchors_path = os.path.expanduser('./model_data/lisa_anchors.txt')
classes_path = os.path.expanduser('./model_data/lisa_classes.txt')
self.class_names = utils.get_classes(classes_path)
anchors = utils.get_anchors(anchors_path)
if SHALLOW_DETECTOR:
anchors = anchors * 2
self.yolo_model, _ = create_model(
anchors,
self.class_names,
load_pretrained=True,
feature_extractor=FEATURE_EXTRACTOR,
pretrained_path=model_path,
freeze_body=True)
# Check if model is fully convolutional, assuming channel last order.
self.model_image_size = self.yolo_model.layers[0].input_shape[1:3]
self.sess = K.get_session()
# Generate output tensor targets for filtered bounding boxes.
self.yolo_outputs = decode_yolo_output(self.yolo_model.output,
anchors,
len(self.class_names))
self.input_image_shape = K.placeholder(shape=(2, ))
self.boxes, self.scores, self.classes = yolo_eval(
self.yolo_outputs,
self.input_image_shape,
score_threshold=.6,
iou_threshold=.6)
self.graph = tensorflow.get_default_graph()
else:
try:
model_path = os.path.expanduser('./weights/vgg16_1.h5')
self.vgg_model = load_model(model_path)
self.graph = tensorflow.get_default_graph()
except:
rospy.logerr(
"Could not load model. Have you downloaded the vgg16_1.h5 file to the weights folder? You can download it here: https://s3-eu-west-1.amazonaws.com/sdcnddata/vgg16_1.h5"
)
self.initialized = True
rospy.spin()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.08) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.scene = scene
self.scene_pub = scene_pub
self.colors = dict()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m
self.approach_distance = 0.06
self.back_distance = 0.05
# sub and pub point cloud
self.point_cloud = None
self.update_cloud_flag = False
rospy.Subscriber('/camera/depth/color/points', PointCloud2,
self.callback_pointcloud)
thread.start_new_thread(self.publish_pointcloud, ())
def __init__(self):
rospy.init_node('turtlebot3_sandbot',
anonymous=False,
disable_signals=True)
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
position = Point()
move_cmd = Twist() #make empty twist message
r = rospy.Rate(10)
self.tf_listener = tf.TransformListener()
self.odom_frame = '/odom'
self.isFirst = True
print("isFirst initialized")
self.offset_x = 0
self.offset_y = 0
self.offset_rot = 0
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
(position, rotation) = self.get_odom()
if self.isFirst:
self.offset_x = position.x
self.offset_y = position.y
self.offset_rot = rotation
self.isFirst = False
print("offset initialized")
(position, rotation) = self.get_odom()
print("x, y, rotation", position.x, position.y, np.rad2deg(rotation))
last_rotation = 0
last_distance = 10
linear_speed = 1
angular_speed = 1
# (goal_x, goal_y, goal_z) = self.getkey()
# go through path array
ind = 1
length = len(arr_path_B)
distanceIncreasing = False
init_goal = arr_path_B[0]
goal_x = arr_path_B[ind][0] - init_goal[0]
goal_y = arr_path_B[ind][1] - init_goal[1]
while ind < length:
# if goal_z > 180 or goal_z < -180:
# print("you input worng z range.")
# self.shutdown()
# goal_z = np.deg2rad(goal_z)
# (position,rotation) = self.get_odom()
distance = sqrt(
pow(goal_x - position.x, 2) + pow(goal_y - position.y, 2))
while distance > 0.1:
try:
(position, rotation) = self.get_odom()
distance = sqrt(
pow((goal_x - position.x), 2) +
pow((goal_y - position.y), 2))
path_angle = atan2(goal_y - position.y,
goal_x - position.x)
# if last_distance<distance:
# ind = ind - increment
# distanceIncreasing = True
#Normalization of path_angle
if path_angle < -pi / 4 or path_angle > pi / 4:
if goal_y < 0 and position.y < goal_y:
path_angle = -2 * pi + path_angle
elif goal_y >= 0 and position.y > goal_y:
path_angle = 2 * pi + path_angle
#Normalization of rotation
if last_rotation > pi - 0.1 and rotation <= 0:
rotation = 2 * pi + rotation
elif last_rotation < -pi + 0.1 and rotation > 0:
rotation = -2 * pi + rotation
rot_angle = path_angle - rotation
if rot_angle > pi or (rot_angle < 0 and rot_angle > -pi):
diff_sign = -1.0
else:
diff_sign = 1.0
diff_magnitude = pi - abs(abs(path_angle - rotation) - pi)
diff = diff_sign * diff_magnitude
print("CURRENT ", position.x, position.y,
np.rad2deg(rotation))
print("GOAL", goal_x, goal_y, np.rad2deg(path_angle))
print("DISTANCE(m) ", distance)
# move_cmd.angular.z = angular_speed * diff
move_cmd.angular.z = angular_speed * rot_angle
move_cmd.linear.x = min(linear_speed * distance, 0.1)
if move_cmd.angular.z > 0:
move_cmd.angular.z = min(move_cmd.angular.z, 0.2)
else:
move_cmd.angular.z = max(move_cmd.angular.z, -0.2)
#if heading angle is over limit (while driving)
if diff_magnitude > pi / 4.0:
move_cmd.linear.x = 0
# move_cmd.angular.z = move_cmd.angular.z * (-1.0)
# move_cmd.angular.z = move_cmd.angular.z
last_rotation = rotation
last_distance = distance
# if distanceIncreasing == True:
# print("distance is increasing!!!")
# self.cmd_vel.publish(Twist())
# r.sleep()
# continue
print("linear.x angular.z")
print(move_cmd.linear.x, move_cmd.angular.z)
self.cmd_vel.publish(move_cmd)
r.sleep()
except KeyboardInterrupt:
rospy.signal_shutdown("KeboardInterrupt")
break
if rospy.is_shutdown():
break
#self.cmd_vel.publish(Twist())
print("Now at Waypoint No.", ind)
if ind < length - increment:
ind = ind + increment
goal_x = arr_path_B[ind][0] - init_goal[0]
goal_y = arr_path_B[ind][1] - init_goal[1]
else:
#arrived at the final destination
print("Robot at the final destination")
rospy.signal_shutdown("Robot Task Done")
break
(position, rotation) = self.get_odom()
path_angle = atan2(goal_y - position.y, goal_x - position.x)
rot_angle = path_angle - rotation
while True:
try:
print("rotation", np.rad2deg(rotation))
print("path_angle", np.rad2deg(path_angle))
move_cmd.linear.x = 0
#diff is always positive
diff = pi - abs(abs(rot_angle) - pi)
print("diff", np.rad2deg(diff))
rot_angle = path_angle - rotation
if rot_angle > pi or (rot_angle < 0 and rot_angle > -pi):
diff_sign = -1.0
else:
diff_sign = 1.0
diff_magnitude = pi - abs(abs(path_angle - rotation) - pi)
diff = diff_sign * diff_magnitude
if diff_sign < 0:
if diff_magnitude < np.deg2rad(20):
move_cmd.angular.z = -0.1
else:
move_cmd.angular.z = -0.2
else:
if diff_magnitude < np.deg2rad(20):
move_cmd.angular.z = 0.1
else:
move_cmd.angular.z = 0.2
(position, rotation) = self.get_odom()
path_angle = atan2(goal_y - position.y,
goal_x - position.x)
rot_angle = path_angle - rotation
# 8 too small
if abs(rot_angle) < np.deg2rad(8):
r.sleep()
break
self.cmd_vel.publish(move_cmd)
r.sleep()
except KeyboardInterrupt:
rospy.signal_shutdown("KeboardInterrupt")
break
#self.cmd_vel.publish(Twist())
if rospy.is_shutdown():
break
# if goal_z >= 0:
# if rotation <= goal_z and rotation >= goal_z - pi:
# move_cmd.linear.x = 0.00
# move_cmd.angular.z = 0.2
# else:
# move_cmd.linear.x = 0.00
# move_cmd.angular.z = -0.2
# else:
# if rotation <= goal_z + pi and rotation > goal_z:
# move_cmd.linear.x = 0.00
# move_cmd.angular.z = -0.2
# else:
# move_cmd.linear.x = 0.00
# move_cmd.angular.z = 0.2
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
# WE CHANGED THIS STUFF
# Time to stop at a stop sign
self.stop_time = rospy.get_param("~stop_time", 3.)
# Minimum distance from a stop sign to obey it (originally 0.5)
self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.7)
# Time taken to cross an intersection
self.crossing_time = rospy.get_param("~crossing_time", 3.)
# Number of stop sign detections in a row
self.stop_detections = 0
# Stop sign detector
rospy.Subscriber('/detector/stop_sign', DetectedObject,
self.stop_sign_detected_callback)
# END OF WE CHANGED THIS STUFF
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0, 0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0., 0.]
# Robot limits
self.v_max = 0.2 # maximum velocity
self.om_max = 0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
#self.v_des = 0.2
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
# goal state threshold
self.at_thresh = 0.09 #0.05
self.at_thresh_theta = 0.2 #0.1
# trajectory smoothing
self.spline_alpha = 0.005 #0.01 #0.05 # 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5 # 0.5
self.kpy = 0.5 # 0.5
self.kdx = 1.5 # 1.5
self.kdy = 1.5 # 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
self.kdy, self.v_max,
self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max,
self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path',
Path,
queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
Path,
queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher(
'/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
# ADD CURRENT STATE PUBLISHER (11/15/20 DEM)
self.current_state_pub = rospy.Publisher('/current_state',
Pose2D,
queue_size=10)
print "finished init"
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
#self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
self.cmd_vel = rospy.Publisher('/mobile_base/commands/velocity',
Twist,
queue_size=5)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.15
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 0.5
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(
pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
setSpeed(tcp=100, ori=30)
setZone(0)
# set the parameters
limit = 10000 # number of data points to be collected
ori = [0, 0.7071, 0.7071, 0]
threshold = 0.3 # the threshold force for contact, need to be tuned
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
step_size = 0.0002
obj_des_wrt_vicon = [
0, 0, -(9.40 / 2 / 1000 + 14.15 / 2 / 1000), 0, 0, 0, 1
]
# visualize the block
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
# 0. move to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos, ori)
start_pos = [0.3, 0.06, z]
setCart(start_pos, ori)
curr_pos = start_pos
# 0.1 zero the ft reading
rospy.sleep(1)
setZero()
rospy.sleep(3)
# 1. move in -y direction till contact -> normal
setSpeed(tcp=30, ori=30)
direc = np.array([0, -step_size, 0])
normal = [0, 0, 0]
while True:
curr_pos = np.array(curr_pos) + direc
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = ftmsg2list(
ROS_Wait_For_Msg('/netft_data',
geometry_msgs.msg.WrenchStamped).getmsg())
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link',
'vicon/SteelBlock/SteelBlock',
listener)
# correct box_pose
box_pose_des_global = mat2poselist(
np.dot(poselist2mat(list(box_pos) + list(box_quat)),
poselist2mat(obj_des_wrt_vicon)))
print 'box_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
break
#pause()
# 2. use the normal to move along the block
setSpeed(tcp=20, ori=30)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 2.1 move
direc = np.dot(tfm.euler_matrix(0, 0, 2), normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = getAveragedFT()
print index #, '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link',
'vicon/SteelBlock/SteelBlock',
listener)
# correct box_pose
box_pose_des_global = mat2poselist(
np.dot(poselist2mat(list(box_pos) + list(box_quat)),
poselist2mat(obj_des_wrt_vicon)))
#box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
vizBlock(obj_des_wrt_vicon)
br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7],
rospy.Time.now(), "SteelBlockDesired", "map")
#print 'box_pos', box_pos, 'box_quat', box_quat
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radis
contact_pts.append(contact_pt.tolist())
contact_pt[2] = 0.01
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(), (contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z. Also the normal is in toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] +
(-normal).tolist()[0:2] + [0] +
box_pose_des_global[0:3] +
box_pose_des_global[6:7] +
box_pose_des_global[3:6] + curr_pos.tolist())
index += 1
if len(contact_pts) > limit:
break
if len(
contact_pts
) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=5, ori=30)
setCart(curr_pos + normal * move_away_size, ori)
rospy.sleep(1)
print 'bad ft:', getAveragedFT()
setZero()
rospy.sleep(3)
setCart(curr_pos - normal * overshoot_size, ori)
setSpeed(tcp=20, ori=30)
#all_contact(1:2,:); % x,y of contact position
#all_contact(4:5,:); % x,y contact normal
#all_contact(7:9,:); % box x,y
#all_contact(10:13,:); % box quaternion
#all_contact(14:16,:); % pusher position
# save contact_nm and contact_pt as json file
with open(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.json',
'w') as outfile:
json.dump({
'contact_pts': contact_pts,
'contact_nms': contact_nms
}, outfile)
# save all_contact as mat file
sio.savemat(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.mat',
{'all_contact': all_contact})
setSpeed(tcp=100, ori=30)
# 3. move back to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos, ori)
def __init__(self):
# 设置rospy在终止节点时执行的关闭函数
rospy.on_shutdown(self.shutdown)
# 获取参数
self.goal_distance = rospy.get_param("~goal_distance", 1.0) # meters
self.goal_angle = radians(rospy.get_param(
"~goal_angle", 90)) # degrees converted to radians
self.linear_speed = rospy.get_param("~linear_speed",
0.2) # meters per second
self.angular_speed = rospy.get_param("~angular_speed",
0.7) # radians per second
self.angular_tolerance = radians(
rospy.get_param("~angular_tolerance", 2)) # degrees to radians
self.velocity_topic = rospy.get_param(
'~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel')
self.rate_pub = rospy.get_param('~velocity_pub_rate', 20)
self.rate = rospy.Rate(self.rate_pub)
# 初始化发布速度的topic
self.cmd_vel = rospy.Publisher(self.velocity_topic,
Twist,
queue_size=1)
self.base_frame = rospy.get_param('~base_frame',
'/base_link') #set the base_frame
self.odom_frame = rospy.get_param('~odom_frame',
'/odom') #set the odom_frame
# 初始化tf
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
try:
self.tf_listener.waitForTransform(self.base_frame, self.odom_frame,
rospy.Time(0),
rospy.Duration(5.0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr(
'tf catch exception when robot waiting for transform......')
exit(-1)
# 循环四次画出正方形
for i in range(4):
self.Linear_motion()
# 再进行旋转之前时机器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
self.Rotational_motion()
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
#结束时使机器人停止
self.cmd_vel.publish(Twist())
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.waypoints_2d = None
self.closest_waypoint_id = 0
self.last_car_position = 0
self.last_light_pos_wp = []
self.waypoints_tree = None
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.environment_param = rospy.get_param("/traffic_light_environment")
self.model_path = rospy.get_param("/traffic_light_model_name")
self.detection_threshhold = rospy.get_param(
"/traffic_light_detection_threshhold")
self.max_light_distance = rospy.get_param(
"/traffic_light_max_distance")
self.yellow_light_max_braking_distance = rospy.get_param(
"/yellow_max_braking_distance")
self.yellow_light_min_braking_distance = rospy.get_param(
"/yellow_min_braking_distance")
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.brake_on_yellow = False
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.TrafficLightState = rospy.Publisher('/traffic_light_state',
Int32,
queue_size=1)
self.light_classifier = TLClassifier(self.environment_param,
self.model_path,
self.detection_threshhold)
rospy.spin()
def main():
global settings
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('controller')
controller = Controller()
PID_controller = Pid_Controller()
cfg = Config()
udp = sub_udp()
states = States()
pose_dict = mt.get_csv()
cx = pose_dict['x']
cy = pose_dict['y']
target_course = TargetCourse(cx, cy)
udp.GearNo = -9
udp.VC_SwitchOn = 1
controller.loop = 0
v_dt = 0
distance_old = 0
count = 0
y_line_state = 9
tar_vel_ = 0
Lf_k = 1.6
listener = tf.TransformListener()
# start -----------------------------------------------
while 1:
# cal dt
if controller.car_lasted_time == 0:
controller.car_lasted_time = controller.car_now_time
controller.car_dt_time = controller.car_now_time - controller.car_lasted_time
controller.car_lasted_time = controller.car_now_time
key = getKey()
try:
(trans, rot) = listener.lookupTransform('localization',
'Wheel_Rear',
rospy.Time(0))
print(trans)
print(rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
try:
# use localization
state = State(x=controller.car_local_point_x,
y=controller.car_local_point_y,
yaw=controller.car_local_point_theta,
v=controller.cur_vel)
last_index = len(cx) - 1
target_ind, Ld, now_ind, test_mode, test_ind_1, test_ind_2 = target_course.search_target_index(
state, Lf_k)
cur_vel_ = controller.cur_vel
cur_dt = controller.car_dt_time
except (AttributeError, IndexError) as set_err:
print(set_err)
print("*-*-*-L-O-D-I-N-G-*-*-*")
continue
if last_index > target_ind:
Ks = 0.08
mov_vel = PID_controller.accel_control(tar_vel_, cur_vel_, cur_dt,
1, 0, 1)
udp.Ax = mov_vel
nx = target_course.cx[now_ind]
zx = target_course.cx[0]
ny = target_course.cy[now_ind]
zy = target_course.cy[0]
# pure mode
# delta_gain, tx, ty = pure_pursuit_steer_control(state, target_course, target_ind, Ks)
delta_gain, tx, ty, alpha, delta_rad, delta, alpha_m, delta_m_rad, delta_m, Ld_m, car_alpha_m, car_pr, car_fr, pu_ind, pu_pind = pure_pursuit_steer_control(
state, target_course, target_ind, Ks)
udp.SteeringWheel = delta_gain
print(
"------------------------------------------------------------------------------"
)
print("yaw : ", (controller.cur_yaw) % (np.pi * 2))
print("car v : ", state.v)
print("delta_gain : ", delta_gain)
print("")
print("tx - : ", tx, "ty - : ", ty)
print("Ld _ origin : ", Ld, "Ld _ cal : ", Ld_m)
print("alpha _ origin : ", alpha, "alpha _ cal : ", alpha_m)
print("delta rad _ origin : ", delta_rad, "delta rad _ cal : ",
delta_m_rad)
print("delta _ origin : ", delta, "delta _ cal : ", delta_m)
print("yaw _ origin : ", controller.cur_yaw)
print("yaw _ 360 ", (controller.cur_yaw) % (np.pi * 2))
print("yaw _ - tar : ", car_pr, "yaw _ - cal : ", car_fr,
"yaw - ", car_pr - car_fr)
print("yaw _ - cal : ", car_alpha_m)
tx_tar = target_course.cx[target_ind]
tx_now = target_course.cx[now_ind]
ty_tar = target_course.cy[target_ind]
ty_now = target_course.cy[now_ind]
tx_tar_1 = target_course.cx[now_ind + 1]
ty_tar_1 = target_course.cy[now_ind + 1]
tx_tar_2 = target_course.cx[now_ind + 2]
ty_tar_2 = target_course.cy[now_ind + 2]
tx_tar_3 = target_course.cx[now_ind + 3]
ty_tar_3 = target_course.cy[now_ind + 3]
print("test mode", test_mode, "\n test ind 1", test_ind_1,
"test in 2", test_ind_2)
print(" tx_tar : ", tx_tar, " ty_tar : ", ty_tar)
print(" tx_tar_1 : ", tx_tar_1, " ty_tar_1 : ", ty_tar_1)
print(" tx_tar_2 : ", tx_tar_2, " ty_tar_2 : ", ty_tar_2)
print(" tx_now : ", tx_now, " ty_now : ", ty_now)
print(target_ind - now_ind, "", last_index)
print("")
print("localization1 x : ", controller.car_local_point_x,
"localization1 y : ", controller.car_local_point_y)
print("localization2 x : ", controller.car_local_2_point_x,
"localization2 y : ", controller.car_local_2_point_y)
print("rear x : ", state.rear_x, "rear y : ", state.rear_y)
print("front x : ", state.front_x, "front y : ", state.front_y)
print("lo_1 dx : ", controller.car_local_point_x - tx,
"lo_1 dy : ", controller.car_local_point_y - ty)
print("lo_2 dx : ", controller.car_local_2_point_x - tx,
"lo_2 dy : ", controller.car_local_2_point_y - ty)
dlo_1 = np.hypot(tx - controller.car_local_point_x,
ty - controller.car_local_point_y)
dlo_1_0 = np.hypot(tx_tar - controller.car_local_point_x,
ty_tar - controller.car_local_point_y)
dlo_1_1 = np.hypot(tx_tar_1 - controller.car_local_point_x,
ty_tar_1 - controller.car_local_point_y)
dlo_1_2 = np.hypot(tx_tar_2 - controller.car_local_point_x,
ty_tar_2 - controller.car_local_point_y)
dlo_1_3 = np.hypot(tx_tar_3 - controller.car_local_point_x,
ty_tar_3 - controller.car_local_point_y)
dlo_1_now = np.hypot(tx_now - controller.car_local_point_x,
ty_now - controller.car_local_point_y)
dlo_2 = np.hypot(tx - controller.car_local_2_point_x,
ty - controller.car_local_2_point_y)
dlo_2_0 = np.hypot(tx_tar - controller.car_local_2_point_x,
ty_tar - controller.car_local_2_point_y)
dlo_2_1 = np.hypot(tx_tar_1 - controller.car_local_2_point_x,
ty_tar_1 - controller.car_local_2_point_y)
dlo_2_2 = np.hypot(tx_tar_2 - controller.car_local_2_point_x,
ty_tar_2 - controller.car_local_2_point_y)
dlo_2_3 = np.hypot(tx_tar_3 - controller.car_local_2_point_x,
ty_tar_3 - controller.car_local_2_point_y)
dlo_2_now = np.hypot(tx_now - controller.car_local_2_point_x,
ty_now - controller.car_local_2_point_y)
print("lo_1 d : ", dlo_1_now, dlo_1_0, dlo_1_1, dlo_1_2, dlo_1_3)
print("lo_2 d : ", dlo_2_now, dlo_2_0, dlo_2_1, dlo_2_2, dlo_2_3)
print(" target ind : ", target_ind, " | ", last_index)
print(" now ind : ", now_ind, " | ", last_index)
print(" pure_ cal ind : ", pu_ind, "pu ind : ", pu_pind)
print("")
print("tx : ", tx, "ty : ", ty)
print("nx : ", nx, "ny : ", ny)
print("state x : ", state.x, "state y : ", state.y)
print("distance target x : ", tx - state.x, "distance target y : ",
ty - state.y)
dtar = np.hypot(state.x - tx, state.y - ty)
dnow = np.hypot(state.x - nx, state.y - ny)
print("")
print("distance target : ", dtar, "distance now : ", dnow)
print("Ld _ origin : ", Ld, "Ld _ cal : ", Ld_m)
# ----------------------------------------------------------------------------------
controller.loop += 1
# draw plot
v_dt += cur_dt
states.append(v_dt, state)
# -------------------------------------------------------------------------------------
# drawing plot
if cfg.show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)
if cfg.show_animation_gain:
plt.cla()
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.grid(True)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.legend()
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.axis("equal")
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.grid(True)
plt.pause(0.00001)
#
else:
print("lastIndex < target_ind")
print("*-*-*-*-*-*-END-*-*-*-*-*-*")
# ----------------------------------------------------
# set target V
if key == '0':
udp.VC_SwitchOn = 0
print('maneuver')
if key == '1':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 10
if key == '2':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 20
if key == '3':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 30
if key == '4':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 40
if key == '5':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 50
if key == '9':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 1
if key == '8':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 3
if key == '7':
udp.VC_SwitchOn = 1
udp.GearNo = 1
tar_vel_ = 5
if key == 'w':
udp.VC_SwitchOn = 1
if udp.Ax >= 0:
udp.GearNo = 1
else:
udp.GearNo = -1
udp.Ax += 1
if key == 'a':
udp.VC_SwitchOn = 1
udp.SteeringWheel += 0.1
# mode_pub.publish(udp)
print('SteeringWheel +')
if key == 'd':
udp.VC_SwitchOn = 1
udp.SteeringWheel -= 0.1
# mode_pub.publish(udp)
print('SteeringWheel -')
if key == 'e':
udp.VC_SwitchOn = 1
udp.SteeringWheel = 0.0
# mode_pub.publish(udp)
print('SteeringWheel 0')
if key == 's':
udp.VC_SwitchOn = 1
if udp.Ax >= 0:
udp.GearNo = 1
else:
udp.GearNo = -1
udp.Ax -= 1
# mode_pub.publish(udp)
print('Ax -')
# Brake
if key == 'c':
udp.VC_SwitchOn = 1
udp.GearNo = 0
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 0.5')
if key == 'x':
udp.VC_SwitchOn = 1
udp.GearNo = -9
udp.Ax = 0
udp.SteeringWheel = 0
print('stop - 1')
else:
if (key == '\x03'):
break
controller.car_pub.publish(udp)
# draw plot
try:
if controller.show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(states.x, states.y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(states.t, [iv for iv in states.v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
except AttributeError:
print("*-*-*-n-o-d-a-t-a-*-*-*")
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
config_string = rospy.get_param("/traffic_light_config")
# Folder of where images are stored.
self.dirData = rospy.core.rospkg.environment.get_test_results_dir()
self.config = yaml.load(config_string)
self.stop_line_positions = self.config['stop_line_positions']
self.stop_line_waypoints = []
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.idx = 0
self.waypointlist = np.array([])
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.ego_x = None
self.ego_y = None
self.closest_wp_index = None
self.num_waypoints = 0
self.light_wp = -1
self.state_red_count = -1
self.tree = []
self.image_count = 0
self.waypoints_prepared = False
self.idx_traffic_light_found = False
self.last_pose = None
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.gpu_ready_pub = rospy.Publisher('/gpu_ready', Int32, queue_size=1)
# Philippe
# sub6 = rospy.Subscriber('/image_color', Image, self.image_callback)
# self.light_classifier = TLClassifier(deep_learning=False)
# self.gpu_ready_pub.publish(Int32(1))
# self.loop()
# Peter and Mikkel
sub6 = rospy.Subscriber('/image_color', Image, self.image_callback)
self.light_classifier = TLClassifier(deep_learning=True)
self.gpu_ready_pub.publish(Int32(1))
self.loop2()
