def get_carts_in_range(cls, border_a, border_b, msg_laser, front_side):
start_id = 0
carts_in_range = []
x_or_y = 1 if front_side else 0
#if border_a >= 0:
# start_id = len(msg_laser.ranges)/2
for i in range(start_id, len(msg_laser.ranges)):
#if front_side == False:
cart_coordinate = Utils.get_cart_from_id(i, msg_laser)
if front_side == False and cart_coordinate[1] < 0.0 and DEBUG:
pass
#rospy.logdebug("x:=%s (>= border_b): %s :: (<= border_a:) %s :: cart_coordinate= %s", cart_coordinate[x_or_y], cart_coordinate[x_or_y] >= border_b, cart_coordinate[x_or_y] <= border_a, cart_coordinate)
if cart_coordinate[x_or_y] >= border_b and cart_coordinate[x_or_y] <= border_a: #cart_coordinate[1] := y distance
if front_side:
carts_in_range.append(cart_coordinate)
else:
if cart_coordinate[1] > 0.0:
carts_in_range.append(cart_coordinate)
#if cart_coordinate[x_or_y] >= border_a:
# break
if front_side == False and DEBUG:
Utils.publish_marker(carts_in_range[0][0],carts_in_range[0][1], 0)
last_id = len(carts_in_range) -1
Utils.publish_marker(carts_in_range[last_id][0],carts_in_range[last_id][1], 5)
rospy.logdebug("border_a= %f, border_b= %f, front_side= %s,x_or_y = %i",border_a, border_b, front_side, x_or_y)
rospy.logdebug("carts_in_range= %s", carts_in_range)
return carts_in_range
def execute(self,userdata):
global locomotionGoal
#global movement_client
#rospy.loginfo('Executing state VISION_PROCESSING')
self.client = actionlib.SimpleActionClient('LocomotionServer', bbauv_msgs.msg.ControllerAction)
self.client.wait_for_server()
self.client.cancel_all_goals()
while(not rospy.is_shutdown()):
if(len(gate.centroidx) == 2):
target = (gate.centroidx[0] + gate.centroidx[1])/2 - 640*0.5
if(len(gate.centroidx)==2):
centroid_dist = fabs(gate.centroidx[0] - gate.centroidx[1])
if(target>0 and fabs(target) >10):
self.sidemove = centroid_dist*self.Kp
elif(target<0 and fabs(target) >10):
self.sidemove = -centroid_dist*self.Kp
else:
self.sidemove = 0
self.client.cancel_all_goals()
locomotionGoal.sidemove_setpoint = 0
userdata.complete_output = True
try:
resp = mission_srv_request(False,True,locomotionGoal)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return 'task_complete'
rospy.logdebug("target:" + str(target) + "sidemove:" + str(self.sidemove))
if(not self.isCorrection):
rospy.loginfo("vision action issued!")
goal = bbauv_msgs.msg.ControllerGoal(forward_setpoint=2,heading_setpoint=locomotionGoal.heading_setpoint,depth_setpoint=locomotionGoal.depth_setpoint,sidemove_setpoint=self.sidemove)
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration(2))
self.isCorrection = True
def dur_cb(self,data):
secs = data.data.secs
nsecs = data.data.nsecs
rospy.logdebug("Tablet manager got: (" + str(secs) + "," + str(nsecs) + ")")
topic = data._connection_header["topic"].strip('/')
self.subs[topic]["pressed"] = True
self.subs[topic]["last_press"] = data.data
def found_people_handler(self, loc):
if not self.is_following:
return
x = loc.x
y = loc.y
if x == 0 and y == 0:
result = FollowResult()
rospy.loginfo('[Simple follow] lost')
result.last_found_loc.point = self.last_loc
result.last_found_loc.header.stamp = self.last_time
result.last_found_loc.header.frame_id = 'base_link'
self.follow_server.set_succeeded(result)
self.is_following = False
self.move_to(0, 0, 0)
pose = PoseStamped()
pose.header.seq = self.seq
self.seq += 1
pose.header.frame_id = 'base_link'
self.last_loc = loc
self.last_time = rospy.Time.now()
rospy.logdebug('x at %f, y at %f' % (x, y))
if x*x + y*y < 4:
rospy.logdebug('found human, too close')
return
self.move_to(x, y, -2)
def wait_for_press(self,topic, value=None):
rospy.logdebug("Tablet manager waiting on topic: " + str(topic) + " for value: " + str(value))
topic = topic.strip('/')
if value == None:
self.subs[topic]["pressed"] = False
while not self.subs[topic]["pressed"]:
if rospy.is_shutdown():
return
rospy.sleep(0.1)
ret = self.subs[topic]["last_press"]
else:
#don't return right away if the value we are waiting for was the
#last value pressed before we started waiting
self.subs[topic]["pressed"] = False
while not self.subs[topic]["pressed"]:
if rospy.is_shutdown():
return
rospy.sleep(0.1)
#now we can just wait until the last_press value is what we want
while not self.subs[topic]["last_press"] == value:
if rospy.is_shutdown():
return
rospy.sleep(0.1)
ret = self.subs[topic]["last_press"]
return ret
def twistUpdate(self):
if self.goal is None:
w = 0
v = 0
else:
errX = self.goal.position.x-self.pose.position.x
errY = self.goal.position.y-self.pose.position.y
if errX*errX<0.01 and errY*errY<0.01:
self.goal = None
w = 0
v = 0
errYaw = 0
else:
euler = tf.transformations.euler_from_quaternion([self.pose.orientation.x,self.pose.orientation.y,self.pose.orientation.z,self.pose.orientation.w])
goalYaw = numpy.arctan2(errY,errX)
errYaw = goalYaw-euler[2]
errYaw = numpy.arctan2(numpy.sin(errYaw),numpy.cos(errYaw))
w = -self.Kp*errYaw
if abs(w)>self.w_max:
w = self.w_max*(abs(w)/w)
if abs(errYaw)<0.75:
v = self.v_max / ( abs(w) + 1 )**0.5
else:
v = 0
rospy.logdebug("%s pX:%f pY:%f eX:%f eY:%f eYaw:%f -> w:%f v:%f"
, self.nodeName,self.pose.position.x,self.pose.position.y,errX,errY,errYaw,w,v
)
self.twist.linear.x = v
self.twist.angular.z = w
def _test_sel_index(self, selected, deselected):
"""
Method for Debug only
"""
#index_current = self.selectionModel.currentIndex()
src_model = self._item_model
index_current = None
index_deselected = None
index_parent = None
curr_qstd_item = None
if selected.indexes():
index_current = selected.indexes()[0]
index_parent = index_current.parent()
curr_qstd_item = src_model.itemFromIndex(index_current)
elif deselected.indexes():
index_deselected = deselected.indexes()[0]
index_parent = index_deselected.parent()
curr_qstd_item = src_model.itemFromIndex(index_deselected)
if selected.indexes() > 0:
rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format(
index_current, index_parent, index_deselected,
index_current.data(Qt.DisplayRole),
index_parent.data(Qt.DisplayRole),)
+ ' desel.d={} cur.item={}'.format(
None, # index_deselected.data(Qt.DisplayRole)
curr_qstd_item))
elif deselected.indexes():
rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format(
index_current, index_parent, index_deselected,
None, index_parent.data(Qt.DisplayRole)) +
' desel.d={} cur.item={}'.format(
index_deselected.data(Qt.DisplayRole),
curr_qstd_item))
def __init__(self, sensor_data, robot_state, slam_map):
self.data = []
cells = set()
for i in xrange(sensor_data.scan_size):
if i < RobotParams.detectionMargin or sensor_data.scan_size - RobotParams.detectionMargin < i:
continue
if sensor_data.scan_data[i] == -1:
continue
dist, val = (sensor_data.scan_data[i], Map.OBSTACLE)
if dist == 0:
dist, val = (sensor_data.max_distance, Map.NO_OBSTACLE)
if dist < RobotParams.holeWidth / 2.0:
continue
angle_offset = i * (sensor_data.angle_max - sensor_data.angle_min) / (sensor_data.scan_size - 1)
angle = sensor_data.angle_min + angle_offset
# TODO: derivation & to radians
#angle_deg += robot_state.angular_v * angle_offset / 3600.0
x = dist * math.cos(angle)
# TODO: derivation & to radians
#x -= robot_state.linear_v * 1000 * angle_offset / 3600.0
y = dist * math.sin(angle)
cell = slam_map.to_cell(x, y)
if cell in cells:
continue
cells.add(cell)
self.data.append((x, y, val))
rospy.logdebug("[ScanInit] Hit rate: %.3f", 1.0 - 1.0*len(cells)/sensor_data.scan_size)
def execute(self, user_data):
rospy.logdebug('SynchronousSubscribeState base class "execute" running')
self._done = False
self._outcome = None
self._user_data = user_data
self._sub1 = message_filters.Subscriber(self._topic1, self._msg_type1)
self._sub2 = message_filters.Subscriber(self._topic2, self._msg_type2)
self._sub = message_filters.ApproximateTimeSynchronizer(
[self._sub1, self._sub2], 10, self._sync_time)
self._sub.registerCallback(self._sync_callback)
if self._setup_callback:
self._setup_callback()
start_time = rospy.Time.now()
while not rospy.is_shutdown():
if self._timeout and rospy.Time.now() - start_time > self._timeout:
self._outcome = 'timeout'
break
if self._done is True:
break
else:
self._poll_rate.sleep()
self._sub1.unregister()
self._sub2.unregister()
return self._outcome
def doPid(self):
#####################################################
pid_dt_duration = rospy.Time.now() - self.prev_pid_time
pid_dt = pid_dt_duration.to_sec()
self.prev_pid_time = rospy.Time.now()
self.error = self.target - self.vel
self.integral = self.integral + (self.error * pid_dt)
# rospy.loginfo("i = i + (e * dt): %0.3f = %0.3f + (%0.3f * %0.3f)" % (self.integral, self.integral, self.error, pid_dt))
self.derivative = (self.error - self.previous_error) / pid_dt
self.previous_error = self.error
self.motor = (self.Kp * self.error) + (self.Ki * self.integral) + (self.Kd * self.derivative)
if self.motor > self.out_max:
self.motor = self.out_max
self.integral = self.integral - (self.error * pid_dt)
if self.motor < self.out_min:
self.motor = self.out_min
self.integral = self.integral - (self.error * pid_dt)
if (self.target == 0):
self.motor = 0
rospy.logdebug("vel:%0.2f tar:%0.2f err:%0.2f int:%0.2f der:%0.2f ## motor:%d " %
(self.vel, self.target, self.error, self.integral, self.derivative, self.motor))
def __cleanup_graph(self):
"""Iterate over the running capabilities and shutdown ones which are no longer needed
For each running capability, if it was not started by the user then look at who depends on it.
If no other capabilities depend on it, then shut it down.
"""
# Collect all running capabilities
running_capabilities = [x
for x in self.__capability_instances.values()
if x.state == 'running']
for cap in running_capabilities:
if cap.started_by == USER_SERVICE_REASON:
# Started by user, do not garbage collect this
continue
rdepends = get_reverse_depends(cap.interface, self.__capability_instances.values())
if rdepends:
# Someone depends on me, do not garbage collect this
rospy.logdebug("Keeping the '{0}' provider of the '{1}' interface, ".format(cap.name, cap.interface) +
"because other running capabilities depend on it.")
continue
if cap.state == 'running':
rospy.loginfo("Stopping the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
self.__stop_capability(cap.interface)
elif cap.state == 'waiting': # pragma: no cover
rospy.loginfo("Canceling the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
cap.cancel()
def _load_launchfile_slot(self, launchfile_name):
# Not yet sure why, but everytime combobox.currentIndexChanged occurs,
# this method gets called twice with launchfile_name=='' in 1st call.
if launchfile_name == None or launchfile_name == '':
return
_config = None
rospy.logdebug('_load_launchfile_slot launchfile_name={}'.format(
launchfile_name))
try:
_config = self._create_launchconfig(launchfile_name,
self._port_roscore)
#TODO: folder_name_launchfile should be able to specify arbitrarily
# _create_launchconfig takes 3rd arg for it.
except IndexError as e:
msg = 'IndexError={} launchfile={}'.format(e.message,
launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
except RLException as e:
msg = 'RLException={} launchfile={}'.format(e.message,
launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
self._create_widgets_for_launchfile(_config)
def calcVelocity(self):
#####################################################
self.dt_duration = rospy.Time.now() - self.then
self.dt = self.dt_duration.to_sec()
rospy.logdebug("-D- %s caclVelocity dt=%0.3f wheel_latest=%0.3f wheel_prev=%0.3f" % (self.nodename, self.dt, self.wheel_latest, self.wheel_prev))
if (self.wheel_latest == self.wheel_prev):
# we haven't received an updated wheel lately
cur_vel = (1 / self.ticks_per_meter) / self.dt # if we got a tick right now, this would be the velocity
if abs(cur_vel) < self.vel_threshold:
# if the velocity is < threshold, consider our velocity 0
rospy.logdebug("-D- %s below threshold cur_vel=%0.3f vel=0" % (self.nodename, cur_vel))
self.appendVel(0)
self.calcRollingVel()
else:
rospy.logdebug("-D- %s above threshold cur_vel=%0.3f" % (self.nodename, cur_vel))
if abs(cur_vel) < self.vel:
rospy.logdebug("-D- %s cur_vel < self.vel" % self.nodename)
# we know we're slower than what we're currently publishing as a velocity
self.appendVel(cur_vel)
self.calcRollingVel()
else:
# we received a new wheel value
cur_vel = (self.wheel_latest - self.wheel_prev) / self.dt
self.appendVel(cur_vel)
self.calcRollingVel()
rospy.logdebug("-D- %s **** wheel updated vel=%0.3f **** " % (self.nodename, self.vel))
self.wheel_prev = self.wheel_latest
self.then = rospy.Time.now()
self.pub_vel.publish(self.vel)
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
self._nav.inner_led = value
self._nav.outer_led = value
def _handle_focus(self, req):
taskdata = req.taskdata
rospy.loginfo('Executing state FocusObject')
rospy.loginfo('Trying to focus %s' % taskdata.object_to_focus.object.id)
taskdata.focused_object = None
centroid = taskdata.object_to_focus.c_centroid
centroid.z -= 0.03
poses = calc_grasp_position.make_scan_pose(centroid, *utils.focus_poses[self._ctr])
utils.manipulation.set_planning_time_arm(2)
move_method = utils.manipulation.move_to
transition = ""
for pose in poses:
if move_method(pose):
taskdata.focused_object = taskdata.object_to_focus
rospy.logdebug('Wait for clock')
time.sleep(1)
rospy.logdebug('Wait for tf again.')
rospy.sleep(4)
self._ctr = 0
transition = self.RETURN_VAL_SUCSS
if transition == "":
utils.manipulation.set_planning_time_arm(5)
if self._ctr < 6:
self._ctr += 1
transition = self.RETURN_VAL_RETRY
else:
self._ctr = 0
transition = self.RETURN_VAL_FAIL
return TaskDataServiceResponse(taskdata = taskdata, result = transition)
def transition_cb(self, userdata, active_states):
"""Transition callback passed to state machine.
This method is called each time the state machine transitions.
"""
rospy.logdebug("Publishing action feedback.")
# Publish action feedback
self.publish_feedback(userdata)
def build_blocks(self, M_robot):
"""
Input:
- M_robot: A calibration sample of type calibration_msgs/RobotMeasurement
Returns:
- sensors: A list of CameraChainSensor objects that corresponds to all camera chain sensors found
in the calibration message that was passed in.
"""
sensors = []
for cur_config in self._valid_configs:
if cur_config["camera_id"] in [ x.camera_id for x in M_robot.M_cam ]:
if cur_config["chain"]["chain_id"] in [ x.chain_id for x in M_robot.M_chain ] :
# print "if cur_config[chain][chain_id]: ", cur_config["chain"]["chain_id"]
M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["camera_id"])]
M_chain = M_robot.M_chain[ [ x.chain_id for x in M_robot.M_chain ].index(cur_config["chain"]["chain_id"]) ]
# elif cur_config["chain"]["chain_id"] == 'NULL':
elif cur_config["chain"]["chain_id"] == None:
# print "elif cur_config[chain][chain_id]: ", cur_config["chain"]["chain_id"]
M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["camera_id"])]
M_chain = None
else:
print "else cur_config[chain][chain_id]: ", cur_config["chain"]["chain_id"]
break
cur_sensor = CameraChainSensor(cur_config, M_cam, M_chain)
sensors.append(cur_sensor)
else:
rospy.logdebug(" Didn't find block")
return sensors
def get_angle_and_position(self, laser_msg):
# detection
angle = laser_msg.angle_min
last_change_idx=1
self.min_object_size=100
detected_object=[-1,-1]
for i in range(0,len(laser_msg.ranges)):
if (angle > -self.angle_threshold) and (angle < self.angle_threshold): # in front of the robot
if (abs(laser_msg.ranges[i] - previous_range) > self.change_threshold): # change found!
if self.check_new_change(laser_msg, last_change_idx, i-1):
detected_object = [last_change_idx, i-1]
last_change_idx = i
rospy.logdebug( str(angle) + ": " + str(laser_msg.ranges[i]) )
previous_range = laser_msg.ranges[i]
angle += laser_msg.angle_increment
if (detected_object[0] == -1):
return [0,0]
winner_min_angle = laser_msg.angle_min + (detected_object[0] * laser_msg.angle_increment)
winner_max_angle = laser_msg.angle_min + (detected_object[1] * laser_msg.angle_increment)
rospy.logdebug( "Winner was from " + str(winner_min_angle) + " to " + str(winner_max_angle))
index_mid = int((detected_object[0] + detected_object[1])/2)
distance = laser_msg.ranges[index_mid]
angle_mid = (winner_max_angle + winner_min_angle)/2
rospy.loginfo( "Distance is " + str(distance) + " - Angle is " + str(angle_mid))
return [distance, angle_mid]
def update(self, name, uri, discoverer_name, monitoruri, timestamp):
'''
Sets a request to synchronize the local ROS master with this ROS master.
@note: If currently a synchronization is running this request will be ignored!
@param name: the name of the ROS master synchronized with.
@type name: C{str}
@param uri: the URI of the ROS master synchronized with
@type uri: C{str}
@param discoverer_name: the name of the discovery node running on ROS master synchronized with.
@type discoverer_name: C{str}
@param monitoruri: The URI of RPC server of the discovery node to get the ROS master state by calling a method only once.
@type monitoruri: C{str}
@param timestamp: The timestamp of the current state of the ROS master info.
@type timestamp: C{float64}
'''
# rospy.logdebug("SyncThread[%s]: update request", self.name)
with self.__lock_intern:
if (self.timestamp_local != timestamp):
rospy.logdebug("SyncThread[%s]: update notify new timestamp(%.9f), old(%.9f)", self.name, timestamp, self.timestamp_local)
self.name = name
self.uri = uri
self.discoverer_name = discoverer_name
self.monitoruri = monitoruri
self.timestamp_remote = timestamp
self._request_update()
def __catch_and_log(self, func, *args, **kwargs):
try:
return func(*args, **kwargs)
except Exception as exc:
rospy.logdebug(traceback.format_exc())
rospy.logerr('{0}: {1}'.format(exc.__class__.__name__, str(exc)))
raise
def accelerate(self, dv):
"accelerate dv meters/second^2"
rospy.logdebug('acceleration: ' + str(dv))
self.car_ctl.acceleration = dv
# update speed
dv *= 0.05 # scale by cycle duration (dt)
vabs = abs(self.car_ctl.speed)
# never shift gears via acceleration, stop at zero
if -dv > vabs:
vabs = 0.0
else:
vabs += dv
if self.car_ctl.gear.value == Gear.Drive:
self.car_ctl.speed = vabs
elif self.car_ctl.gear.value == Gear.Reverse:
self.car_ctl.speed = -vabs
else: # do nothing in Park
self.car_ctl.speed = 0.0
self.car_ctl.acceleration = 0.0
# never exceeded forward or reverse speed limits
self.car_ctl.speed = clamp(self.minspeed,
self.car_ctl.speed,
self.maxspeed)
def some_input_callback(self, msg):
rospy.logdebug('Got input: ' + str(msg))
self.latest_input = msg
if self.latest_input.data > self.param_one:
self.bool_topic_pub.publish(Bool(True))
else:
self.bool_topic_pub.publish(Bool(False))
def __init__(self, client=None, listener=None):
rospy.logdebug("ArmPlanner Init")
print "ArmPlanner Init"
if client == None:
self.listener = listener
self.pr2_arm = PR2Arm_Planning(ARM, self.listener)
# ARD: uncomment next line
rospy.loginfo("ArmPlanner pr2lite_move_right_arm")
self.move_arm_client = actionlib.SimpleActionClient("pr2lite_move_" + ARM + "_arm", MoveArmAction)
service = ARM[0] + "_gripper_controller/gripper_action"
rospy.loginfo("Gripper wait_for_server")
self.gripper = actionlib.SimpleActionClient(service, Pr2GripperCommandAction)
self.gripper.wait_for_server()
# rospy.init_node('single_joint_position', anonymous=True)
# self.tuck_server = tuck_arm()
self.tuck_server = tuck_arm()
self.torque = dyno_torque()
self.torso_client = actionlib.SimpleActionClient(
"torso_controller/position_joint_action", SingleJointPositionAction
)
self.torso_client.wait_for_server()
self.block_client = actionlib.SimpleActionClient(
"interactive_manipulation", InteractiveBlockManipulationAction
)
rospy.loginfo("ArmPlanner wait for interactive manip")
self.block_client.wait_for_server()
# self.block_client.block_size = 0.03
else:
self.move_arm_client = client
self.success = True
# setup tf for translating poses
rospy.loginfo("ArmPlanner Init done")
def accelerate(self, dv):
"accelerate dv meters/second^2"
rospy.logdebug('acceleration: ' + str(dv))
self.car_ctl.acceleration = abs(dv)
# update speed
dv *= 0.05 # scale by cycle duration (dt)
vabs = abs(self.car_ctl.speed)
# never shift gears via acceleration, stop at zero
if -dv > vabs:
vabs = 0.0
else:
vabs += dv
# never exceeded forward or reverse speed limits
if self.car_ctl.gear.value == Gear.Drive:
if vabs > self.limit_forward:
vabs = self.limit_forward
elif self.car_ctl.gear.value == Gear.Reverse:
if vabs > self.limit_reverse:
vabs = self.limit_reverse
else: # do nothing in Park or Neutral
vabs = 0.0
self.car_ctl.acceleration = 0.0
self.car_ctl.speed = vabs
def getPose(self, col, rank, board, z=0):
# x_fudge = 5 * SQUARE_SIZE
# y_fudge = 4 * SQUARE_SIZE
x_fudge = 0 * SQUARE_SIZE
y_fudge = 0 * SQUARE_SIZE
""" Find the reach required to get to a position """
rospy.logdebug("ArmPlanner getPose")
p = Pose()
if board.side == board.WHITE:
# p.position.x = (col * SQUARE_SIZE) + SQUARE_SIZE/2
# p.position.x = ((col-4.5) * SQUARE_SIZE)
# p.position.y = -1*(((rank-1) * SQUARE_SIZE) + SQUARE_SIZE/2)
p.position.x = (col * SQUARE_SIZE) + SQUARE_SIZE / 2
p.position.y = ((rank - 1) * SQUARE_SIZE) + SQUARE_SIZE / 2
p.position.z = z
else:
p.position.x = ((7 - col) * SQUARE_SIZE) + SQUARE_SIZE / 2
p.position.y = ((8 - rank) * SQUARE_SIZE) + SQUARE_SIZE / 2
p.position.z = z
# p.position.x = ((7-col) * SQUARE_SIZE) + SQUARE_SIZE/2
# p.position.y = ((8-rank) * SQUARE_SIZE) + SQUARE_SIZE/2
# p.position.x = ((4.5-col) * SQUARE_SIZE)
p.position.x += x_fudge
p.position.y += y_fudge
return p
def build_blocks(self, M_robot):
"""
Input:
- M_robot: A calibration sample of type calibration_msgs/RobotMeasurement
Returns:
- sensors: A list of CameraChainSensor objects that corresponds to all camera chain sensors found
in the calibration message that was passed in.
"""
sensors = []
for cur_config in self._valid_configs:
if cur_config["sensor_id"] in [ x.camera_id for x in M_robot.M_cam ]:
if "chain_id" in cur_config.keys() and cur_config["chain_id"] != None:
if cur_config["chain_id"] in [ x.chain_id for x in M_robot.M_chain ] :
M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["sensor_id"])]
M_chain = M_robot.M_chain[ [ x.chain_id for x in M_robot.M_chain ].index(cur_config["chain_id"]) ]
else:
continue
else:
M_cam = M_robot.M_cam [ [ x.camera_id for x in M_robot.M_cam ].index(cur_config["sensor_id"])]
M_chain = None
cur_config["chain_id"] = None
cur_sensor = CameraChainSensor(cur_config, M_cam, M_chain)
sensors.append(cur_sensor)
else:
rospy.logdebug(" Didn't find block")
return sensors
def __init__(self):
if rospy.has_param('joint_names'):
self.joint_names = rospy.get_param('joint_names')
else:
rospy.logerror("joint_names not available")
return
self.configuration = [0,0,0,0,0,0,0]
self.lock = threading.Lock()
self.received_state = False
self.listener = tf.TransformListener()
time.sleep(0.5)
self.service = rospy.Service("move_cart_abs", MoveCart, self.cbIKSolverAbs)
#self.service = rospy.Service("move_cart_rel", MoveCart, self.cbIKSolverRel)
self.client = actionlib.SimpleActionClient('joint_trajectory_action', JointTrajectoryAction)
self.as_ = actionlib.SimpleActionServer("move_cart_rel", MoveCartAction, execute_cb=self.cbIKSolverRel)
if not self.client.wait_for_server(rospy.Duration(15)):
rospy.logerr("arm action server not ready within timeout, aborting...")
return
else:
rospy.logdebug("arm action server ready")
rospy.Subscriber('/joint_states', JointState, self.joint_states_callback)
#self.thread = threading.Thread(target=self.joint_states_listener)
#self.thread.start()
rospy.wait_for_service('get_ik')
self.iks = rospy.ServiceProxy('get_ik', GetPositionIK)
def SendScore(self, score):
rospy.logdebug("BmBox.SendScore()")
if not self._fsm.check_state(BmBoxState.READY): return
self._fsm.update(BmBoxState.TRANSMITTING_SCORE, score)
self._fsm.wait_transition(BmBoxState.TRANSMITTING_SCORE, None)
def record_trigger(self, trigger):
#print "record_trigger: name=", trigger.name, "trigger=", trigger.trigger, "stamp=", trigger.stamp
if trigger.name not in self.config["test_config"]:
raise ATFRecorderError("Testblock '%s' not in test config" % trigger.name)
# Send message to all recorder plugins
#print "self.recorder_plugin_list=", self.recorder_plugin_list
for recorder_plugin in self.recorder_plugin_list:
#FIXME: need to filter the topics not needed for current trigger
recorder_plugin.trigger_callback(trigger)
rospy.logdebug(" recorder plugin callback : '%s'", trigger.name)
# Only process message if testblock requests topics
#print "self.testblock_list=", self.testblock_list
if trigger.name in self.testblock_list:
if trigger.trigger == TestblockTrigger.START:
self.active_sections.append(trigger.name)
self.add_requested_topics(trigger.name)
elif trigger.trigger == TestblockTrigger.STOP or trigger.trigger == TestblockTrigger.PAUSE:
self.active_sections.remove(trigger.name)
self.remove_requested_topics(trigger.name)
# elif trigger.trigger == TestblockTrigger.ERROR:
# self.topic_pipeline = []
else:
rospy.loginfo("!!!!!!!!!!!!")
self.bag_file_writer.write_to_bagfile(self.ns + trigger.name + "/trigger", trigger, trigger.stamp)
def tf_write(self, transf):
# writes tf messages on the bagfile
rospy.logdebug("Tf_writer")
self.tfa.lock()
# loops through the transforms dictionaries and get the current transforms
# for the specific frames and further records the messages on the bagfile
for t in transf:
for j in transf[t]:
# rospy.loginfo(t)
# rospy.loginfo(j)
trans, rot = self.tfL.lookupTransform(t, j, rospy.Time(0))
self.tfposed.header.frame_id = t
self.tfposed.header.stamp = rospy.Time.now()
self.tfposed.child_frame_id = j
self.tfposed.transform.translation.x = trans[0]
self.tfposed.transform.translation.y = trans[1]
self.tfposed.transform.translation.z = trans[2]
self.tfposed.transform.rotation.x = rot[0]
self.tfposed.transform.rotation.y = rot[1]
self.tfposed.transform.rotation.z = rot[2]
self.tfposed.transform.rotation.w = rot[3]
tfMsg = tfMessage([self.tfposed])
bagfile.write("/tf", tfMsg)
self.tfa.unlock()
def process_subcam(image):
""" process the subcam image """
if not SCHEDULER.is_subcam_enable():
return
if SCHEDULER.is_subcam_occupied() or TURTLE.is_occupied():
return
info = EYE.see_sub(image)
if SCHEDULER.debug_option["show_timer"]:
# Check delay only if has line
# SCHEDULER.check_time("subcam", min=0.28, stop_when_delay=info["has_line"])
SCHEDULER.check_time("subcam", min=0.28, stop_when_delay=False)
if info is None:
rospy.logwarn("[PROC] No Information!")
return
center = info["center"]
slope = info["slope"]
if slope < -0.5:
limit = 1.6
amplitude = 1.0
else:
limit = 1.2
amplitude = 0.8
limit /= 1.9
# amplitude /= 2
state = SCHEDULER.get_state()
if (EYE.get_front_state() == "straight") and (state is not "zigzag"):
if (abs(center) < 30) and slope < -0.4:
degree = pow(abs(slope) / 1.8, 1.1) * amplitude
elif center < 0:
degree = pow(abs(center) / 100, 2.0) * amplitude / 2
elif center > 0:
degree = - pow(abs(center) / 100, 2.0) * amplitude
else:
degree = 0
else:
if slope < 0:
degree = - pow(abs(slope) / 1.8, 2.9) * amplitude * 28.0
else:
degree = pow(abs(slope) / 1.0, 1.2) * amplitude * 4.0
# degree = pow(abs(slope) / 1.8, 0.9) * amplitude
# elif center < 0:
# degree = pow(abs(center) / 100, 1.9) * amplitude
# elif center > 0:
# degree = - pow(abs(center) / 100, 1.9) * amplitude
# else:
# degree = 0
buf_sum = sum(BUF_ANGULAR)
if EYE.get_front_state() == "straight":
adjust_angular = BUF_ANGULAR.pop(0) * 0.9
BUF_ANGULAR.append(degree)
degree -= adjust_angular
# if abs(buf_sum) > 1:
else:
reset_buffer()
adjust_angular = 0
degree = max(min(degree, limit), -limit)
if not info["has_line"]:
if center > 200:
# degree = -1.2
degree = -1.1 # For enhancing frequency
# elif EYE.get_front_state() == "straight":
# degree = 0.6
else:
# degree = 1.4
degree = 1.3 # For enhancing frequency
# if not info["has_line"]:
# if center < -55:
# # degree = 1.6
# degree = 1.4 # For slow speed
# elif center > 50:
# degree = -1.2
# # degree = -1.2 # For slow speed
# # elif center > 19:
# # degree = -1.2
# else:
# degree = 1.4
if SCHEDULER.debug_option["show_center_slope"]:
rospy.logdebug(
"[PROC] center: {:.2f} slope: {:.2f} degree: {:.2f} adj: {:.2f} buf_sum: {:.2f} {} {}".format(
center, slope, degree, adjust_angular, buf_sum, EYE.get_front_state(), info["has_line"])
)
rospy.Timer(
rospy.Duration(0.15), SCHEDULER.release_subcam_occupied, oneshot=True
)
TURTLE.turn(0.13, degree)
def waypoints_cb(self, lane):
rospy.logdebug("total base waypoints %s", len(lane.waypoints))
self.latest_waypoints = lane.waypoints
def move_base(self, action, horizon, update_rate=100, init= False):
import tf
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id="world"
if init:
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8
t = np.random.choice(63,1);
outPose.position.x = r*np.cos(self.theta[t[0]])
outPose.position.y = r*np.sin(self.theta[t[0]])
outPose.position.z = -r
else:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', rospy.Time(0))
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
homogeneous_matrix = tf.transformations.euler_matrix(0,0,y,axes='sxyz')
homogeneous_matrix[0:3,3] = trans
yaw = y + action[2]
outPose.POI.x = trans[0]+2*np.cos(yaw)
outPose.POI.y = trans[1]+2*np.sin(yaw)
outPose.POI.z = 1
rospy.logwarn("END ACTION ==>"+str(action))
gt_odom = self.get_gt_odom()
# Using MPC
# goal_MPC = np.array([self.mpc_command_data.position.x,self.mpc_command_data.position.y, self.mpc_command_data.position.z, 1])
# action_MPC = np.dot(np.linalg.inv(homogeneous_matrix),goal_MPC)
# if np.isnan(action_MPC).any():
# firefly_odom = self.get_firefly_odom()
# action_MPC=np.array([gt_odom.position.x,gt_odom.position.y,gt_odom.position.z])
act = []
act.extend([action[0],action[1]])
act.extend([0])
if self.robotID==1:
goal = homogeneous_matrix.dot(np.concatenate((np.array(act),np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((np.array(action)+np.array([action_MPC[0],action_MPC[1],action_MPC[2]]),np.array([1]))))
# goal = homogeneous_matrix.dot(np.concatenate((action_MPC[0:3],np.array([1]))))
# rospy.logwarn("END ACTION MPC==>"+str(action_MPC))
rospy.logwarn("END ACTION==>"+str(action))
outPose.position.x = goal[0]
outPose.position.y = goal[1]
outPose.position.z = -8
rospy.logwarn("Current MAV Pose ==>x:"+str(gt_odom.position.x)+", y:"+str(gt_odom.position.y)+", z:"+str(gt_odom.position.z))
rospy.logdebug("Firefly Command>>" + str(outPose))
self._check_publishers_connection()
#publish desired position and velocity
self._cmd_vel_pub.publish(outPose)
rate = rospy.Rate(update_rate) # 10hz
try:
rate.sleep()
except:
pass
def __init__(self, ros_ws_abspath,**kwargs):
"""
Initializes a new FireflyEnv environment.
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that the stream of data doesnt flow. This is for simulations
that are pause for whatever the reason
2) If the simulation was running already for some reason, we need to reset the controllers.
This has to do with the fact that some plugins with tf, dont understand the reset of the simulation
and need to be reseted to work properly.
The Sensors: The sensors accesible are the ones considered usefull for AI learning.
Sensor Topic List:
* /pose : Reads the estimated pose of the MAV. Type: uav_msgs
* /target_tracker/pose : Reads the fused estimate of the target person. Type: geometry_msgs
Actuators Topic List:
* /command, publishes the desired waypoints and velocity for the robot. Type: uav_msgs
Args:
"""
rospy.logdebug("Start FireflyEnv INIT...")
# Variables that we give through the constructor.
# None in this case
# We launch the ROSlaunch that spawns the robot into the world
# ROSLauncher(rospackage_name="turtlebot_gazebo",
# launch_file_name="put_robot_in_world.launch",
# ros_ws_abspath=ros_ws_abspath)
# Internal Vars
# Doesnt have any accesibles
self.controllers_list = []
# It doesnt use namespace
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(FireflyMultiAgentGTEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False,
start_init_physics_parameters=False,
reset_world_or_sim="WORLD",**kwargs)
machine_name = '/machine_'+str(self.robotID);rotors_machine_name = '/firefly_'+str(self.robotID)
self.rotors_machine_name = rotors_machine_name
self.num_robots=2
if self.robotID==self.num_robots:
neighbor_name = '/machine_'+str((self.robotID+1)%self.num_robots);rotors_neighbor_name = '/firefly_'+str((self.robotID+1)%self.num_robots)
else:
neighbor_name = '/machine_'+str(self.robotID+1);rotors_neighbor_name = '/firefly_'+str(self.robotID+1)
self.rotors_neighbor_name = rotors_neighbor_name
self.pose_topic = machine_name+"/pose"
self.velocity_topic = rotors_machine_name+"/ground_truth/odometry"
self.gt_pose_topic = machine_name+"/pose/groundtruth"
self.gt_neighbor_pose_topic = neighbor_name+"/pose/groundtruth"
self.firefly_pose_topic = rotors_machine_name+"/ground_truth/pose_with_covariance"
self.target_topic = machine_name+"/target_tracker/pose"
self.gt_target_topic = "/actorpose"
self.gt_target_vel_topic = "/actorvel"
self.target_velocity_topic = machine_name+"/target_tracker/twist"
self.command_topic = machine_name+"/command"
self.neighbor_command_topic = neighbor_name+"/command"
self.destination_topic = machine_name+"/destination"
self.detections_feedback_topic = machine_name+"/object_detections/feedback"
self.noisy_joints_topic = machine_name+"/noisy_joints"
self.noisy_joints_neighbor_topic = neighbor_name+"/noisy_joints"
self.noisy_bbox_topic = machine_name+"/noisy_bbox"
self.noisy_bbox_neighbor_topic = neighbor_name+"/noisy_bbox"
self.noisy_detection = machine_name+"/noisy_detection"
self.noisy_neighbor_detection = neighbor_name+"/noisy_detection"
self.alphapose_topic = machine_name+"/result_alpha"
self.alphapose_neighbor_topic = neighbor_name+"/result_alpha"
self.alphapose_bbox_topic = machine_name+"/result_bbox"
self.alphapose_bbox_neighbor_topic = neighbor_name+"/result_bbox"
self.alphapose_detection = machine_name+"/detection"
self.alphapose_neighbor_detection = neighbor_name+"/detection"
self.camera_info = rotors_machine_name+rotors_machine_name+"/xtion/rgb/camera_info"
self.camera_info_neighbor = rotors_neighbor_name+rotors_neighbor_name+"/xtion/rgb/camera_info"
self.joints_gt = '/gt_joints'
self.mpc_command = machine_name+"/command_MPC"
import tf
self.listener = tf.TransformListener()
self.gazebo.unpauseSim()
#self.controllers_object.reset_controllers()
# We Start all the ROS related Subscribers and publishers
rospy.Subscriber(self.firefly_pose_topic,PoseWithCovarianceStamped , self._firefly_pose_callback)
rospy.Subscriber(self.pose_topic, uav_pose, self._pose_callback)
rospy.Subscriber(self.velocity_topic, Odometry, self._vel_callback)
rospy.Subscriber(self.gt_pose_topic, uav_pose, self._gt_pose_callback)
rospy.Subscriber(self.gt_neighbor_pose_topic, uav_pose, self._gt_neighbor_pose_callback)
rospy.Subscriber(self.target_topic, PoseWithCovarianceStamped, self._target_callback)
rospy.Subscriber(self.gt_target_topic, Odometry, self._gt_target_callback)
rospy.Subscriber(self.target_velocity_topic, TwistWithCovarianceStamped, self._target_vel_callback)
rospy.Subscriber(self.detections_feedback_topic, NeuralNetworkFeedback, self._detections_feedback_callback)
noisy_joints = message_filters.Subscriber(self.noisy_joints_topic, AlphaRes)
self.noisy_joints_cache = message_filters.Cache(noisy_joints, 100);self.noisy_joints_cache.registerCallback(self._noisy_joints_callback)
noisy_joints_neighbor = message_filters.Subscriber(self.noisy_joints_neighbor_topic, AlphaRes)
self.noisy_joints_neighbor_cache = message_filters.Cache(noisy_joints_neighbor, 100);self.noisy_joints_neighbor_cache.registerCallback(self._noisy_joints_neighbor_callback)
noisy_bbox = message_filters.Subscriber(self.noisy_bbox_topic, AlphaRes)
self.noisy_bbox_cache = message_filters.Cache(noisy_bbox, 100);self.noisy_bbox_cache.registerCallback(self._noisy_bbox_callback)
noisy_bbox_neighbor = message_filters.Subscriber(self.noisy_bbox_neighbor_topic, AlphaRes)
self.noisy_bbox_neighbor_cache = message_filters.Cache(noisy_bbox_neighbor, 100);self.noisy_bbox_neighbor_cache.registerCallback(self._noisy_bbox_neighbor_callback)
rospy.Subscriber(self.noisy_detection, Int16,self._noisy_detection_callback)
rospy.Subscriber(self.noisy_neighbor_detection, Int16,self._noisy_neighbor_detection_callback)
rospy.Subscriber(self.alphapose_topic, AlphaRes, self._alphapose_callback)
rospy.Subscriber(self.alphapose_neighbor_topic, AlphaRes, self._alphapose_neighbor_callback)
rospy.Subscriber(self.alphapose_bbox_topic, AlphaRes, self._alphapose_bbox_callback)
rospy.Subscriber(self.alphapose_bbox_neighbor_topic, AlphaRes, self._alphapose_bbox_neighbor_callback)
rospy.Subscriber(self.alphapose_detection, Int16, self._alphapose_detection_callback)
rospy.Subscriber(self.alphapose_neighbor_detection, Int16, self._alphapose_neighbor_detection_callback)
rospy.Subscriber(self.command_topic, uav_pose, self._command_callback)
rospy.Subscriber(self.camera_info, CameraInfo, self._camera_info_callback)
rospy.Subscriber(self.camera_info_neighbor, CameraInfo, self._camera_info_neighbor_callback)
rospy.Subscriber(self.joints_gt, PoseArray, self._joints_gt_callback)
rospy.Subscriber(self.mpc_command, uav_pose, self._mpc_command_callback)
self._dest_vel_pub = rospy.Publisher(self.destination_topic, PoseStamped, queue_size=1)
self._cmd_vel_pub = rospy.Publisher(self.command_topic, uav_pose, queue_size=1)
self._cmd_neighbor_vel_pub = rospy.Publisher(self.neighbor_command_topic, uav_pose, queue_size=1)
self._target_init_pub = rospy.Publisher(self.target_topic, PoseWithCovarianceStamped, queue_size=1)
self.create_circle(radius=8)
# self.gazebo.pauseSim()
outPose = uav_pose()
outPose.header.stamp = rospy.Time.now()
outPose.header.frame_id="world"
outPose.POI.x = 0
outPose.POI.y = 0
outPose.POI.z = 0
r = 8#
t = np.random.choice(63,1);
# outPose.position.x = r*np.cos(self.theta[t[0]])
# outPose.position.y = r*np.sin(self.theta[t[0]])
# outPose.position.z = -r
# if self.robotID==2:
# #[6,0,-6]
# outPose.position.x = r*np.cos(self.theta[t[0]])
# outPose.position.y = r*np.sin(self.theta[t[0]])
# outPose.position.z = -r
# else:
outPose.position.x = r*np.cos(self.theta[t[0]])
outPose.position.y = r*np.sin(self.theta[t[0]])
outPose.position.z = -r
self._cmd_vel_pub.publish(outPose)
rospy.logwarn("Finished FireflyEnv INIT...")
self._check_all_sensors_ready()
rospy.logwarn("SENSORS OK!.")
self._check_publishers_connection()
rospy.logwarn("PUBLISHERS OK!.")
def process_frontcam(image):
""" process the frontcam image """
if not SCHEDULER.is_frontcam_enable():
return
state = SCHEDULER.get_state()
info = EYE.see_front(image)
if SCHEDULER.debug_option["show_front_info"]:
rospy.logdebug(info)
if state == "default":
if EYE.is_boostable(image):
process_acceleration(info)
# return
# SCHEDULER.set_state("to_intersection")
# signal = is_construction(image)
# rospy.logdebug(signal)
# if is_construction(image):
# TURTLE.boost()
# SCHEDULER.set_state("construction")
return
if state == "traffic_light":
if is_light_green(image):
TURTLE.enable()
SCHEDULER.set_state("to_intersection")
return
# NOTE: temporary settings:
if state == "to_intersection":
# rospy.Timer(rospy.Duration(35), SCHEDULER.enable_lidar, oneshot=True)
SCHEDULER.set_state("intersection_left")
# if state == "to_intersection":
# signal = check_left_right_sign(image)
# if signal == "right":
# SCHEDULER.set_state("intersection_right")
# elif signal == "left":
# SCHEDULER.set_state("intersection_left")
# return
if state == "intersection_right":
# TODO: make algorithms for right
if EYE.is_boostable(image):
TURTLE.boost()
SCHEDULER.set_state("to_construction")
return
# NOTE: temporary settings:
if state == "intersection_left":
# if EYE.is_boostable(image):
# TURTLE.boost()
# SCHEDULER.set_state("to_construction")
return
if state == "to_construction":
# if EYE.is_boostable(image):
# TURTLE.boost()
EYE.check_yellow = True
# if is_construction(image):
# SCHEDULER.set_state("construction_searching")
if state == "construction_searching":
pass
def process_construction():
""" process construction state """
global STEP
global NUM_OBSTRUCTION
global LANE_TO
global BUF_ANGULAR
global BUF_SIZE
if TURTLE.is_occupied():
return
if STEP == 0:
# TURTLE.set_speed("normal")
leftside = get_object_distance("leftside")
left = get_object_distance("left")
if leftside > 0:
rospy.logdebug("[PROC] LIDAR LEFTSIDE: {}".format(
leftside))
if (leftside > 0) and (leftside < 0.50) and (left > 1.00):
# EYE.check_yellow = False
SCHEDULER.set_state("construction")
rospy.loginfo("[PROC] construction state started.")
STEP = 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
TURTLE.go_forward(3.5)
return
else:
return
elif STEP == 1:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("slow")
TURTLE.turn(0.13, 0)
left = get_object_distance("left")
leftback = get_object_distance("leftback")
rospy.logdebug("[PROC] LIDAR LEFT: {:.2f} LEFTBACK: {:.2f}}".format(left, leftback))
if (left > 0) and (left < 0.50):
return
else:
TURTLE.set_speed("slow")
if (leftback > 0.5):
# TURTLE.go_forward(2.5)
STEP = 3
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 2:
# TODO: write code for first left lane
pass
elif STEP == 3:
TURTLE.set_speed("normal")
TURTLE.set_speed_smooth("stop")
front = get_object_distance("front")
right_biased = get_object_distance("right_biased")
if (front > 1.0) and (right_biased < 1.0) and (right_biased > 0.0):
print("passed", front, right_biased)
print("BUF_SIZE: ", BUF_SIZE)
BUF_SIZE = 15
reset_buffer()
TURTLE.set_speed_smooth("slow")
pass
elif front * right_biased == 0:
return
else:
TURTLE.set_speed("stop")
print("turning...", front, right_biased)
TURTLE.turn(0.13, 1.0)
# rospy.sleep(rospy.Duration(5.0))
return
# TURTLE.go_turn("left")
elif STEP == 4:
right_biased = get_object_distance("right_biased")
left_biased = get_object_distance("left_biased")
front = get_object_distance("front")
if right_biased == 0.0:
right_biased = 3.0
if left_biased == 0.0:
left_biased = 3.0
if front == 0.0:
front = 3.0
elif front < 0.2:
TURTLE.set_speed_smooth("stop")
else:
TURTLE.set_speed_smooth("slow")
min_distance = min(right_biased, left_biased)
degree = 0
if (front < 1.0):
degree += max(pow(1.0 - front, 2), 0)
else:
degree += max(0.5 - min_distance, 0) * 3
# if min_distance < 0.5:
# degree += max((0.5 - min_distance), 0) * 1.5
# elif (min_distance > 1.0) and (min_distance < 3.0):
# degree = 0.2
if (left_biased == min_distance) and (min_distance < 0.5):
degree *= -1
# max_distance = max(right_biased, left_biased)
# if (left_biased == max_distance):
# degree *= -1
# degree = 0
# if min_distance > 0 and min_distance < 0.5:
# if right_biased > left_biased:
# degree = (0.5 - min_distance) * (-7)
# LANE_TO = "right"
# elif right_biased < left_biased:
# degree = (0.5 - min_distance) * (7)
# LANE_TO = "left"
# if is_left_crashable():
# degree = -1.7
# elif is_right_crashable():
# degree = 1.7
degree *= 3
degree = max(min(degree, 2.0), -2.0)
BUF_ANGULAR.append(degree)
degree -= BUF_ANGULAR.pop(0)
print("BUF_ANGULAR:", BUF_ANGULAR)
# if degree != 0:
# BUF_ANGULAR.append(degree)
# elif len(BUF_ANGULAR) > 9:
# STEP = 5
if SCHEDULER.debug_option["show_construction_lidar"]:
rospy.logdebug("[PROC] r_based: {:.2f} l_based: {:.2f} min: {:.2f} front: {:.2f} deg: {:.2f}"
.format(right_biased, left_biased, min_distance, front, degree))
TURTLE.turn(0.13, degree)
return
elif STEP == 5:
print("[StEP 5]")
if len(BUF_ANGULAR) > 0:
TURTLE.turn(0.13, -BUF_ANGULAR.pop(0))
return
else:
front = get_object_distance("front")
print(front)
# if (front > 0) and (front < 1.0):
# if LANE_TO == "right":
# TURTLE.turn(0.13, 0.8)
# else:
# TURTLE.turn(0.13, -0.8)
# return
# else:
if NUM_OBSTRUCTION < 1:
NUM_OBSTRUCTION += 1
STEP = 4
return
elif STEP == 6:
TURTLE.go_forward(1)
TURTLE.set_speed("normal")
TURTLE.go_turn("left")
TURTLE.set_speed("normal")
TURTLE.set_speed("fast")
TURTLE.set_speed_smooth("normal")
TURTLE.go_forward(5)
STEP = 10
SCHEDULER.set_state("parking")
BUF_SIZE = 3
reset_buffer()
return
STEP += 1
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
def process_parking():
""" process parking state """
global STEP
# if TURTLE.is_occupied():
# return
if STEP == 10:
frontleft = get_object_distance("frontleft")
frontright = get_object_distance("frontright")
left = get_object_distance("left")
right = get_object_distance("right")
if SCHEDULER.debug_option["show_parking_lidar"]:
rospy.logdebug("frontright: {:.2f} frontleft: {:.2f}".format(
frontright, frontleft
))
if (frontleft > 0) and (frontleft < 1.0):
STEP = 11
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
elif (frontright > 0) and (frontright < 1.0):
STEP = 12
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
if (left > 0) and (left < 0.5):
STEP = 13
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
elif (right > 0) and (right < 0.5):
STEP = 14
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
# NOTE: return is needed to prevent executing STEP += 1
return
elif STEP == 11:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_forward(2.5)
TURTLE.go_turn("right", 2)
STEP = 15
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 12:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_forward(2.5)
TURTLE.go_turn("left", 2)
STEP = 15
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 13:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("right", angular=1.8, duration=1.2)
STEP = 15
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 14:
SCHEDULER.disable_cams()
TURTLE.set_speed("normal")
TURTLE.go_turn("left", angular=1.8, duration=1.2)
STEP = 15
rospy.logdebug("[PROC] STEP changed to {}".format(STEP))
return
elif STEP == 15:
TURTLE.go_forward(2.0)
# for stopping for a while
rospy.sleep(rospy.Duration(0.5))
elif STEP == 16:
TURTLE.go_backward(1.5)
elif STEP == 17:
TURTLE.go_turn_backward(1.2)
elif STEP == 18:
TURTLE.set_speed("normal")
TURTLE.go_forward(3)
# rospy.sleep(rospy.Duration(3))
elif STEP == 19:
TURTLE.set_speed("fast")
SCHEDULER.set_state("zigzag")
else:
return
STEP += 1
def update(self):
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
if self.fake:
# x = cos(self.th)*self.dx*elapsed
# y = -sin(self.th)*self.dx*elapsed
# self.x += cos(self.th)*self.dx*elapsed
# self.y += sin(self.th)*self.dx*elapsed
# self.th += self.dr*elapsed
x = cos(self.th) * self.dx * elapsed
y = -sin(self.th) * self.dx * elapsed
self.x += cos(self.th) * self.dx * elapsed + sin(
self.th) * self.dy * elapsed
self.y += sin(self.th) * self.dx * elapsed + cos(
self.th) * self.dy * elapsed
self.th += self.dr * elapsed
else:
# read encoders
try:
left, right = self.status()
except Exception as e:
rospy.logerr("Could not update encoders: " + str(e))
return
rospy.logdebug("Encoders: " + str(left) + "," + str(right))
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (left - self.enc_left) / self.ticks_meter
d_right = (right - self.enc_right) / self.ticks_meter
self.enc_left = left
self.enc_right = right
d = (d_left + d_right) / 2
th = (d_right - d_left) / self.base_width
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
x = cos(th) * d
y = -sin(th) * d
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
# publish or perish
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
if now > (self.last_cmd + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
# update motors
if not self.fake:
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.write(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def execute(self, userdata):
rospy.logdebug('FastGrasp: Executing state FastGrasp')
self.__t_point.header.frame_id = "/odom_combined"
# TODO: Auf 10 Min grenzen testen
if not utils.manipulation.is_gripper_open():
rospy.logdebug('FastGrasp: Open Gripper')
utils.manipulation.open_gripper()
r = self.percieve_object()
i = 0
while r == -1:
if i == 2 and not userdata.request_second_object:
# TODO: Zeit anpassen, ab wann gecalled wird
rospy.logdebug('FastGrasp: Request next object')
rospy.ServiceProxy("/euroc_interface_node/request_next_object",
RequestNextObject).call()
userdata.request_second_object = True
if i == 19 and userdata.request_second_object:
rospy.logdebug('FastGrasp: Time Expired')
return 'noObjectsLeft'
r = self.percieve_object()
i += 1
# TODO: Was passiert wenn das Objekt nur einmal gesehen wurde
rospy.logdebug('FastGrasp: Begin movement, Plan 1')
for j in range(0, 4):
self.extrapolate(j)
self.calculate_target_point(self.__pose_comp)
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 1: moved!")
break
else:
rospy.logdebug("FastGrasp: Plan 1: No Plan fount in step " +
str(j))
if j == 3:
return 'noPlanFound'
if userdata.object_index == 1:
offset = rospy.Duration(2)
elif userdata.object_index == 2:
offset = rospy.Duration(4)
else:
offset = rospy.Duration(4)
while rospy.get_rostime() < self.__t_point_time - offset:
rospy.sleep(0.01)
self.__t_point.pose.position.z -= 0.07
rospy.logdebug("FastGrasp: Plan 2")
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 2: moved!")
else:
rospy.logdebug("FastGrasp: Plan 2: Cant grasp")
return 'noPlanFound'
utils.manipulation.close_gripper(self.__pose_comp)
self.__t_point.pose.position.z += 0.1
rospy.logdebug("FastGrasp: Plan 3")
for k in range(0, 4):
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 3: moved!")
break
else:
if k == 3:
rospy.logdebug("FastGrasp: No Plan found to lift object")
return 'noPlanFound'
rospy.logdebug("FastGrasp: Plan 3: Cant lift: " + str(k))
if self.__t_point.pose.position.y > 0:
self.__t_point.pose.position.y -= 0.1
else:
self.__t_point.pose.position.y += 0.1
if self.__t_point.pose.position.x > 0:
self.__t_point.pose.position.x -= 0.1
else:
self.__t_point.pose.position.x += 0.1
rospy.sleep(Duration.from_sec(0.5))
tfs = TorqueForceService()
if tfs.is_free():
rospy.logdebug("FastGrasp: Grasp Fail")
return 'graspingFailed'
rospy.logdebug("FastGrasp: objectGrasped, finished")
userdata.object_index += 1
return 'objectGrasped'
def _command_test(*args, **kwargs):
logger.info("logging info test")
rospy.logdebug("ros logdebug test")
print("stdout test")
def __init__(self):
rospy.logdebug("Poses Handlers - Entering in Init")
# Subscribers
self.__tcp_enabled = False
rospy.Subscriber('/niryo_robot_tools_commander/tcp', TCP,
self.__callback_tcp)
self.__tool_id = 0
rospy.Subscriber('/niryo_robot_tools_commander/current_id', Int32,
self.__callback_tool_id)
# Workspaces
ws_dir = rospy.get_param("~workspace_dir")
self.__ws_manager = WorkspaceManager(ws_dir)
rospy.Service('~manage_workspace', ManageWorkspace,
self.__callback_manage_workspace)
rospy.Service('~get_workspace_ratio', GetWorkspaceRatio,
self.__callback_workspace_ratio)
rospy.Service('~get_workspace_list', GetNameDescriptionList,
self.__callback_workspace_list)
rospy.Service('~get_workspace_poses', GetWorkspaceRobotPoses,
self.__callback_get_workspace_poses)
if rospy.has_param('~gazebo_workspaces'):
for ws_name, ws_poses in rospy.get_param(
'~gazebo_workspaces').items():
if ws_name not in self.get_available_workspaces()[0]:
rospy.loginfo(
"Poses Handler - Adding the {} workspace...".format(
ws_name))
self.create_workspace_from_points(
ws_name, "", [Point(*point) for point in ws_poses])
# Grips
tool_config_dict = rospy.get_param(
"niryo_robot_tools_commander/tool_list", dict())
self.__tool_id_gripname_dict = {
tool["id"]: "default_" + tool["name"].replace(" ", "_")
for tool in tool_config_dict
}
self.__tool_id_gripname_dict[-1] = "default_Calibration_Tip"
grip_dir = rospy.get_param("~grip_dir")
self.__grip_manager = GripManager(
grip_dir, self.__tool_id_gripname_dict.values())
rospy.Service('~get_target_pose', GetTargetPose,
self.__callback_target_pose)
# Transform Handlers
self.__transform_handler = PosesTransformHandler(self.__grip_manager)
rospy.logdebug("Poses Handlers - Transform Handler created")
# -- POSES AND TRAJECTORIES
# Poses
poses_dir = rospy.get_param("~poses_dir")
self.__pos_manager = PoseManager(poses_dir)
rospy.Service('~manage_pose', ManagePose, self.__callback_manage_pose)
rospy.Service('~get_pose', GetPose, self.__callback_get_pose)
rospy.Service('~get_pose_list', GetNameDescriptionList,
self.__callback_get_position_list)
# Trajectories
trajectories_dir = rospy.get_param("~trajectories_dir")
self.traj_manager = TrajectoryManager(trajectories_dir)
rospy.Service('~manage_trajectory', ManageTrajectory,
self.__callback_manage_trajectory)
rospy.Service('~get_trajectory', GetTrajectory,
self.__callback_get_trajectory)
rospy.Service('~get_trajectory_list', GetNameDescriptionList,
self.__callback_get_trajectory_list)
# Set a bool to mentioned this node is initialized
rospy.set_param('~initialized', True)
rospy.loginfo("Poses Handlers - Started")
def see_sub(self, compressed_data=None):
""" callback handler when sub image received """
self.sub_occupied = True
self.set_sub_image(compressed_data)
road_lines = self.get_road_line_by_sub()
# if road_lines is not None and len(road_lines) is not 4:
# if SCHEDULER.debug_option["warn_road_lines"]:
# rospy.logwarn("[LIB_EYE] road_lines: ")
# pprint(road_lines)
# if road_lines is None:
# self.info_fish = {
# # "len": len(road_lines),
# "left": 0,
# "right": IMAGE_FISH_WIDTH,
# "slope": 0
# }
# return self.info_fish
# rospy.logdebug("\n[EYE] SUB lines: " + str(road_lines))
if not isinstance(road_lines, list):
self.info_sub["has_line"] = False
elif len(road_lines) < 2:
self.info_sub["has_line"] = False
else:
self.info_sub["has_line"] = True
if road_lines and len(road_lines) > 1:
sum_x, sum_y = 0, 0
divider = len(road_lines) * 2
for line in road_lines:
sum_x = sum_x + line['points'][0] + line['points'][2]
sum_y = sum_y + line['points'][1] + line['points'][3]
average_x, average_y = sum_x / divider, sum_y / divider
self._buffer_sub.append(average_x)
# rospy.logdebug("\n[EYE] average: {:.2f} {:.2f}".format(average_x, average_y))
if len(self._buffer_sub) == BUF_SIZE:
self.info_sub["center"] = int(
sum(self._buffer_sub) /
BUF_SIZE) - self.threshold_sub_center
# self.info_sub["line_center"][0] = self.info_sub["line_center"][2] = int(sum(self._buffer_sub) / BUF_SIZE)
self._buffer_sub.pop(0)
else:
rospy.logdebug("[LIB_EYE] len(_buffer_sub): {:d}".format(
len(self._buffer_sub)))
# self.info_sub["line_center"][3] = int(average_y)
# self.info_sub["line_center"][3] = 240
if road_lines:
slope = 0
for line in road_lines:
slope = slope + line['slope']
# NOTE: 1 added to slope due to the lens is biased to 45 degree
self.info_sub["slope"] = float(
"{:.2f}".format(1 + (slope / len(road_lines))))
image_center = int(self.info_sub["center"] + (IMAGE_SUB_WIDTH / 2))
line_center = [{"points": [image_center, 0, image_center, 480]}]
# self.draw_lines(line_center, self.images["sub/canny"], color="blue")
# self.draw_lines(road_lines, self.images["sub/canny"], color="blue")
# self.set_info_fish_to_image(self.images["sub/canny"])
self.publish_image()
return self.info_sub
def is_near(msg, x, y):
rospy.logdebug("Position %s: local: %d, target: %d, abs diff: %d",
msg, x, y, abs(x - y))
return abs(x - y) < 0.2
def DownloadRepo(self, package_name, ros_ws_abspath):
"""
This has to be installed
sudo pip install gitpython
"""
commands_to_take_effect = "\ncd "+ros_ws_abspath + \
"\ncatkin_make\nsource devel/setup.bash\nrospack profile\n"
ros_ws_src_abspath_src = os.path.join(ros_ws_abspath, "src")
pkg_path = None
# We retrieve the got for the package asked
package_git = None
rospy.logdebug("package_name===>" + str(package_name) + "<===")
if package_name == "moving_cube_description":
package_git = [
"https://bitbucket.org/theconstructcore/moving_cube.git"
]
elif package_name == "rosbot_gazebo" or package_name == "rosbot_description":
package_git = [
"https://bitbucket.org/theconstructcore/rosbot_husarion.git"
]
elif package_name == "fetch_gazebo":
package_git = [
"https://bitbucket.org/theconstructcore/fetch_tc.git"
]
elif package_name == "cartpole_description" or package_name == "cartpole_v0_training":
package_git = [
"https://bitbucket.org/theconstructcore/cart_pole.git"
]
elif package_name == "legged_robots_sims" or package_name == "legged_robots_description" or package_name == "my_legged_robots_description" or package_name == "my_legged_robots_sims" or package_name == "my_hopper_training":
package_git = ["https://bitbucket.org/theconstructcore/hopper.git"]
elif package_name == "iri_wam_description" or package_name == "iri_wam_gazebo" or package_name == "iri_wam_reproduce_trajectory" or package_name == "iri_wam_aff_demo":
package_git = [
"https://bitbucket.org/theconstructcore/iri_wam.git"
]
package_git.append(
"https://bitbucket.org/theconstructcore/hokuyo_model.git")
elif package_name == "drone_construct" or package_name == "drone_demo" or package_name == "sjtu_drone" or package_name == "custom_teleop" or package_name == "ardrone_as":
package_git = [
"https://bitbucket.org/theconstructcore/parrot_ardrone.git"
]
elif package_name == "sawyer_gazebo":
package_git = [
"https://bitbucket.org/theconstructcore/sawyer_full.git"
]
elif package_name == "shadow_gazebo":
package_git = [
"https://bitbucket.org/theconstructcore/shadow_robot_smart_grasping_sandbox.git"
]
elif package_name == "summit_xl_gazebo":
package_git = [
"https://bitbucket.org/theconstructcore/summit_xl.git"
]
elif package_name == "gym_construct":
package_git = [
"https://bitbucket.org/theconstructcore/open_ai_gym_construct.git"
]
elif package_name == "turtlebot_gazebo":
package_git = [
"https://bitbucket.org/theconstructcore/turtlebot.git"
]
elif package_name == "turtlebot3_gazebo":
package_git = [
"https://bitbucket.org/theconstructcore/turtlebot3.git"
]
elif package_name == "robotx_gazebo":
package_git = ["https://bitbucket.org/theconstructcore/vmrc.git"]
package_git.append(
"https://bitbucket.org/theconstructcore/spawn_robot_tools.git")
elif package_name == "fetch_simple_description":
package_git = [
"https://bitbucket.org/theconstructcore/fetch_simple_simulation.git"
]
package_git.append(
"https://bitbucket.org/theconstructcore/spawn_robot_tools.git")
# ADD HERE THE GITs List To Your Simuation
elif package_name == "iiwa_description" or package_name == "iiwa_gazebo" or package_name == "iiwa":
package_git = ["https://github.com/Deastan/reflex-ros-pkg.git"]
package_git = ["https://github.com/Deastan/iiwa_stack.git"]
# package_git.append(
# "https://bitbucket.org/theconstructcore/spawn_robot_tools.git")
else:
rospy.logerr(
"Package [ >" + package_name +
"< ] is not supported for autodownload, do it manually into >"
+ str(ros_ws_abspath))
assert False, "The package "++ \
" is not supported, please check the package name and the git support in openai_ros_common.py"
# If a Git for the package is supported
if package_git:
for git_url in package_git:
try:
rospy.logdebug("Lets download git=" + git_url +
", in ws=" + ros_ws_src_abspath_src)
git.Git(ros_ws_src_abspath_src).clone(git_url)
rospy.logdebug("Download git=" + git_url + ", in ws=" +
ros_ws_src_abspath_src + "...DONE")
except git.exc.GitCommandError:
rospy.logwarn("The Git " + git_url +
" already exists in " +
ros_ws_src_abspath_src + ", not downloading")
# We check that the package is there
try:
pkg_path = self.rospack.get_path(package_name)
rospy.logwarn("The package " + package_name +
" was FOUND by ROS.")
if ros_ws_abspath in pkg_path:
rospy.logdebug("Package FOUND in the correct WS!")
else:
rospy.logwarn("Package FOUND in=" + pkg_path +
", BUT not in the ws=" + ros_ws_abspath)
rospy.logerr(
"IMPORTANT!: You need to execute the following commands and rerun to dowloads to take effect."
)
rospy.logerr(commands_to_take_effect)
sys.exit()
except rospkg.common.ResourceNotFound:
rospy.logerr("Package " + package_name + " NOT FOUND by ROS.")
# We have to make the user compile and source to make ROS be able to find the new packages
# TODO: Make this automatic
rospy.logerr(
"IMPORTANT!: You need to execute the following commands and rerun to dowloads to take effect."
)
rospy.logerr(commands_to_take_effect)
sys.exit()
return pkg_path
def get_lanes(self, request: LaneSrvRequest) -> LaneSrvResponse:
"""Answer lane service request."""
response = LaneSrvResponse()
response.lane_msg = self.groundtruth.get_lane_msg(request.id)
rospy.logdebug(f"Answering lane request {response.lane_msg}")
return response
running_step = rospy.get_param("/moving_cube/running_step")
# Initialises the algorithm that we are going to use for learning
qlearn = qlearn.QLearn(actions=range(env.action_space.n),
alpha=Alpha,
gamma=Gamma,
epsilon=Epsilon)
initial_epsilon = qlearn.epsilon
start_time = time.time()
highest_reward = 0
# Starts the main training loop: the one about the episodes to do
for x in range(nepisodes):
rospy.logdebug("############### START EPISODE=>" + str(x))
cumulated_reward = 0
done = False
if qlearn.epsilon > 0.05:
qlearn.epsilon *= epsilon_discount
# Initialize the environment and get first state of the robot
observation = env.reset()
state = ''.join(map(str, observation))
episode_time = rospy.get_rostime().to_sec()
# for each episode, we test the robot for nsteps
for i in range(nsteps):
rospy.loginfo("############### Start Step=>" + str(i))
# Pick an action based on the current state
def close_node(self):
rospy.logdebug(" in close_node")
def step(self, dt):
# Make sure we access bpy data and do other task in blender thread
if not self.init:
self.anim = animate.Animate('Armature')
self.animationsList = self.anim.getAnimationList()
# Initialize the old-style ROS node
self.animationsPub = rospy.Publisher('animations_list',
animations_list,
latch=True,
queue_size=10)
self.animationsPub.publish(list(self.animationsList.keys()))
rospy.Subscriber('cmd_animations', String, self.parseCommand)
# Initialize the new blender_api_msgs ROS node
self.emoPub = rospy.Publisher(
'/blender_api/available_emotion_states',
AvailableEmotionStates,
latch=True,
queue_size=10)
self.emoPub.publish(list(self.animationsList.keys()))
# XXX FIXME No gestures!?!
self.gestPub = rospy.Publisher('/blender_api/available_gestures',
AvailableGestures,
latch=True,
queue_size=10)
self.gestPub.publish(list())
rospy.Subscriber('/blender_api/set_emotion_state', EmotionState,
self.setEmotionState)
# Other initilizations
self.init = True
self.anim.resetAnimation()
# Parse any pending commands
if self.command:
if self.command == 'play':
if not self.isPlaying:
if not self.current:
self._setNext()
# If next was null, then, after above, current will
# become null, too.
if self.current:
self.isPlaying = True
self.anim.playAnimation()
else:
rospy.logwarn("Play: no pending animation to restart")
elif self.command == 'pause':
if self.isPlaying:
self.anim.stopAnimation()
self.isPlaying = False
else:
rospy.logwarn("Pause: no animation playing, can't pause")
self.command = None
if self.isPlaying:
rospy.logdebug(self.animationsList[self.current]['length'])
rospy.logdebug(bpy.context.scene.frame_current)
# self.anim.playAnimation()
# Check to see if animation is done
if bpy.context.scene.frame_current > self.animationsList[
self.current]['length']:
if self.next:
self._setNext()
self.anim.playAnimation()
else:
self.isPlaying = False
self.current = None
self.anim.resetAnimation()
outputs.store.full_head.transmit()
def close_inspector_window(self):
rospy.logdebug(' ------ Statusitem close_inspector_window 1')
self.inspector = None
def get_sections(self, request: SectionSrvRequest) -> SectionSrvResponse:
"""Answer section service request."""
response = SectionSrvResponse()
response.sections = self.groundtruth.get_section_msgs()
rospy.logdebug(f"Answering section request {response.sections}")
return response
def set_config(self, grpc_url='grpc://localhost:12321', data=''):
rospy.logdebug("set config to %s" % (grpc_url))
uri, _ = nmdurl.split(grpc_url)
sm = self.get_settings_manager(uri)
sm.set_config(data)
def update_children(self, status_new, diag_array):
"""
Recursive for tree node's children.
Update text on treeWidgetItem, set icon on it.
:type status: DiagnosticStatus
:type msg: DiagnosticArray
"""
self.status = status_new
if self.inspector:
self.inspector.update_status_display(self.status)
children_diag_statuses = Util.get_children(self.name, diag_array)
names_toplevel_local = [s.name for s in self._children_statusitems]
errors = 0
warnings = 0
for child_diagnostic_status in children_diag_statuses:
name = child_diagnostic_status.name
device_name = Util.get_grn_resource_name(
child_diagnostic_status.name)
if (child_diagnostic_status.level != DiagnosticStatus.OK):
Util.gen_headline_warn_or_err(child_diagnostic_status)
if (child_diagnostic_status.level == DiagnosticStatus.ERROR):
errors = errors + 1
elif (child_diagnostic_status.level == DiagnosticStatus.WARN):
warnings = warnings + 1
else:
Util.gen_headline_status_green(child_diagnostic_status)
rospy.logdebug(' update_children level= %s',
child_diagnostic_status.level)
if name in names_toplevel_local:
index_child = names_toplevel_local.index(name)
status_item = self._children_statusitems[index_child]
# Recursive call.
status_item.update_children(child_diagnostic_status,
diag_array)
Util.update_status_images(child_diagnostic_status, status_item)
rospy.logdebug(' StatusItem update 33 index= %d dev_name= %s',
index_child, device_name)
status_item.setText(0, device_name)
status_item.setText(1, child_diagnostic_status.message)
elif len(self.strip_child(name).split('/')) <= 2:
status_item = StatusItem(child_diagnostic_status)
# Recursive call.
status_item.update_children(child_diagnostic_status,
diag_array)
status_item.setText(0, device_name)
status_item.setText(1, child_diagnostic_status.message)
self._children_statusitems.append(status_item)
self.addChild(status_item)
rospy.logdebug(' ------ Statusitem.update_children err=%d warn=%d',
errors, warnings)
return {
Util._DICTKEY_TIMES_ERROR: errors,
Util._DICTKEY_TIMES_WARN: warnings
}
def __init__(self):
self.sim_mode = get_param('simulation_mode', False)
self.publish_joint_angles = get_param(
'publish_joint_angles', True) # if self.sim_mode else False
self.publish_temperatures = get_param('publish_temperatures', True)
self.axis_for_right = float(get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = get_param('~connect_on_startup', False)
#self.calibrate_on_startup = get_param('~calibrate_on_startup', False)
#self.engage_on_startup = get_param('~engage_on_startup', False)
self.has_preroll = get_param('~use_preroll', True)
self.publish_current = get_param('~publish_current', True)
self.publish_raw_odom = get_param('~publish_raw_odom', True)
self.publish_odom = get_param('~publish_odom', True)
self.publish_tf = get_param('~publish_odom_tf', False)
self.odom_topic = get_param('~odom_topic', "odom")
self.odom_frame = get_param('~odom_frame', "odom")
self.base_frame = get_param('~base_frame', "base_link")
self.odom_calc_hz = get_param('~odom_calc_hz', 10)
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
# odometry update, disable during preroll, whenever wheels off ground
self.odometry_update_enabled = True
rospy.Service('enable_odometry_updates', std_srvs.srv.SetBool,
self.enable_odometry_update_service)
self.status_pub = rospy.Publisher('status',
std_msgs.msg.String,
latch=True,
queue_size=2)
self.status = "disconnected"
self.status_pub.publish(self.status)
self.command_queue = Queue.Queue(maxsize=5)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
self.publish_diagnostics = True
if self.publish_diagnostics:
self.diagnostic_updater = diagnostic_updater.Updater()
self.diagnostic_updater.setHardwareID("Not connected, unknown")
self.diagnostic_updater.add("ODrive Diagnostics",
self.pub_diagnostics)
if self.publish_temperatures:
self.temperature_publisher_left = rospy.Publisher(
'left/temperature', Float64, queue_size=2)
self.temperature_publisher_right = rospy.Publisher(
'right/temperature', Float64, queue_size=2)
self.i2t_error_latch = False
if self.publish_current:
#self.current_loop_count = 0
#self.left_current_accumulator = 0.0
#self.right_current_accumulator = 0.0
self.current_publisher_left = rospy.Publisher('left/current',
Float64,
queue_size=2)
self.current_publisher_right = rospy.Publisher('right/current',
Float64,
queue_size=2)
self.i2t_publisher_left = rospy.Publisher('left/i2t',
Float64,
queue_size=2)
self.i2t_publisher_right = rospy.Publisher('right/i2t',
Float64,
queue_size=2)
rospy.logdebug("ODrive will publish motor currents.")
self.i2t_resume_threshold = get_param('~i2t_resume_threshold', 222)
self.i2t_warning_threshold = get_param('~i2t_warning_threshold',
333)
self.i2t_error_threshold = get_param('~i2t_error_threshold', 666)
self.last_cmd_vel_time = rospy.Time.now()
if self.publish_raw_odom:
self.raw_odom_publisher_encoder_left = rospy.Publisher(
'left/raw_odom/encoder', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_encoder_right = rospy.Publisher(
'right/raw_odom/encoder', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_left = rospy.Publisher(
'left/raw_odom/velocity', Int32,
queue_size=2) if self.publish_raw_odom else None
self.raw_odom_publisher_vel_right = rospy.Publisher(
'right/raw_odom/velocity', Int32,
queue_size=2) if self.publish_raw_odom else None
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.old_pos_l = 0
self.old_pos_r = 0
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
tcp_nodelay=True,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
self.odom_msg.twist.twist.angular.z = 0.0
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
if self.publish_joint_angles:
self.joint_state_publisher = rospy.Publisher(
'/odrive/joint_states', JointState, queue_size=2)
jsm = JointState()
self.joint_state_msg = jsm
#jsm.name.resize(2)
#jsm.position.resize(2)
jsm.name = ['joint_left_wheel', 'joint_right_wheel']
jsm.position = [0.0, 0.0]
def floris_greedy_association(self, costs, max_cost=None, split_cost=None):
"""
costs: a numpy array of dimension (# contours in this frame x # targets in last frame).
entries are the distance between centers.
"""
def drop_max_cost(coord_val):
"""
"""
if self.debug:
rospy.loginfo('dropping objects {} from cost matrix b/c max cost'.format(\
np.where(coord_val[:,2] < max_cost)[0]))
return coord_val[coord_val[:, 2] < max_cost, :]
#indices = np.where(costs[c,:] < ncov)[0]
#new_contour_object_errors = [[costs[c,i], tracked_object_ids[i], c] for i in indices]
#contour_to_object_error.extend(new_contour_object_errors)
contour_to_object_error = cost_matrix_to_coord_val_format(costs)
#print 'costs converted to coord-val format'
#print contour_to_object_error
# values over threshold # covariances are set to max cost before this function
# TODO maybe this is what is causing some to be lost b/c never added to all_objects?
contour_to_object_error = drop_max_cost(contour_to_object_error)
#print 'after dropping max_cost entries'
#print contour_to_object_error
# TODO compare floris / ctrax versions for correctness
# TODO multiple assignments to same / suboptimal / dissapearing?
# TODO TODO not currently equipped to deal with splits. fix.
# TODO set behind a high verbosity level?
#print 'unsorted by costs\n', contour_to_object_error
sorted_indices = np.argsort(contour_to_object_error[:, 2])
contour_to_object_error = contour_to_object_error[sorted_indices, :]
#print 'sorted by costs\n', contour_to_object_error
objects2contours = {}
all_objects = set(range(costs.shape[1]))
all_contours = set(range(costs.shape[0]))
objects_accounted_for = set()
contours_accounted_for = set()
for data in contour_to_object_error:
# TODO check these are the correct indexes
c = int(data[1])
# TODO was this really an objid before?
o = int(data[0])
if o not in objects_accounted_for:
if c not in contours_accounted_for:
if self.debug:
rospy.loginfo('assigning c=' + str(c) + ' to o=' + str(o) + \
' w/ cost=' + str(data[2]))
objects2contours[o] = c
objects_accounted_for.add(o)
contours_accounted_for.add(c)
if self.debug:
rospy.logdebug('all_objects: {}'.format(all_objects))
rospy.logdebug(
'objects_accounted_for: {}'.format(objects_accounted_for))
unmatched_objects = []
for o in all_objects:
if not o in objects_accounted_for:
unmatched_objects.append(o)
unmatched_contours = []
for c in all_contours:
if not c in contours_accounted_for:
unmatched_contours.append(c)
# will have to convert to current object id after returning
# here, objects are just the indices in the cost matrix, which is the order in
# tracked_object_ids
return objects2contours, unmatched_objects, unmatched_contours
loop = rospy.get_param('~loop', False)
rosbag_args = rospy.get_param('~rosbag_play_args', '--clock --queue=5')
if pause:
rosbag_args += " --pause"
if loop:
rosbag_args += " --loop"
initialWaitTime = rospy.get_param('~initial_wait_time', 0.0)
if initialWaitTime > 0.0:
rospy.loginfo("Waiting %.1f seconds before starting playback..." %
initialWaitTime)
time.sleep(initialWaitTime)
### Run rosbag play / rqt_bag ###
rospy.logdebug("Topic remappings which will be forwarded to %s: %s" %
("rqt_bag" if rqt else "rosbag play", str(argsToForward)))
if not rqt:
commandLine = " ".join(["rosbag play", rosbag_args, "__name:=playback"] +
filesToPlay + argsToForward)
else:
commandLine = " ".join(["rqt_bag", "--clock", "__name:=playback"] +
filesToPlay + argsToForward + ["> /dev/null 2>&1"])
rospy.loginfo("Starting %s..." % "rqt_bag" if rqt else "rosbag play")
rospy.loginfo("Command line: " + commandLine)
subprocess.call(commandLine, shell=True)
time.sleep(1)
print("\n*** Playback has been stopped or finished. ***")
def _send_cmd(self, cmd, *args):
_cmd = " ".join([cmd] + [str(_) for _ in args])
rospy.logdebug('Sending command "%s"' % (_cmd))
self.command_socket.sendto(_cmd.encode('utf-8'), self.command_address)
save_freq = 1000
obs, done, ep_ret, ep_len = env.reset(), False, 0, 0
start_time = time.time()
# start training
while step_counter <= total_steps:
while 'blown' in env.status:
obs, ep_ret, ep_len = env.reset(), 0, 0
s0 = obs[[0, -1]].flatten()
s1 = obs[[1, -1]].flatten()
a0 = np.squeeze(agent.act(np.expand_dims(s0, axis=0)))
a1 = np.squeeze(agent.act(np.expand_dims(s1, axis=0)))
n_obs, rew, done, info = env.step(np.array([int(a0), int(a1)]))
n_s0 = n_obs[[0, -1]].flatten()
n_s1 = n_obs[[1, -1]].flatten()
rospy.logdebug(
"\nstate: {} \naction: {} \nreward: {} \ndone: {} \nn_state: {}".
format(obs, (a0, a1), rew, done, n_obs))
ep_ret += np.sum(rew)
ep_len += 1
replay_buffer.store(s0, a0, np.sum(rew), done, n_s0)
replay_buffer.store(s1, a1, np.sum(rew), done, n_s1)
obs = n_obs.copy() # SUPER CRITICAL
step_counter += 1
# train one batch
if not step_counter % train_freq and step_counter > train_after:
for _ in range(train_freq):
minibatch = replay_buffer.sample_batch(batch_size=batch_size)
loss_q = agent.train_one_batch(data=minibatch)
print("\nloss_q: {}".format(loss_q))
# handle episode termination
if done or (ep_len == env.max_episode_steps):
def debug(self, msg):
rospy.logdebug(msg) # print(msg) #
def command_rejected(self, error_code=FlexGraspErrorCodes.FAILURE):
"""method called when the state machine rejects the requested model."""
rospy.logdebug("[%s] Rejected command in message", self.NODE_NAME)
msg = FlexGraspErrorCodes(error_code)
self.pub_e_out.publish(msg)
self.reset()
def grasp_object(self, object_to_pick, pick_scene):
rospy.loginfo("Removing any previous 'part' object")
self.planning_scene_interface.remove_attached_object("arm_tool_link")
self.planning_scene_interface.remove_world_object("table1")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
# Add object description in scene
for object in pick_scene.box_objects_in_scene:
self.planning_scene_interface.remove_world_object(
object.object_name)
print object.object_name
print object.object_pose
object_pose = PoseStamped()
object_pose.header.frame_id = "base_footprint"
object_pose.header.stamp = rospy.get_rostime()
object_pose.pose = self.get_model_state(object.object_name,
"base_footprint")
self.planning_scene_interface.add_box(
object.object_name, object_pose,
(self.object_depth, self.object_width, self.object_height))
self.wait_for_planning_scene_object(object.object_name)
rospy.loginfo("Second%s", object_pose.pose)
# Define table
table_pose = PoseStamped()
table_pose.header.frame_id = "base_footprint"
table_pose.header.stamp = rospy.get_rostime()
table_pose.pose = self.get_model_state("table1", "base_footprint")
table_pose.pose.position.z += self.surface_height / 2
self.planning_scene_interface.add_box(
"table1", table_pose, (self.surface_depth, self.surface_width,
self.surface_height - 0.008)
) # remove few milimeters to prevent contact between the object and the table
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object("table1")
# compute grasps
object_to_pick_name = object_to_pick
object_to_pick_pose = PoseStamped()
object_to_pick_pose.pose = self.get_model_state(
object_to_pick_name, "base_footprint")
object_to_pick_pose.header.frame_id = "base_footprint"
object_to_pick_pose.header.stamp = rospy.get_rostime()
possible_grasps = self.sg.create_grasps_from_object_pose(
object_to_pick_pose)
goal = createPickupGoal("arm_torso", object_to_pick_name,
object_to_pick_pose, possible_grasps,
self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo("Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val