def manipulate(T):
print("programstarted")
T = int(T)
rospy.init_node('turtlesim_node', anonymous=True)
rate = rospy.Rate(70.0) # 70hz
twist = Twist()
t0 = rospy.get_time()
while not rospy.is_shutdown():
t = rospy.get_time() - t0
if(t>10):#reset the time when t reaches T
t0 = rospy.get_time()
vx = 12*np.pi*np.cos(4*np.pi*t/T)/T#dx/dt
vy = 6*np.pi*np.cos(2*np.pi*t/T)/T#dy/dt
us = np.sqrt(np.power(vx,2) + np.power(vy,2))
ax = (-48 * np.pi * np.pi * np.sin(4 * np.pi * t / T)) / (T * T)#d2x/dt2
ay = (-12 * np.pi * np.pi * np.sin(2 * np.pi * t / T)) / (T * T)#d2y/dt2
omega = (vx * ay - vy * ax) / (vx * vx + vy * vy)#angular velocity
twist.linear.x = us
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = omega
rospy.loginfo(str(twist))
pub.publish(twist)
rate.sleep()
def updateAndPublish(self):
#first we update the obstacle map
start_time = time.time()
rospy.loginfo("Updating Obstacle map")
self.updateObstacleMap()
rospy.loginfo("Publishing Obstacles")
self.publishObstacles()
rospy.loginfo("Finding Shared-Control Command")
start_time = rospy.get_time()
curr_cmd = self.curr_cmd
#uncomment to choose the algorithm you want to test
best_cmd = self.findBasicSafeguardedCmd(curr_cmd)
#best_cmd = self.findLimitedDWACmd(curr_cmd)
#best_cmd = self.findVFHCmd(curr_cmd)
elapsed_time = rospy.get_time() - start_time
self.publishTimeTaken(elapsed_time)
rospy.loginfo("Publishing Shared-Control Command")
self.publishCmd(best_cmd)
return
def callback_pose(self, data):
if self.pause == False or True:
dt = float(rospy.get_time() - self.lastCall_linear_)
self.lastcall_linear_ = rospy.get_time()
error_x = data.x
error_y = data.y
error_theta = data.theta
print error_theta
x_cmd = 0
y_cmd = 0
if error_theta < self.cutoff_angular_:
x_cmd = self.pid_linear_x_.update(error_x, dt)
y_cmd = self.pid_linear_y_.update(error_y, dt)
theta_cmd = self.pid_angular_.update(error_theta, dt)
twist = Twist()
twist.linear.x = -x_cmd
twist.linear.y = -y_cmd
twist.angular.z = -theta_cmd
self.pub_twist.publish(twist)
def callback(data):
global slamPoses
global gtPoses
global stamps
global listener
global rmse
global lastTime
if rospy.get_time() - lastTime < 1:
return
lastTime = rospy.get_time()
t = rospy.Time(data.header.stamp.secs, data.header.stamp.nsecs)
try:
listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
(trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
gtPoses.append(Point(trans[0], trans[1], 0))
stamps.append(t.to_sec())
slamPoses.append(data.pose.pose.position)
first_xyz = numpy.empty([0,3])
second_xyz = numpy.empty([0,3])
for g in gtPoses:
newrow = [g.x,g.y,g.z]
first_xyz = numpy.vstack([first_xyz, newrow])
for p in slamPoses:
newrow = [p.x,p.y,p.z]
second_xyz = numpy.vstack([second_xyz, newrow])
first_xyz = numpy.matrix(first_xyz).transpose()
second_xyz = numpy.matrix(second_xyz).transpose()
rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
rmse.append(rmse_v)
print " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
print str(e)
def turtlerun():
#T = input('PLease enter the number of T:')
pub = rospy.Publisher('/turtlesim1/turtle1/cmd_vel',Twist, queue_size=10)
rate = rospy.Rate(50)
rospy.wait_for_service('/turtlesim1/turtle1/teleport_absolute')
start = rospy.ServiceProxy('/turtlesim1/turtle1/teleport_absolute',TeleportAbsolute)
start(5.5,5.5,0.5)
T = rospy.get_param("turtlesim1/T")
t0 = rospy.get_time()
while not rospy.is_shutdown():
now_time = rospy.get_time()
t = now_time-t0
x = 3*sin((4*pi*t)/T)
y = 3*sin((2*pi*t)/T)
dx = 12*pi*cos((4.0*pi*t)/T)/T
dy = 6*pi*cos((2.0*pi*t)/T)/T
v = sqrt(dx*dx+dy*dy)
dx2 = -48*pi*pi*sin((4*pi*t)/T)/(T*T)
dy2 = -12*pi*pi*sin((2*pi*t)/T)/(T*T)
w = ((dx*dy2)-(dy*dx2)) /((dx*dx) + (dy*dy))
turtlemove = Twist(Vector3(v,0,0),Vector3(0,0,w))
rospy.loginfo(turtlemove)
pub.publish(turtlemove)
rate.sleep()
def sphero_pos_callback(self, msg):
if self.runnable == True:
current_pos = [float(msg.x), float(msg.y)]
if current_pos[0] < 0 or current_pos[1] < 0:
# stop when running out of view
cv = Twist()
cv.linear.y = 0.0
cv.linear.y = 0.0
cv.linear.z = 0.0
cv.angular.x = 0.0
cv.angular.y = 0.0
cv.angular.z = 0.0
self.cmd_vel_pub.publish(cv)
elif self.sphero_target_pos[0] >= 0 and self.sphero_target_pos[1] >= 0:
delta_x = self.sphero_target_pos[0] - current_pos[0]
delta_y = self.sphero_target_pos[1] - current_pos[1]
d_delta_x = 0.0
d_delta_y = 0.0
if self.last_time == None:
self.last_time = rospy.get_time()
self.last_x = current_pos[0]
self.last_y = current_pos[1]
else:
new_time = rospy.get_time()
delta_t = new_time - self.last_time
delta_last_x = current_pos[0] - self.last_x
delta_last_y = current_pos[1] - self.last_y
self.last_time = new_time
self.last_x = current_pos[0]
self.last_y = current_pos[1]
d_delta_last_x = delta_last_x / delta_t
d_delta_last_y = delta_last_y / delta_t
gain_x = self.Kp * delta_x + self.Kd * d_delta_last_x
gain_y = self.Kp * delta_y + self.Kd * d_delta_last_y
cv = Twist()
if gain_x > 0:
cv.linear.x = min(- gain_x , -20.0)
elif gain_x < 0:
cv.linear.x = max(- gain_x , 20.0)
if gain_y > 0:
cv.linear.y = max( gain_y, 20.0)
elif gain_y < 0:
cv.linear.y = min( gain_y, -20.0)
cv.linear.z = 0.0
cv.angular.x = 0.0
cv.angular.y = 0.0
cv.angular.z = 0.0
if cv.linear.x != 0.0 and cv.linear.y != 0.0:
#print "tar : " + str(self.sphero_target_pos)
#print "curr : " + str(current_pos)
#print "del : " + str([delta_x, delta_y])
print "d_t : " + str(delta_t)
print "d_del : " + str([d_delta_last_x, d_delta_last_y])
print "vel : " + str([cv.linear.x, cv.linear.y])
self.cmd_vel_pub.publish(cv)
def moveTurtle():
rate = rospy.Rate(70.0) # Duration of the command rosply.sleep => 70 was the duration that gave the smoother 8 shape
T = input('\nEnter a value for T (1/Hz): ') # for T=5 we should have around 354 msgs for a full lap
Pi = math.pi # define the value of PI
t0 = rospy.get_time() # get starting time for the script to
while not rospy.is_shutdown():
t = rospy.get_time() - t0 # make sure that t always starts 0 instead of getting the computer internal clock
vX = (12 * Pi * math.cos(4 * Pi * t / T)) / T # velocity on the X axis
vY = (6 * Pi * math.cos(2 * Pi * t / T)) / T # velocity on the Y axis
aX = (-48 * Pi * Pi * math.sin(4 * Pi * t / T)) / (T * T) # aceleration on the X axis
aY = (-12 * Pi * Pi * math.sin(2 * Pi * t / T)) / (T * T) # aceleration on the Y axis
v = math.sqrt(vX * vX + vY * vY) # straight velocity calculation
omega = (vX * aY - vY * aX) / (vX * vX + vY * vY) # angular velocity calculation
command = Twist() # create a new msg Twist to be published
command.linear.x = v # insert the velocity on X (front of the turtle)
command.linear.y = 0
command.linear.z = 0
command.angular.x = 0
command.angular.y = 0
command.angular.z = omega # insert angular velocity to steer the turtle
pub.publish(command) # publish the command
rate.sleep() # sleep for the rate defined at the top of the code
def callLeft(data):
#gets time
time = rospy.get_time()
#reads encoder inputs
channelA = GPIO.input("P9_23")
channelB = GPIO.input("P9_30")
#puts inputs into the quadrature decoder program
quadCalcL.update(channelA, channelB, time)
#creates messages
leftFMSG = JointState()
leftRMSG = JointState()
header = Header()
#appends velocity and position data to the joinstate message
#front wheel
leftFMSG.header.stamp.secs = rospy.get_time()
leftFMSG.name.append('front_left_wheel')
leftFMSG.position.append(quadCalcL.position)
#rear wheel
leftRMSG.header.stamp.secs = rospy.get_time()
leftRMSG.name.append('rear_left_wheel')
leftRMSG.position.append(quadCalcL.position)
#publishes message
pubFL.publish(leftFMSG)
pubRL.publish(leftRMSG)
def callRight(data):
#gets time
time = rospy.get_time()
#reads encoder inputs
channelA = GPIO.input("P8_17")
channelB = GPIO.input("P8_26")
#puts inputs into the quadrature decoder program
quadCalcR.update(channelA, channelB, time)
#creates messages
rightFMSG = JointState()
rightRMSG = JointState()
header = Header()
#appends velocity and position data to the joinstate message
#front wheel
rightFMSG.header.stamp.secs = rospy.get_time()
rightFMSG.name.append('front_right_wheel')
rightFMSG.position.append(quadCalcL.position)
#rear wheel
rightRMSG.header.stamp.secs = rospy.get_time()
rightRMSG.name.append('rear_right_wheel')
rightRMSG.position.append(quadCalcL.position)
#publishes message
pubFR.publish(rightFMSG)
pubRR.publish(rightRMSG)
def eat(self): have_forks=False while (not(have_forks) and not rospy.is_shutdown()): have_forks=self.philosopher.getForks(num_forks) if self.kikloi<=self.allagi: self.hunger_level_=self.philosopher.recalculateHunger(self.philosopherIState,self.last_hunger_update_,self.hunger_level_) else: self.hunger_level_=self.recalculateHungerConstant(self.philosopherIState,self.hunger_level_,self.constant) #self.hunger_level_=self.philosopher.recalculateHunger(self.philosopherIState,self.last_hunger_update_,self.hunger_level_) self.last_hunger_update_=rospy.get_time() self.philosopherIState="ISTATE_EATING" print "istateeating?", self.philosopherIState duration=randrange(0,(self.max_eating_time_*100)/100) rospy.sleep(duration) print "my state is:", self.philosopherIState if self.kikloi<=self.allagi: self.hunger_level_=self.philosopher.recalculateHunger(self.philosopherIState,self.last_hunger_update_,self.hunger_level_) else: self.hunger_level_=self.recalculateHungerConstant(self.philosopherIState,self.hunger_level_,self.constant) #self.hunger_level_=self.philosopher.recalculateHunger(self.philosopherIState,self.last_hunger_update_,self.hunger_level_) self.last_hunger_update_=rospy.get_time() philosopherIState="ISTATE_WAITING" if have_forks==True: self.philosopher.giveForks(num_forks)
def wait_for(test, timeout=1.0, raise_on_error=True, rate=100,
timeout_msg="timeout expired", body=None):
"""
Waits until some condition evaluates to true.
@param test: zero param function to be evaluated
@param timeout: max amount of time to wait. negative/inf for indefinitely
@param raise_on_error: raise or just return False
@param rate: the rate at which to check
@param timout_msg: message to supply to the timeout exception
@param body: optional function to execute while waiting
"""
end_time = rospy.get_time() + timeout
rate = rospy.Rate(rate)
notimeout = (timeout < 0.0) or timeout == float("inf")
while not test():
if rospy.is_shutdown():
if raise_on_error:
raise OSError(errno.ESHUTDOWN, "ROS Shutdown")
return False
elif (not notimeout) and (rospy.get_time() >= end_time):
if raise_on_error:
raise OSError(errno.ETIMEDOUT, timeout_msg)
return False
if callable(body):
body()
rate.sleep()
return True
def twist_ramp_linear(v_min, v_max, accel):
rospy.loginfo('Generating a ramp from %f m/s to %f m/s at %f m/s^2',v_min,v_max, accel)
t_step = .05
cmdPub = rospy.Publisher("cmd_vel", Twist)
rospy.init_node('twist_ramp')
twistMsg = Twist()
twistMsg.angular.x = 0
twistMsg.angular.y = 0
twistMsg.angular.z = 0
twistMsg.linear.x = 0
twistMsg.linear.z = 0
if accel > 0:
twistMsg.linear.y = v_min
else:
twistMsg.linear.y = v_max
wallTime = rospy.get_time()
while not rospy.is_shutdown() and twistMsg.linear.y <= v_max and twistMsg.linear.y >= v_min:
rospy.sleep(.05)
oldWallTime = wallTime
wallTime = rospy.get_time()
timeStep = wallTime - oldWallTime
cmdPub.publish(twistMsg)
twistMsg.linear.y = twistMsg.linear.y + accel * timeStep
twistMsg.linear.y = 0
cmdPub.publish(twistMsg)
def update(self):
with self._mutex:
diag = DiagnosticArray()
diag.header.stamp = rospy.get_rostime()
info_update_ok = rospy.get_time() - self._last_info_update < 5.0 / self._batt_info_rate
state_update_ok = rospy.get_time() - self._last_state_update < 5.0 / self._batt_state_rate
if info_update_ok:
self._msg.design_capacity = self._batt_design_capacity
self._msg.capacity = self._batt_last_full_capacity
else:
self._msg.design_capacity = 0.0
self._msg.capacity = 0.0
if info_update_ok and state_update_ok and self._msg.capacity != 0:
self._msg.percentage = int(self._msg.charge / self._msg.capacity * 100.0)
diag_stat = _laptop_charge_to_diag(self._msg)
if not info_update_ok or not state_update_ok:
diag_stat.level = DiagnosticStatus.ERROR
diag_stat.message = 'Laptop battery data stale'
diag.status.append(diag_stat)
self._diag_pub.publish(diag)
self._power_pub.publish(self._msg)
def move(self, pos, speed, pos2):
start = False
end = False
while abs(self.getpos()-pos)>self.tolerance:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
if self.getpos()>=pos2[0] and self.getpos()<=pos2[1]:
if not start:
start = [rospy.get_time(),self.getpos()]
else:
end = [rospy.get_time(),self.getpos()]
f = 1
if self.getpos()-pos>0:
f=-1
self.intf.set_val(self.conf_out, f*speed)
self.intf.set_val(self.conf_out, 0)
print start, end
return abs(end[1]-start[1])/(end[0]-start[0])
def epc_motion(self, equi_pt_generator, time_step, arm, arg_list,
rapid_call_func=None, control_function=None):
stop, ea = equi_pt_generator(*arg_list)
t_end = rospy.get_time()
while stop == '':
if rospy.is_shutdown():
stop = 'rospy shutdown'
continue
t_end += time_step
#self.robot.set_jointangles(arm, ea)
#import pdb; pdb.set_trace()
control_function(arm, *ea)
# self.robot.step() this should be within the rapid_call_func for the meka arms.
t1 = rospy.get_time()
while t1<t_end:
if rapid_call_func != None:
stop = rapid_call_func(arm)
if stop != '':
break
#self.robot.step() this should be within the rapid_call_func for the meka arms
rospy.sleep(0.01)
t1 = rospy.get_time()
if stop == '':
stop, ea = equi_pt_generator(*arg_list)
if stop == 'reset timing':
stop = ''
t_end = rospy.get_time()
return stop, ea
def process_odom(self, msg):
"""
in training mode, save incoming odometry data as training data
also publish a marker which should mimic odom
"""
if self.train:
t_const = 1 # incoming angle rate may or may not be in rad/s
new_x = msg.pose.pose.position.x # get x from odom
new_y = msg.pose.pose.position.y # get y from odom
x_diff = new_x - self.x # get deltas
y_diff = new_y - self.y
dist = sqrt(x_diff ** 2.0 + y_diff ** 2) # calculate distance traveled
self.x = new_x # reset old x and y
self.y = new_y
t_rate = msg.twist.twist.angular.z * t_const # get angle change rate from odom
d_time = rospy.get_time() - self.last_time # get change in time
self.t = self.t + t_rate * (d_time) # calculate current angle from t_rate and d_time
self.last_time = rospy.get_time() # reset last_time
t_drive = atan2(y_diff, x_diff) # find angle of travel
t_diff = self.t - t_drive # compare travel angle to robot angle
if not (pi / 2 > abs(t_diff)): # if they are in approximately opposite directions
print "hello"
dist = -dist # set direction of travel to backwards
x_rate = dist / d_time # calculate forward travel rate
self.training_data = [x_rate, t_rate]
self.build_message(self.x, self.y, self.t) # make marker to show odom position
def publishWayPoints(xPos, yPos, angle, w_p):
print "publishing waypoint"
pub = rospy.Publisher("/move_base_simple/goal", PoseStamped)
goal = PoseStamped()
initTime = rospy.get_time()
mapWidth = globalCostMapGrid.info.width
mapOriginX = int(math.floor(globalCostMapGrid.info.origin.position.x * 20))
mapOriginY = int(math.floor(globalCostMapGrid.info.origin.position.y * 20))
goal.header = g.header
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = xPos
goal.pose.position.y = yPos
goal.pose.position.z = 0
quat = quaternion_from_euler(0, 0, angle)
goal.pose.orientation.x = quat[0]
goal.pose.orientation.y = quat[1]
goal.pose.orientation.z = quat[2]
goal.pose.orientation.w = quat[3]
valX = int(math.floor(d.way_points[w_p][0] * 20)) - mapOriginX
valY = int(math.floor(d.way_points[w_p][1] * 20)) - mapOriginY
r = rospy.Rate(0.7)
while math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2)) > 0.5:
print "d:", math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2))
finalTime = rospy.get_time()
changeTime = finalTime - initTime
if (globalCostMapGrid.data[(valX * mapWidth) + valY] > g.threshold) or (changeTime > 120):
break
else:
pub.publish(goal)
def updater(self): while not rospy.is_shutdown(): # default is True prev_act_en = self.act_en_msg.data self.act_en_msg.data = True # cricital fault if true or too old if self.critfault_en == True: if self.critfault_state != 0 or self.critfault_next_tout < rospy.get_time(): self.act_en_msg.data = False # deadman fault if false or too old if self.deadman_en == True: if self.deadman_state == False or self.deadman_next_tout < rospy.get_time(): self.act_en_msg.data = False #print self.deadman_next_tout, rospy.get_time(), self.deadman_state # publish actuation_enable message self.act_en_msg.header.stamp = rospy.get_rostime() self.act_en_pub.publish (self.act_en_msg) if prev_act_en != self.act_en_msg.data: if self.act_en_msg.data == True: rospy.logwarn(rospy.get_name() + ": Enabling actuation") else: rospy.logwarn(rospy.get_name() + ": Disabling actuation") # go back to sleep self.r.sleep()
def updater(self): while not rospy.is_shutdown(): if self.hmi_digital_joy_a_up == True and self.hmi_digital_joy_a_up_changed == True: self.hmi_digital_joy_a_up_changed = False rospy.loginfo(rospy.get_name() + ": User skipped waypoint") self.goto_next_wpt() if self.hmi_digital_joy_a_down == True and self.hmi_digital_joy_a_down_changed == True: self.hmi_digital_joy_a_down_changed = False rospy.loginfo(rospy.get_name() + ": User selected previous waypoint") self.goto_previous_wpt() if self.state == self.STATE_NAVIGATE: (self.status, self.linear_vel, self.angular_vel) = self.wptnav.update(rospy.get_time()) if self.status == self.wptnav.UPDATE_ARRIVAL: rospy.loginfo(rospy.get_name() + ": Arrived at waypoint: %s (error %.2fm)" % (self.wpt[self.wptnav.W_ID], self.wptnav.dist)) # activate wait mode if self.wpt[self.wptnav.W_WAIT] >= 0.0: self.wait_after_arrival = self.wpt[self.wptnav.W_WAIT] else: self.wait_after_arrival = self.wpt_def_wait_after_arrival if self.wait_after_arrival > 0.01: self.linear_vel = 0.0 self.angular_vel = 0.0 self.publish_cmd_vel_message() self.publish_implement_message() rospy.loginfo(rospy.get_name() + ": Waiting %.1f seconds" % (self.wait_after_arrival)) self.state = self.STATE_WAIT self.wait_timeout = rospy.get_time() + self.wait_after_arrival else: self.state = self.STATE_NAVIGATE self.goto_next_wpt() else: self.publish_cmd_vel_message() self.publish_implement_message() elif self.state == self.STATE_WAIT: if rospy.get_time() > self.wait_timeout: self.state = self.STATE_NAVIGATE self.goto_next_wpt() else: self.linear_vel = 0.0 self.angular_vel = 0.0 self.publish_cmd_vel_message() self.publish_implement_message() # publish status if self.status_publish_interval != 0: self.status_publish_count += 1 if self.status_publish_count % self.status_publish_interval == 0: self.publish_status_message() # publish pid if self.pid_publish_interval != 0: self.pid_publish_count += 1 if self.pid_publish_count % self.pid_publish_interval == 0: self.publish_pid_message() self.r.sleep()
def activity_callback(self, topic_name=None, state=True, strategy=None):
"""
ActivitySource uses this callback to set it's state in ActivityTracker
ActivityTracker keeps state for all ActivitySources with timestamps.
If all states turned False (inactive) with respect to self.timeout then message is emitted
If all states turned True (active) then proper message is emitted
"""
with self._lock:
if topic_name not in self.activity_states:
self.activity_states[topic_name] = {"state": state, "time": rospy.get_time()}
try:
try:
if self.activity_states[topic_name]['state'] == state:
rospy.logdebug("State of %s didnt change" % topic_name)
else:
self.activity_states[topic_name] = {"state": state, "time": rospy.get_time()}
rospy.logdebug("Topic name: %s state changed to %s" % (topic_name, state))
except KeyError:
rospy.loginfo("Initializing topic name state: %s" % topic_name)
self.activity_states[topic_name] = {"state": state, "time": rospy.get_time()}
self._check_states()
return True
except Exception, e:
rospy.logerr("activity_callback for %s failed because %s" % (topic_name, e))
return False
def test_from_disk():
print ('Testing from disk')
start_time = rospy.get_time()
total = 0
failure = 0
for filename in os.listdir(TEST_PATH):
total += 1
rotation_num = int(filename.rsplit('_', 3)[1])
# print ('Label ' + str(LABEL))
# print 'Rotation ' + str(ROTATION)
imagee = cv2.imread(TEST_PATH + filename)
imagee = cv2.resize(imagee, (128, 128))
found_rot = get_img_rot(imagee)
if not abs(rotation_num - found_rot) < 0.5:
# print ('Testing ' + str(filename))
failure += 1
# print ('Does not work')
# cv2.imshow('Did not work',imagee)
# cv2.waitKey(100)
# print (found_rot)
# print (ROTATION)
percentage = 100 * failure / total
print ('Failure = ' + str(percentage) + '%')
print ('Failures = ' + str(failure))
print('Elapsed Time Testing = ' + str(rospy.get_time() - start_time) + '\n')
print ('Done')
def publish_labjack():
topic = 'is_blocked'
blocked_pub = rospy.Publisher(topic, Bool, queue_size=100) # Boolean
topic = 'motor_angle'
angle_pub = rospy.Publisher('motor_angle', Float64, queue_size=10) # Radians
connect_pub = rospy.Publisher('is_connected', Bool, queue_size=100) # Boolean
r = rospy.Rate(rospy.get_param("labjack_rate"))
start_time = rospy.get_time()
end_time = rospy.get_time()
state = True
last_state = True
while not rospy.is_shutdown():
state = check_gate_blocked() # Boolean
rospy.loginfo("labjack_publisher: The photogate is open: %s", state)
blocked_pub.publish(state)
now = rospy.get_time() # Float
angle = (((now - start_time) / (end_time - start_time)) * 2*pi) % (2*pi)
rospy.loginfo("labjack_publisher: The motor is at %f radians from the photogate intersection point", angle)
angle_pub.publish(angle)
if last_state and not state:
start_time = end_time
end_time = now
last_state = state
connect_state = check_connectivity()
rospy.loginfo("labjack_publisher: The finger wire is still connected: %s", connect_state)
connect_pub.publish(connect_state)
r.sleep()
def __init__(self, namespace='', read_compensated=True, timeout=3.0):
"""
FTSensor constructor. It subscribes to the following topics:
- C{ft_sensor/diagnostics},
- C{ft_sensor/raw}, and
- C{endpoint_state}.
It reports the raw and compensated (if available) FT sensor values.
@type namespace: string
@param namespace: Override ROS namespace manually. Useful when accessing multiple FT sensors from the same node.
@type timeout: double
@param timeout: Time to wait for each of the FT sensor's topics to start publishing.
"""
ns = solve_namespace(namespace)
# Set-up subscribers
self.rate = read_parameter('%sft_sensor/ft_sensor_controller/publish_rate' % ns, 250.0)
self.raw_queue = collections.deque(maxlen=self.queue_len)
rospy.Subscriber('%sft_sensor/diagnostics' % ns, DiagnosticArray, self.cb_diagnostics)
rospy.Subscriber('%sft_sensor/raw' % ns, WrenchStamped, self.cb_raw)
rospy.Subscriber('%sendpoint_state' % ns, EndpointState, self.cb_compensated)
initime = rospy.get_time()
while not rospy.is_shutdown() and not self.is_raw_alive():
rospy.sleep(0.1)
if (rospy.get_time() - initime) > timeout:
rospy.logwarn('FTSensor: Cannot read raw wrench')
return
if read_compensated:
initime = rospy.get_time()
while not rospy.is_shutdown() and not self.is_compensated_alive():
rospy.sleep(0.1)
if (rospy.get_time() - initime) > timeout:
rospy.logwarn('FTSensor: Cannot read compensated wrench')
return
rospy.loginfo('FTSensor successfully initialized')
def laser_callback(self, scan):
cmd = self.get_driving_info(scan.ranges[180:900]) #returns a tuple with the speed and angle
speed = cmd[0] #information given by the method
angle = cmd[1]
if (min(scan.ranges[525:555]) < self.safety_threshold):
print "Robot suicide RIP"
speed = -0.2
elif (self.is_stuck()):
#unstuck
#speed = -0.7
self.stuck_time = rospy.get_time()
self.stuck = True
elif (self.stuck): #if it is stuck and the time has not elapsed
print 'Is Stuck'
if not rospy.get_time() - self.stuck_time < self.stuck_threshold:
self.stuck = False
self.speeds.append(speed)
if(len(self.speeds) > 40):
self.speeds.pop(0)
msg = AckermannDriveStamped()
msg.drive.speed = speed
msg.drive.steering_angle = angle
self.pub.publish(msg)
def calc_angular(self, yaw):
angular_vel = 0.
if self.yaw is not None:
angular_vel = (yaw - self.yaw)/(rospy.get_time() - self.prev_time)
self.yaw = yaw
self.prev_time = rospy.get_time()
return angular_vel
def talker(rate_T):
# set initial position
rospy.wait_for_service('turtle1/teleport_absolute')
turtle1_teleport = rospy.ServiceProxy('turtle1/teleport_absolute', TeleportAbsolute)
turtle1_teleport(5.4444444, 5.4444444,(math.pi)/6.5)
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
velocity = Twist()
start_time = rospy.get_time()
while not rospy.is_shutdown():
now = rospy.get_time() - start_time
x_in = (4*math.pi*now)/rate_T
y_in = (2*math.pi*now)/rate_T
x_t = 3*math.sin(x_in)
x_t_1 = ((12*math.pi)/rate_T) * math.cos(x_in)
x_t_2 = -((48*(math.pow(math.pi,2)))/(math.pow(rate_T,2)))*math.sin(x_in)
y_t = 3*math.sin(y_in)
y_t_1 = ((6*math.pi)/rate_T) * math.cos(y_in)
y_t_2 = -((12*(math.pow(math.pi,2)))/(math.pow(rate_T,2)))*math.sin(y_in)
cur_angle = math.atan2(y_t,x_t)
v_t = math.sqrt(math.pow(x_t_1,2) + math.pow(y_t_1,2))
w_t = ((x_t_1*y_t_2) - (y_t_1*x_t_2))/(math.pow(x_t_1,2) + math.pow(y_t_1,2))
velocity.linear.x = v_t #* math.cos(cur_angle)
velocity.angular.z = w_t#cur_angle
pub.publish(velocity)
def callback(self, IncomingData):
# Method calls that send the XYZ location of the roomba and return xyz acceleration
IncomingData = IncomingData.data()
dt = rospy.get_time() - self.old_time
self.oldtime = rospy.get_time()
if IncomingData[0] == 0:
x_vel = IncomingData[3]
else:
x_vel = self.pid_methods.pid_x(IncomingData[3], dt)
if IncomingData[1] == 0:
y_vel = IncomingData[4]
else:
y_vel = self.pid_methods.pid_y(IncomingData[4], dt)
if IncomingData[2] == 0:
z_vel = IncomingData[5]
else:
z_vel = self.pid_methods.pid_z(IncomingData[5], dt)
# set linear to value
self.vel_twist.twist.linear.x = x_vel
self.vel_twist.twist.linear.y = y_vel
self.vel_twist.twist.linear.z = z_vel
# Set angular to zero
self.vel_twist.twist.angular.x = 0
self.vel_twist.twist.angular.y = 0
self.vel_twist.twist.angular.z = 0
# logs the xyz accel data
rospy.loginfo(self.vel_twist)
def main():
global bumper
global pub_song, song
rospy.sleep(2)
# topic subscribed
rospy.Subscriber('/mobile_base/events/bumper', BumperEvent, callback_bumper)
rospy.Subscriber('/facedetector/faces', Detection, callback_face)
rospy.sleep(1)
song.song = song.Indiana_Jones
pub_song.publish(song)
bumper.bumper = 3
# moving loop
while not rospy.is_shutdown():
#randomm direction changing
duree = 3 + random()*5
time_end = rospy.get_time() + duree
while(rospy.get_time() < time_end):
scenario()
if bumper > 2:
print('aleatoire')
bumper.bumper = choice([bumper.LEFT, bumper.RIGHT, 3])
rospy.spin()
def execute(self, userdata):
#Listen for a point if we don't have one.
point_stamped = self.message
t_start = rospy.get_time()
r = rospy.Rate(10)
waitobj = WaitForMessage('/cloud_click_point', geo.PoseStamped)
pose_stamped = None
while point_stamped == None:
try:
pose_stamped = waitobj.get_message()
if pose_stamped != None:
point_stamped = geo.PointStamped()
point_stamped.header = pose_stamped.header
point_stamped.point = pose_stamped.pose.position
r.sleep()
except rospy.ROSException, e:
pass
if self.preempt_requested():
self.service_preempt()
return 'preempted'
if (self.time_out != None) and ((rospy.get_time() - t_start) > self.time_out):
return 'timed_out'
def callback(self, data):
self.lock.acquire()
self.cloud = list(pc2py.points(data, ['x', 'y', 'z']))
self.new_cloud = True
print rospy.get_time()-self.start, "time in between call backs"
self.start = rospy.get_time()
self.lock.release()
def __init__(self):
rospy.sleep(
5.0
) #put a leep so it can connect to the force/torque sensor first
self._num_grippers = rospy.get_param('~num_grippers', 1)
self._comport = rospy.get_param('~comport', '/dev/ttyUSB0')
self._baud = rospy.get_param('~baud', '115200')
self._prefix = rospy.get_param('~prefix', '')
if self._prefix != "" and self._prefix[-1] != "_":
self._prefix += "_"
self._gripper = Robotiq85Gripper(self._num_grippers, self._comport,
self._baud)
if not self._gripper.init_success:
rospy.logerr("Unable to open commport to %s" % self._comport)
return
if (self._num_grippers == 1 and not self._prefix):
rospy.Subscriber("/gripper/cmd",
GripperCmd,
self._update_gripper_cmd,
queue_size=10)
self._gripper_pub = rospy.Publisher('/gripper/stat',
GripperStat,
queue_size=10)
self._gripper_joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=10)
elif (self._num_grippers == 1 and self._prefix):
rospy.logwarn('gripper prefix = {}'.format(self._prefix))
rospy.Subscriber("/" + self._prefix + "gripper/cmd",
GripperCmd,
self._update_gripper_cmd,
queue_size=10)
self._gripper_pub = rospy.Publisher("/" + self._prefix +
"gripper/stat",
GripperStat,
queue_size=10)
self._gripper_joint_state_pub = rospy.Publisher(
"/" + self._prefix + "gripper/joint_states",
JointState,
queue_size=10)
elif (self._num_grippers == 2):
rospy.Subscriber("/left_gripper/cmd",
GripperCmd,
self._update_gripper_cmd,
queue_size=10)
self._left_gripper_pub = rospy.Publisher('/left_gripper/stat',
GripperStat,
queue_size=10)
self._left_gripper_joint_state_pub = rospy.Publisher(
'/left_gripper/joint_states', JointState, queue_size=10)
rospy.Subscriber("/right_gripper/cmd",
GripperCmd,
self._update_right_gripper_cmd,
queue_size=10)
self._right_gripper_pub = rospy.Publisher('/right_gripper/stat',
GripperStat,
queue_size=10)
self._right_gripper_joint_state_pub = rospy.Publisher(
'/right_gripper/joint_states', JointState, queue_size=10)
else:
rospy.logerr(
"Number of grippers not supported (needs to be 1 or 2)")
return
self._seq = [0] * self._num_grippers
self._prev_js_pos = [0.0] * self._num_grippers
self._prev_js_time = [rospy.get_time()] * self._num_grippers
self._driver_state = 0
self._driver_ready = False
success = True
for i in range(self._num_grippers):
success &= self._gripper.process_stat_cmd(i)
if not success:
bad_gripper = i
if not success:
rospy.logerr("Failed to contact gripper %d....ABORTING" %
bad_gripper)
return
self._run_driver()
def update_platform_state(self, msg):
self._last_platform_state_message = rospy.get_time()
self._base_status_widget.update_platform_state(msg)
self._arm_status_widget.update_platform_state(msg)
self._drivers_status_widget.update_platform_state(msg)
def get_elapsed(self):
elapsed = rospy.get_time() - self._start_time
return elapsed
def update(self, launched, running, stale, note):
if running and not launched:
rospy.logerr(
'Reported impossible state of running and not launched')
return 0, ''
if stale and running:
rospy.logerr('Reported impossible state of running and stale')
return 0, ''
# Something wrong here, cum seconds not updating
d_seconds = 0
if self._was_running and running:
d_seconds = rospy.get_time() - self._last_update_time
self._cum_seconds += d_seconds
alert = 0 # 0 - None, 1 - Notify, 2 - alert
msg = ''
state = 'Running'
if launched and (not running) and stale:
state = 'Stale'
elif launched and (not running):
state = 'Halted'
elif not launched:
state = 'Stopped'
if (not self._was_launched) and launched:
alert = 1
msg = "Launched."
elif self._was_launched and (not launched):
alert = 1
msg = "Shut down."
elif self._was_running and (not running):
alert = 2
self._num_halts += 1
if stale:
msg = "Stale."
else:
msg = "Stopped."
elif (not self._was_running) and running:
alert = 1
msg = "Restarted."
if alert > 0:
self._num_events += 1
# Update cumulative parameters
for param in self._test._params:
if param.is_rate():
self._cum_data[
param.get_name()] += d_seconds * param.get_value()
self._was_running = running
self._was_launched = launched
self._last_update_time = rospy.get_time()
if alert or note != '' or (
running and self._last_log_time - rospy.get_time() > 7200):
self.write_csv_row(self._last_update_time, state, msg, note)
self._log[rospy.get_time()] = msg + ' ' + note
self._last_log_time = self._last_update_time
return alert, msg
import rospy
from phx_uart_msp_bridge.msg import Diagnostics
rospy.init_node('diagnostics_gui_test')
ros_publish_diagnostics = rospy.Publisher('/diag_out',
Diagnostics,
queue_size=1)
# initialize 'speed'-limit for endless loop
r = rospy.Rate(1)
# start endless loop until rospy.is_shutdown()
while not rospy.is_shutdown():
m = Diagnostics()
m.header.stamp.secs = rospy.get_time()
m.val_a0 = 0
m.val_a1 = np.random.normal()
m.val_a2 = 1
m.val_b0 = np.sin(time.time())
m.val_b1 = np.random.normal()
m.val_b2 = np.cos(time.time())
m.val_c0 = 0
m.val_c1 = np.cos(2. * time.time())
m.val_c2 = 1
ros_publish_diagnostics.publish(m)
print 'published diagnostics'
r.sleep() # this prevents the node from using 100% CPU
def callback(data):
#data.buttons[1] contains button B info (1=pressed, 0=not pressed)
global move
global speed
move = data.buttons[1]
#ascii u=117 (for up) and ascii d=100 (for down)
if move == 1:
#move bin up
seconds = rospy.get_time()
while (rospy.get_time() - seconds) < 5:
#increment speed
if speed < 250:
speed = speed + 10
elif speed == 250:
speed = speed + 5
#write command to arduino
writeNumber(117, speed)
#sleep for one second
rate.sleep()
#print for testing purposes
print "speed: " + str(speed)
print "going up..."
#slow motor to halt
while speed > 0:
#decrement motor
if speed > 5:
speed = speed - 10
elif speed == 5:
speed = 0
#write command to arduino
writeNumber(32, speed)
#sleep for 2 seconds
rate.sleep()
rate.sleep()
#print for testing
print "speed: " + str(speed)
print "slowing down..."
#move bin down
seconds = rospy.get_time()
while (rospy.get_time() - seconds) < 5:
#increment speed
if speed < 250:
speed = speed + 10
elif speed == 250:
speed = speed + 5
#write command to arduino
writeNumber(100, speed)
#sleep for one second
rate.sleep()
#print for testing purposes
print "speed: " + str(speed)
print "going down..."
#slow motor to halt
while speed > 0:
#decrement speed
if speed > 5:
speed = speed - 10
elif speed == 5:
speed = 0
#write command to arduino
writeNumber(32, speed)
#sleep for 2 seconds
rate.sleep()
rate.sleep()
#pring for testing purposes
print "speed: " + str(speed)
print "slowing down..."
#reset move
move = 0
def main():
global new_image
rs_img = rs_process()
rospy.init_node('hand_tracking', anonymous=True)
rospy.loginfo("Hand Detection Start!")
#Marker Publisher Initialize
pub = rospy.Publisher('/hand_marker', Marker)
hand_mark = MarkerGenerator()
hand_mark.type = Marker.SPHERE_LIST
hand_mark.scale = [.04] * 3
hand_mark.frame_id = '/camera_color_optical_frame'
# hand_mark.frame_id = 'iiwa_link_0'
#hand detect args
parser = argparse.ArgumentParser()
parser.add_argument('-sth',
'--scorethreshold',
dest='score_thresh',
type=float,
default=0.2,
help='Score threshold for displaying bounding boxes')
parser.add_argument('-fps',
'--fps',
dest='fps',
type=int,
default=1,
help='Show FPS on detection/display visualization')
parser.add_argument('-src',
'--source',
dest='video_source',
default=0,
help='Device index of the camera.')
parser.add_argument('-wd',
'--width',
dest='width',
type=int,
default=640,
help='Width of the frames in the video stream.')
parser.add_argument('-ht',
'--height',
dest='height',
type=int,
default=360,
help='Height of the frames in the video stream.')
parser.add_argument(
'-ds',
'--display',
dest='display',
type=int,
default=0,
help='Display the detected images using OpenCV. This reduces FPS')
parser.add_argument('-num-w',
'--num-workers',
dest='num_workers',
type=int,
default=4,
help='Number of workers.')
parser.add_argument('-q-size',
'--queue-size',
dest='queue_size',
type=int,
default=5,
help='Size of the queue.')
args = parser.parse_args()
num_hands_detect = 2
im_width, im_height = (args.width, args.height)
#time for fps calculation
start_time = datetime.datetime.now()
num_frames = 0
while not rospy.is_shutdown():
#get rgb,depth frames for synchronized frames
if not new_image:
continue
im_rgb = rs_image_rgb
im_depth = rs_image_depth
new_image = False
#add check
depth_map = cv2.applyColorMap(
cv2.convertScaleAbs(rs_image_depth, alpha=0.03), cv2.COLORMAP_JET)
cv2.imshow("Depth Image", depth_map)
cv2.imshow("Color Image", rs_image_rgb)
try:
image_np = im_rgb #cv2.cvtColor(im_rgb, cv2.COLOR_BGR2RGB)
except:
print("Error converting to RGB")
# Remove background - Set pixels further than clipping_distance to grey
grey_color = 100
depth_image_3d = np.dstack(
(im_depth, im_depth,
im_depth)) #depth image is 1 channel, color is 3 channels
bg_removed = np.where(
(depth_image_3d > clipping_distance) | (depth_image_3d <= 0),
grey_color, image_np)
#
img_hsv = cv2.cvtColor(bg_removed, cv2.COLOR_BGR2HSV)
# lower_red = np.array([120,150,50])
# upper_red = np.array([255,255,180])
## Gen lower mask (0-5) and upper mask (175-180) of RED
mask1 = cv2.inRange(img_hsv, (0, 50, 20), (5, 255, 255))
mask2 = cv2.inRange(img_hsv, (175, 50, 20), (180, 255, 255))
mask3 = cv2.inRange(img_hsv, (120, 150, 50), (255, 255, 180))
## Merge the mask and crop the red regions
mask = cv2.bitwise_or(mask1, mask3)
croped = cv2.bitwise_and(bg_removed, bg_removed, mask=mask)
# croped = cv2.cvtColor(croped, cv2.COLOR_BGR2GRAY)
img_bw = 255 * (cv2.cvtColor(croped, cv2.COLOR_BGR2GRAY) >
10).astype('uint8')
se1 = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
se2 = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
mask4 = cv2.morphologyEx(img_bw, cv2.MORPH_CLOSE, se1)
mask4 = cv2.morphologyEx(mask4, cv2.MORPH_OPEN, se2)
mask5 = np.dstack([mask4, mask4, mask4]) / 255
gray_croped = croped * mask5
# print(gray_croped.shape)
box_index = np.where(gray_croped != 0)
#check if the array is empty or not
if (np.array(box_index).size == 0):
continue
# print(box_index)
i_min = np.amin(box_index[0])
i_max = np.amax(box_index[0])
j_min = np.amin(box_index[1])
j_max = np.amax(box_index[1])
i_centor = int(i_min + i_max) / 2
j_centor = int(j_min + j_max) / 2
box_img = croped[i_min:i_max, j_min:j_max]
print(i_centor, j_centor)
## Display
cv2.imshow("mask", mask)
# cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
#check if the size is valied or not
if (box_img.shape[0] == 0 or box_img.shape[0] == 0
or box_img.shape[0] == 0):
continue
top = j_min
bottom = j_max
left = i_min
right = i_max
align_hand = im_rgb[int(top):int(bottom), int(left):int(right)]
align_depth = depth_map[int(top):int(bottom), int(left):int(right)]
align_hand_detect = np.hstack((align_hand, align_depth))
if (align_hand_detect.shape[0] > 0 and align_hand_detect.shape[1] > 0):
cv2.namedWindow('align hand', cv2.WINDOW_AUTOSIZE)
cv2.imshow('align hand', align_hand_detect)
# cv2.namedWindow("Depth Image", cv2.WINDOW_NORMAL)
# cv2.imshow("Depth Image", im_depth)
# cv2.namedWindow("Color Image", cv2.WINDOW_NORMAL)
# cv2.imshow("Color Image", im_rgb)
cv2.circle(bg_removed, (j_centor, i_centor),
radius=7,
color=(0, 0, 255),
thickness=-1)
cv2.imshow("bg_removed", bg_removed)
print('crop size: ', box_img.shape)
cv2.imshow("croped", box_img)
depth_pixel = [i_centor, j_centor]
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, depth_pixel,
im_depth[depth_pixel[0], depth_pixel[1]] * depth_scale)
print depth_point
hand_mark.counter = 0
t = rospy.get_time()
hand_mark.color = [1, 0, 0, 1]
m = hand_mark.marker(points=[(depth_point[1], depth_point[0],
depth_point[2])])
# rospy.loginfo(m)
pub.publish(m)
# rate.sleep()
if cv2.waitKey(15) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
continue
try:
image_np = im_rgb #cv2.cvtColor(im_rgb, cv2.COLOR_BGR2RGB)
except:
print("Error converting to RGB")
# cv2.imshow("source image np", image_np)
# actual hand detection
boxes, scores = detector_utils.detect_objects(image_np,
detection_graph, sess)
# draw bounding boxes
detector_utils.draw_box_on_image(num_hands_detect, args.score_thresh,
scores, boxes, im_width, im_height,
image_np)
if (scores[0] > args.score_thresh):
(left, right, top,
bottom) = (boxes[0][1] * im_width, boxes[0][3] * im_width,
boxes[0][0] * im_height, boxes[0][2] * im_height)
p1 = (int(left), int(top))
p2 = (int(right), int(bottom))
# print p1,p2,int(left),int(top),int(right),int(bottom)
image_hand = image_np[int(top):int(bottom), int(left):int(right)]
# cv2.namedWindow("hand", cv2.WINDOW_NORMAL)
# cv2.imshow('hand', cv2.cvtColor(image_hand, cv2.COLOR_RGB2BGR))
align_hand = im_rgb[int(top):int(bottom), int(left):int(right)]
align_depth = depth_map[int(top):int(bottom), int(left):int(right)]
align_hand_detect = np.hstack((align_hand, align_depth))
# cv2.namedWindow('align hand', cv2.WINDOW_AUTOSIZE)
# cv2.imshow('align hand', align_hand_detect)
#skin filtering
converted = cv2.cvtColor(align_hand, cv2.COLOR_BGR2HSV)
skinMask = cv2.inRange(converted, lower, upper)
# apply a series of erosions and dilations to the mask
# using an elliptical kernel
# kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (11, 11))
# skinMask = cv2.erode(skinMask, kernel, iterations = 2)
# skinMask = cv2.dilate(skinMask, kernel, iterations = 2)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (7, 7))
skinMask = cv2.erode(skinMask, kernel, iterations=1)
skinMask = cv2.dilate(skinMask, kernel, iterations=1)
# blur the mask to help remove noise, then apply the
# mask to the frame
skinMask = cv2.GaussianBlur(skinMask, (3, 3), 0)
skin = cv2.bitwise_and(align_hand, align_hand, mask=skinMask)
depth_pixel = [(int(left) + int(right)) / 2,
(int(top) + int(bottom)) / 2]
depth_point = rs.rs2_deproject_pixel_to_point(
depth_intrin, depth_pixel,
im_depth[depth_pixel[1], depth_pixel[0]] * depth_scale)
print depth_point
hand_mark.counter = 0
t = rospy.get_time()
hand_mark.color = [1, 0, 0, 1]
m = hand_mark.marker(points=[(depth_point[0], depth_point[1],
depth_point[2])])
# rospy.loginfo(m)
pub.publish(m)
# rate.sleep()
# show the skin in the image along with the mask
# cv2.imshow("images", np.hstack([align_hand, skin]))
#end skin
# Calculate Frames per second (FPS)
num_frames += 1
elapsed_time = (datetime.datetime.now() - start_time).total_seconds()
fps = num_frames / elapsed_time
#display window
if (args.display > 0):
# Display FPS on frame
if (args.fps > 0):
detector_utils.draw_fps_on_image("FPS : " + str(float(fps)),
image_np)
cv2.imshow('Single Threaded Detection',
cv2.cvtColor(image_np, cv2.COLOR_RGB2BGR))
else:
print("frames processed: ", num_frames, "elapsed time: ",
elapsed_time, "fps: ", str(int(fps)))
if cv2.waitKey(10) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
def map_file(self, filename, loops=1):
"""
Loops through csv file
@param filename: the file to play
@param loops: number of times to loop
values < 0 mean 'infinite'
Does not loop indefinitely, but only until the file is read
and processed. Reads each line, split up in columns and
formats each line into a controller command in the form of
name/value pairs. Names come from the column headers
first column is the time stamp
"""
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
rate = rospy.Rate(1000)
if grip_left.error():
grip_left.reset()
if grip_right.error():
grip_right.reset()
if (not grip_left.calibrated() and grip_left.type() != 'custom'):
grip_left.calibrate()
if (not grip_right.calibrated() and grip_right.type() != 'custom'):
grip_right.calibrate()
print("Playing back: %s" % (filename, ))
with open(filename, 'r') as f:
lines = f.readlines()
keys = lines[0].rstrip().split(',')
l = 0
# If specified, repeat the file playback 'loops' number of times
while loops < 1 or l < loops:
i = 0
l += 1
print("Moving to start position...")
_cmd, lcmd_start, rcmd_start, _raw = self.clean_line(
lines[1], keys)
left.move_to_joint_positions(lcmd_start)
right.move_to_joint_positions(rcmd_start)
start_time = rospy.get_time()
for values in lines[1:]:
i += 1
loopstr = str(loops) if loops > 0 else "forever"
sys.stdout.write("\r Record %d of %d, loop %d of %s" %
(i, len(lines) - 1, l, loopstr))
sys.stdout.flush()
cmd, lcmd, rcmd, values = self.clean_line(values, keys)
#command this set of commands until the next frame
while (rospy.get_time() - start_time) < values[0]:
if rospy.is_shutdown():
print("\n Aborting - ROS shutdown")
return False
if len(lcmd):
left.set_joint_positions(lcmd)
if len(rcmd):
right.set_joint_positions(rcmd)
if ('left_gripper' in cmd
and grip_left.type() != 'custom'):
grip_left.command_position(cmd['left_gripper'])
if ('right_gripper' in cmd
and grip_right.type() != 'custom'):
grip_right.command_position(cmd['right_gripper'])
rate.sleep()
print
return True
def HBCB(self, data):
self.last_HB = rospy.get_time()
def callback(message):
#Print the contents of the message to the console
print("Message: %s, Sent at: %f, Received at: %f" %
(message.message, message.timestamp, rospy.get_time()))
def __init__(self):
# 파라미터 설정
self.lin_vel_joy = rospy.get_param('~lin_vel_joy', 0.69)
self.ang_vel_joy = rospy.get_param('~ang_vel_joy', 3.67)
self.camera = rospy.get_param('~camera', 'camera/color/image_raw')
self.spin_cycle = rospy.Duration(rospy.get_param('~spin_cycle', 0.05))
self.scale_arrow = rospy.get_param('~scale_arrow', 50)
self.scale_cross = rospy.get_param('~scale_cross', 30)
# 화면 초기화
os.environ['SDL_VIDEO_WINDOW_POS'] = "0, 0"
pygame.init()
self.monitor = pygame.display.Info()
self.width = rospy.get_param('~width',
int(0.48 * self.monitor.current_w))
self.height = rospy.get_param('~height',
int(0.48 * self.monitor.current_h))
self.screen = pygame.display.set_mode((self.width, self.height))
pygame.mouse.set_visible(False)
pygame.display.set_caption("Shared control interface")
# 토픽 구독
print(C_YELLO + '\rInterfacer, BCI 서비스 준비중...' + C_END)
rospy.Subscriber(self.camera, Image, self.visualize)
self.color = {
'data': [
(255, 223, 36), # default
(255, 223, 36), # M_RIGHT
(255, 223, 36), # M_LEFT
(255, 223, 36), # M_FORWARD
(255, 223, 36),
(255, 223, 36),
(255, 223, 36),
(134, 229, 127)
], # M_MOVE
'time': [rospy.get_time()] * 8
}
# 출력 설정
self.publisher_cmd_intuit = rospy.Publisher('interf/cmd/intuit',
CmdIntuit,
queue_size=1)
self.publisher_cmd_assist = rospy.Publisher('interf/cmd/assist',
CmdAssist,
queue_size=1)
self.publisher_nav_cue = rospy.Publisher('interf/nav_cue',
NavCue,
queue_size=1)
self.publisher_cmd_vel = rospy.Publisher('cmd_vel',
Twist,
queue_size=1)
self.publisher_cmd_joint = rospy.Publisher('cmd_joint',
JointState,
queue_size=1)
# 토픽 구독
self.cmd = CmdIntuit()
self.switch_marker = [False, False, False]
rospy.Subscriber('interf/cmd/intuit', CmdIntuit,
self.update_cmd_intuit)
rospy.Subscriber('interf/robot/motion', RobotMotion,
self.update_marker_color)
rospy.Subscriber('interf/nav_cue', NavCue,
self.update_marker_visibility)
rospy.Subscriber('joy', Joy, self.joystick)
self.path = []
rospy.Subscriber('robot/pose', PoseWithCovarianceStamped,
self.update_robot_pose)
# 서비스 시작
self.publisher_time_start = rospy.Publisher('time/start',
Time,
queue_size=1)
self.publisher_time_end = rospy.Publisher('time/end',
Time,
queue_size=1)
self.time_start = rospy.Time.now()
self.the_timer = rospy.Timer(rospy.Duration(0.1), self.timer)
self.path_publisher = rospy.Publisher('interf/path',
MarkerArray,
queue_size=1)
self.path_visualizer = rospy.Timer(rospy.Duration(0.3),
self.visualize_path)
rospy.Service('interf/nav2cmd', Nav2Cmd, self.nav2cmd)
self.key_watcher = rospy.Timer(self.spin_cycle, self.keyboard)
print(C_YELLO + '\rInterfacer, BCI 서비스 시작' + C_END)
print(C_GREEN + '\rInterfacer, 초기화 완료' + C_END)
#=========================#
# Make States Message #
#=========================#
states = States()
states.x = center[0]
states.y = center[1]
states.z = center[2]
states.phi = phi
states.theta = theta
states.psi = psi
States.name = marker_array.name
#===================#
# Main #
#===================#
if __name__=='__main__':
import sys
rospy.init_node('Mocap_State_Estimator')
time_past = rospy.get_time()
#=====================================#
# Set up Publish/Subscribe Loop #
#=====================================#
sub = rospy.Subscriber('/mocap_data' , Mocap_data, GetMocapData)
rospy.spin()
def is_stale(self):
return rospy.get_time() - self.update_time > 5
def update_marker_color(self, data):
self.color['time'][data.motion] = rospy.get_time()
if (data.motion == M_RIGHT) or (data.motion
== M_LEFT) or (data.motion
== M_FORWARD):
self.color['data'][data.motion] = (241, 95, 95)
def move():
rospy.init_node('target_pos_cmd', anonymous=True)
rate = rospy.Rate(30) # 30hz
# initialize a publisher to send joints' angular position to the robot
robot_joint1_pub = rospy.Publisher("/target/x_position_controller/command",
Float64,
queue_size=10)
robot_joint2_pub = rospy.Publisher("/target/y_position_controller/command",
Float64,
queue_size=10)
robot_joint3_pub = rospy.Publisher("/target/z_position_controller/command",
Float64,
queue_size=10)
robot_joint4_pub = rospy.Publisher(
"/target2/x2_position_controller/command", Float64, queue_size=10)
robot_joint5_pub = rospy.Publisher(
"/target2/y2_position_controller/command", Float64, queue_size=10)
robot_joint6_pub = rospy.Publisher(
"/target2/z2_position_controller/command", Float64, queue_size=10)
# publish the trajectory of the box in form of an array
self.box_trajectory = rospy.Publisher("/target2/box",
Float64MultiArray,
queue_size=10)
t0 = rospy.get_time()
while not rospy.is_shutdown():
cur_time = np.array([rospy.get_time()]) - t0
#y_d = float(6 + np.absolute(1.5* np.sin(cur_time * np.pi/100)))
x_d = 2.5 * np.cos(cur_time * np.pi / 15)
y_d = 2.5 * np.sin(cur_time * np.pi / 15)
z_d = 1 * np.sin(cur_time * np.pi / 15)
joint1 = Float64()
joint1.data = 0.5 + x_d
joint2 = Float64()
joint2.data = 0 + y_d
joint3 = Float64()
joint3.data = 7 + z_d
robot_joint1_pub.publish(joint1)
robot_joint2_pub.publish(joint2)
robot_joint3_pub.publish(joint3)
x_d = 2 + 2 * np.cos(cur_time * np.pi / 15)
y_d = 2.5 + 1.5 * np.sin(cur_time * np.pi / 15)
################### publish the trajectory of the box in form of an array Question 4.2
target2_trajectory = Float64MultiArray()
target2_trajectory.data = np.array(x_d, y_d, 7.5)
box_trajectory.publish(target2_trajectory)
##################
joint4 = Float64()
joint4.data = x_d
joint5 = Float64()
joint5.data = y_d
joint6 = Float64()
joint6.data = 7.5
robot_joint4_pub.publish(joint4)
robot_joint5_pub.publish(joint5)
robot_joint6_pub.publish(joint6)
rate.sleep()
def update(self, msg):
self.level = msg.level
self.message = msg.message
self.update_time = rospy.get_time()
pivoting = True
else:
pivoting = False
rospy.Subscriber("/cmd_vel", Twist, cmd_vel_callback)
seq = 1
# Cumulative offsets
abs_x = 0
abs_y = 0
abs_x_m = 0
abs_y_m = 0
last_time = rospy.get_time()
def sensor_reset():
opti_flow_reset.on()
sleep(0.020) # reset pulse >10us
opti_flow_reset.off()
sleep(0.050) # 35ms from reset to functional
def sensor_init():
opti_flow_cs.on()
sensor_reset()
sleep(0.1)
sensor_reset()
def __init__(self, msg):
self.name = get_raw_name(msg.name)
self.level = msg.level
self.message = msg.message
self.update_time = rospy.get_time()
def motion(rx0=2.0, ry0=2.0, rz0=0.172, ro0=0.0):
# ---------------------------------- INITS ----------------------------------------------
# --- init node ---
rospy.init_node('hector_motion')
# --- cache global vars / constants ---
global msg_stop
msg_stop = None
# --- Service: Calibrate ---
calibrate_imu = rospy.ServiceProxy('/hector/raw_imu/calibrate', Empty)
calibrate_imu()
print('[HEC MOTION] Imu calibrated')
# --- Publishers ---
pub_motion = rospy.Publisher('/hector/motion',
EE4308MsgMotion,
latch=True,
queue_size=1)
msg_motion = EE4308MsgMotion()
pub_motion.publish(msg_motion)
pub_pose = rospy.Publisher('/hector/motion_pose',
PoseStamped,
latch=True,
queue_size=1) # for rviz
msg_pose = PoseStamped()
msg_pose.header.frame_id = "map"
msg_pose_position = msg_pose.pose.position
msg_pose_orientation = msg_pose.pose.orientation
# --- Subscribers ---
rospy.Subscriber('/hector/ground_truth_to_tf/pose',
PoseStamped,
subscribe_true,
queue_size=1)
rospy.Subscriber('/hector/fix', NavSatFix, subscribe_gps, queue_size=1)
rospy.Subscriber('/hector/raw_imu', Imu, subscribe_imu, queue_size=1)
rospy.Subscriber('/hector/magnetic',
Vector3Stamped,
subscribe_magnetic,
queue_size=1)
rospy.Subscriber('/hector/altimeter',
Altimeter,
subscribe_altimeter,
queue_size=1)
rospy.Subscriber('/hector/stop', Bool, subscribe_stop, queue_size=1)
while (not rbt_true[-1] or not rbt_imu[-1] or not rbt_gps[-1] or not rbt_magnet[-1] or\
rospy.get_time() == 0 or not rbt_alt[-1] or msg_stop is None) and not rospy.is_shutdown():
pass
if rospy.is_shutdown():
return
print('=== [HEC MOTION] Initialised ===')
# ---------------------------------- LOOP ----------------------------------------------
if USE_GROUND_TRUTH:
t = rospy.get_time()
while not rospy.is_shutdown() and not msg_stop:
if rospy.get_time() > t:
# publish true motion
msg_motion.x = rbt_true[0]
msg_motion.y = rbt_true[1]
msg_motion.z = rbt_true[2]
msg_motion.o = rbt_true[3]
pub_motion.publish(msg_motion)
# publish results to rviz
msg_pose_position.x = msg_motion.x
msg_pose_position.y = msg_motion.y
msg_pose_position.z = msg_motion.z
tmp = quaternion_from_euler(0, 0, msg_motion.o)
msg_pose_orientation.x = tmp[0]
msg_pose_orientation.y = tmp[1]
msg_pose_orientation.z = tmp[2]
msg_pose_orientation.w = tmp[3]
pub_pose.publish(msg_pose)
# iterate
t += ITERATION_PERIOD
else:
##########################################################
# --- build sensor functions to calculate conversions ---
pass
# --- EKF inits ---
# always initialise arrays in two dimensions using [[ . ]]:
# e.g. X = array([[rx0], [0.]]) # see above for rx0
# e.g. Px = [[0, 0], [0, 0]]
pass
# --- Loop EKF ---
t = rospy.get_time()
while not rospy.is_shutdown() and not msg_master.stop:
if rospy.get_time() > t:
# --- Prediction: from IMU ---
# x, y, z, o
pass
# --- Correction: incorporate measurements from GPS, Baro (altimeter) and Compass ---
# separately and when available
pass
# --- Publish motion ---
msg_motion.x = 0 # m
msg_motion.y = 0 # m
msg_motion.z = 0 # m
msg_motion.o = 0 # orientation in z (rad)
pub_motion.publish(msg_motion)
# publish results to rviz
msg_pose_position.x = msg_motion.x
msg_pose_position.y = msg_motion.y
msg_pose_position.z = msg_motion.z
tmp = quaternion_from_euler(0, 0, msg_motion.o)
msg_pose_orientation.x = tmp[0]
msg_pose_orientation.y = tmp[1]
msg_pose_orientation.z = tmp[2]
msg_pose_orientation.w = tmp[3]
pub_pose.publish(msg_pose)
# --- Iterate ---
et = rospy.get_time() - t
t += ITERATION_PERIOD
if et > ITERATION_PERIOD:
print('[HEC MOTION] {}ms OVERSHOOT'.format(int(et * 1000)))
def get_data():
global seq, abs_x, abs_y, abs_x_m, abs_y_m, last_time
seq += 1
m = sensor_read_motion()
if m.motion & 0x10:
print "Overflow"
qualityMsg.data = m.squal
qualityPub.publish(qualityMsg)
# if m.squal < 90:
# led_lighting.on()
# if m.squal > 90:
# led_lighting.off()
abs_x += m.dx
abs_y += m.dy
time_diff = rospy.get_time() - last_time
last_time = rospy.get_time()
# Convert the counts reading into metres
abs_x_m = (float(abs_x) / ADNS3080_COUNTS_PER_METER)
abs_y_m = (float(abs_y) / ADNS3080_COUNTS_PER_METER)
speed_x_m = (float(m.dx) / ADNS3080_COUNTS_PER_METER) / time_diff
speed_y_m = (float(m.dy) / ADNS3080_COUNTS_PER_METER) / time_diff
# print str(speed_x_m) + ", " + str(speed_y_m)
# print m.squal
# Y forward and back
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "odom"
msg.child_frame_id = "base_link"
pose = Pose()
pose.position.x = abs_y_m
pose.position.y = abs_x_m
pose.position.z = 0
poseC = PoseWithCovariance()
poseC.pose = pose
poseC.covariance = [-1] * 36
msg.pose = poseC
# If we are pivoting the sensor doesnt work properly so don't output anything
if pivoting:
speed_y_m = 0
twist = Twist()
twist.linear.x = speed_y_m
twist.linear.y = 0
msg.twist.twist = twist
msg.twist.covariance = [0.01] * 36
odomPub.publish(msg)
rate.sleep()
def PID(x, y, z, xVel, yVel, zVel, roll, pitch, yaw, f):
# Set variables as global to prevent new assignment
# P, I and D proportionality constants for position in the xyz axes
global kpx, kix, kdx
global kpy, kiy, kdy
global kpz, kiz, kdz
# P, I and D proportionality constants for velocity in the xyz axes
global kpvelx, kivelx, kdvelx
global kpvely, kively, kdvely
global kpvelz, kivelz, kdvelz
# P, I and D proportionality constants for roll, pitch and yaw
global kproll, kiroll, kdroll
global kppitch, kipitch, kdpitch
global kpyaw, kiyaw, kdyaw
# Errors for Parameters
global prevErrorRoll, prevErrorPitch, prevErrorYaw
global prevErrorx, prevErrory, prevErrorz
global prevErrorVelx, prevErrorVely, prevErrorVelz
# P, I and D for position in xyz axes
global P_x, I_x, D_x
global P_y, I_y, D_y
global P_z, I_z, D_z
# P, I and D for velocity in the xyz axes
global P_velx, I_velx, D_velx
global P_vely, I_vely, D_vely
global P_velz, I_velz, D_velz
# P, I and D for roll, pitch and yaw
global P_roll, I_roll, D_roll
global P_pitch, I_pitch, D_pitch
global P_yaw, I_yaw, D_yaw
global setPointRoll, setPointPitch, setPointYaw
global setPointx, setPointz, setPointz
global flag, sampleTime
t = rospy.get_time()
currTime = time.time()
print(t)
'''
kproll = 20
kiroll = 0
kdroll = 89
kppitch = kproll
kipitch = kiroll
kdpitch = kdroll
'''
# PID constants
kpyaw = 70
kiyaw = 0
kdyaw = 0
sampleTime = 0
kpx = 70
kix = 0.0002
kdx = 89
kpy = 79
kiy = 0.0002
kdy = 89
#tuned
kpz = 1500
kiz = 0
kdz = 0
kpvelx = 100
kivelx = -0.0001
kdvelx = 200
kpvely = 200
kively = -0.0001
kdvely = 100
kpvelz = 25
kivelz = 0
kdvelz = 0
setPointYaw = 0
errYaw = math.degrees(float(yaw)) - setPointYaw
setPointx = 0
setPointy = 0
setPointz = 3.275
errx = x - setPointx
erry = y - setPointy
errz = z - setPointz
flag = 0
if flag == 0: # What purpose does this serve? flag was set to 0 just 2 statements back
prevTime = 0
prevErrorRoll = 0
prevErrorPitch = 0
prevErrorYaw = 0
prevErrorx = 0
prevErrory = 0
prevErrorz = 0
prevErrorVelx = 0
prevErrorVely = 0
prevErrorVelz = 0
P_roll = 0
P_pitch = 0
P_yaw = 0
P_x = 0
P_y = 0
P_z = 0
P_velx = 0
P_vely = 0
P_velz = 0
I_roll = 0
I_pitch = 0
I_yaw = 0
I_x = 0
I_y = 0
I_z = 0
I_velx = 0
I_vely = 0
I_velz = 0
D_roll = 0
D_pitch = 0
D_yaw = 0
D_x = 0
D_y = 0
D_z = 0
D_velx = 0
D_vely = 0
D_velz = 0
flag = 1
dt = currTime - prevTime
if dt >= sampleTime:
P_x = computeP(kpx, errx)
P_y = computeP(kpy, erry)
P_z = computeP(kpz, errz)
I_x = computeI(kix, I_x, errx, dt, 600, -600)
I_y = computeI(kiy, I_y, erry, dt, 600, -600)
I_z = computeI(kiz, I_z, errz, dt, 600, -600)
D_x = computeD(kdx, errx, prevErrorx, dt)
D_y = computeD(kdy, erry, prevErrory, dt)
D_z = computeD(kdy, errz, prevErrorz, dt)
P_yaw = computeP(kpyaw, errYaw)
I_yaw = computeI(kiyaw, I_yaw, errYaw, dt, 600, -600)
D_yaw = computeD(kdyaw, errYaw, prevErrorYaw, dt)
#desVelx = -0.1 #P_x + I_x + D_x
#desVely = -0.1 #P_y + I_y + D_y
'''
#Ignore this, it was for a temporary purpose to plan trajectory in probably the worst way possible!
#Horizontal demo 1
if(t<=27):
desVelx = -0.1 #P_x + I_x + D_x
desVely = -0.1 #P_y + I_y + D_y
if(t>27 and t<=28.5):
desVelx = -0.6
desVely = -1.9
if(t>28.5 and t<=30):
desVelx = -0.6
desVely = 3.5
if(t>30 and t<=40):
desVelx = -2.5
desVely = 7
if(t>40):
desVelx = +5
desVely = +6
'''
#Horizontal demo 2
if (t <= 34):
desVelx = -0.5 #P_x + I_x + D_x
desVely = 0.5 #P_y + I_y + D_y
if (t > 34 and t <= 35):
desVelx = -0.8
desVely = 1.3
if (t > 35 and t <= 36.5):
desVelx = -0.8
desVely = -7
if (t > 36.5 and t <= 48):
desVelx = -2.7
desVely = -7
if (t > 48):
desVelx = 0.0
desVely = 0.0
desVelz = P_z + I_z + D_z
newYaw = P_yaw + I_yaw + D_yaw
errVelx = xVel - desVelx
errVely = yVel - desVely
errVelz = zVel - desVelz
if (dt >= sampleTime):
P_velx = computeP(kpvelx, errVelx)
P_vely = computeP(kpvely, errVely)
P_velz = computeP(kpvelz, errVelz)
I_velx = computeI(kivelx, I_velx, errVelx, dt, 600, -600)
I_vely = computeI(kively, I_vely, errVely, dt, 600, -600)
I_velz = computeI(kivelz, I_velz, errVelz, dt, 600, -600)
D_velx = computeD(kdvelx, errVelx, prevErrorVelx, dt)
D_vely = computeD(kdvely, errVely, prevErrorVely, dt)
D_velz = computeD(kdvelz, errVelz, prevErrorVelz, dt)
newAccx = P_velx + I_velx + D_velx
newAccy = P_vely + I_vely + D_vely
newThrottle = P_velz + I_velz + D_velz
newRoll = -newAccy / 9.8
newPitch = newAccx / 9.8
errRoll = newRoll - roll
errYaw = newYaw - yaw
errPitch = newPitch - pitch
prevTime = currTime
prevErrorx = errx
prevErrory = erry
prevErrorz = errz
prevErrorYaw = errYaw
prevErrorVelx = errVelx
prevErrorVely = errVely
prevErrorVelz = errVelz
esc_br = 1500 - desVelz + newPitch + 0.5 * newRoll + newYaw
esc_fr = 1500 - desVelz - newPitch + 0.5 * newRoll + newYaw
esc_fl = 1500 - desVelz - newPitch - 0.5 * newRoll - newYaw
esc_bl = 1500 - desVelz + newPitch - 0.5 * newRoll - newYaw
esc_r = 1500 - desVelz + newRoll - newYaw
esc_l = 1500 - desVelz - newRoll + newYaw
'''
Ignore this...
esc_br = 1500 - newRoll - newPitch + newYaw + newThrottle
esc_bl = 1500 + newRoll - newPitch - newYaw + newThrottle
esc_fl = 1500 + newRoll + newPitch - newYaw + newThrottle
esc_fr = 1500 - newRoll + newPitch + newYaw + newThrottle
esc_r = 1500 - newRoll - newYaw + newThrottle
esc_l = 1500 + newRoll + newYaw + newThrottle
esc_br = 1500 + newPitch + newRoll + newYaw
esc_br = 1500 + newRoll + newPitch + newYaw + newThrottle
esc_bl = 1500 - newRoll + newPitch - newYaw + newThrottle
esc_fl = 1500 - newRoll - newPitch + newYaw + newThrottle
esc_fr = 1500 + newRoll - newPitch - newYaw + newThrottle
esc_r = 1500 - newRoll + newPitch - newYaw + newThrottle
esc_l = 1500 + newRoll - newPitch + newYaw + newThrottle
esc_br = newthrottle + newPitch + newRoll + newYaw
esc_fr = newThrottle - newPitch + newRoll + newYaw
esc_fl = newThrottle - newPitch - newRoll - newYaw
esc_bl = newThrottle + newPitch - newRoll - newYaw
esc_r = newThrottle + newRoll - newYaw
esc_l = newThrottle - newRoll + newYaw
'''
if (esc_br > 2000): esc_br = 2000
if (esc_bl > 2000): esc_bl = 2000
if (esc_fr > 2000): esc_fr = 2000
if (esc_fl > 2000): esc_fl = 2000
if (esc_r > 2000): esc_r = 2000
if (esc_l > 2000): esc_l = 2000
if (esc_br < 1100): esc_br = 1100
if (esc_bl < 1100): esc_bl = 1100
if (esc_fr < 1100): esc_fr = 1100
if (esc_fl < 1100): esc_fl = 1100
if (esc_r < 1100): esc_r = 1100
if (esc_l < 1100): esc_l = 1100
br_motor_vel = ((esc_br - 1500) / 25) + 50
bl_motor_vel = ((esc_bl - 1500) / 25) + 50
fr_motor_vel = ((esc_fr - 1500) / 25) + 50
fl_motor_vel = ((esc_fl - 1500) / 25) + 50
r_motor_vel = ((esc_r - 1500) / 25) + 50
l_motor_vel = ((esc_l - 1500) / 25) + 50
f.data = [
fr_motor_vel, -fl_motor_vel, l_motor_vel, -bl_motor_vel, br_motor_vel,
-r_motor_vel
]
return f, errRoll, errPitch, errYaw, errx, erry, errz
def running(self):
receive_object_pose = PoseStamped()
receive_object_pose.header.frame_id = 'base_link'
if self.goal.context_aware:
# > Pick context-dependent receive object position:
policy_parameter_a = self.receive_object_position_policy_parameters[
0][0]
policy_parameter_A = self.receive_object_position_policy_parameters[
0][1]
if self.goal.posture_type == 'standing':
context_vector = np.array([0.0, 0.0, 0.4])
elif self.goal.posture_type == 'seated':
context_vector = np.array([0.5, 0.0, 0.0])
elif self.goal.posture_type == 'lying':
context_vector = np.array([0.0, 0.7, 0.0])
# Sample upper-level policy (with no exploration) for object reception position;
# by calculating the mean of the linear-Gaussian model, given context vector s
receive_object_position = np.squeeze(
policy_parameter_a + context_vector.dot(policy_parameter_A))
receive_object_pose.pose.position.x = receive_object_position[0]
receive_object_pose.pose.position.y = receive_object_position[1]
receive_object_pose.pose.position.z = receive_object_position[2]
else:
# Pick context-independent object reception position
receive_object_pose.pose.position.x = 0.5
receive_object_pose.pose.position.y = 0.078
receive_object_pose.pose.position.z = 0.8
receive_object_pose.pose.orientation.x = 0.000
receive_object_pose.pose.orientation.y = 0.000
receive_object_pose.pose.orientation.z = 0.000
receive_object_pose.pose.orientation.w = 1.000
self.goal.person_pose = self.tf_listener.transformPose(
"base_link", self.goal.person_pose)
distance_to_person = np.sqrt(self.goal.person_pose.pose.position.x**2 +
self.goal.person_pose.pose.position.y**2)
if distance_to_person > self.person_dist_threshold:
move_to_person_goal = MoveBaseGoal()
move_to_person_goal.goal_type = MoveBaseGoal.POSE
move_to_person_goal.pose.header.frame_id = self.goal.person_pose.header.frame_id
move_to_person_goal.pose.pose.position.x = self.goal.person_pose.pose.position.x - 0.3
move_to_person_goal.pose.pose.position.y = self.goal.person_pose.pose.position.y - 0.3
rospy.loginfo('[receive_object] Moving to person...')
self.move_base_client.send_goal(move_to_person_goal)
# we try moving towards the person before receiving the object;
# the person should be close by, so we move for a short time
# since the robot will either reach the person in that time,
# or will stop as close to the person as possible
self.move_base_client.wait_for_result(rospy.Duration(10.))
self.move_base_client.cancel_goal()
# Start with arm in neutral position:
rospy.loginfo('[receive_object] Moving arm to neutral position...')
self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
# Move to chosen receive_object position, along appropriate trajectory:
rospy.loginfo('[receive_object] Receiving object...')
self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, receive_object_pose)
if not self.goal.reception_detection:
# Naive object release strategy:
rospy.loginfo(
'[receive_object] Waiting before releasing object...')
rospy.sleep(5.)
# Release the object:
rospy.loginfo('[receive_object] Opening the gripper...')
self.gripper.open()
# Since no detection is used here, the object is released and we always set the reception to True.
self.object_reception_detected = True
else:
# Force sensing object release strategy:
rospy.sleep(1.)
rospy.loginfo(
'[receive_object] Waiting for object to be received...')
rospy.Subscriber(self.force_sensor_topic, WrenchStamped,
self.force_sensor_cb)
self.object_reception_detected = False
self.cumsum_z = 0.
start_time = rospy.get_time()
while (rospy.get_time() -
start_time) < 50 and not self.object_reception_detected:
self.detect_object_reception()
rospy.sleep(0.1)
# If a pull is detected, release the object:
if self.object_reception_detected:
rospy.loginfo(
'[receive_object] Closing the gripper to receive the object...'
)
self.gripper.close()
else:
rospy.loginfo(
'[receive_object] Not waiting anymore since no reception was detected'
)
# Return to a neutral arm position:
rospy.loginfo('[receive_object] Moving back to neutral position...')
self.__move_arm(MoveArmGoal.NAMED_TARGET, self.init_config_name)
if self.object_reception_detected:
self.result = self.set_result(True)
else:
self.result = self.set_result(False)
return FTSMTransitions.DONE
def main():
rospy.init_node("PID_controller_test")
Rate = rospy.Rate(200)
# 类实例化
udp = from_udp('left')
endpoint_pose_init = udp.controller.limb.endpoint_pose()
endpoint_pose = endpoint_pose_init['position']
pose_init = udp.controller.limb.joint_angles()
print pose_init
joint_goal_init = udp.controller.limb.joint_angles()
joint_angles_goal_list = [
[0.938, 1.491, 2.167, -1.348, 1.393, -0.466, -0.586],
[0.939, 1.491, 2.169, -1.349, 1.393, -0.464, -0.586],
[0.939, 1.492, 2.174, -1.35, 1.395, -0.458, -0.586],
[0.94, 1.493, 2.18, -1.351, 1.397, -0.45, -0.586],
[0.941, 1.493, 2.188, -1.353, 1.4, -0.44, -0.587],
[0.943, 1.494, 2.196, -1.354, 1.402, -0.429, -0.587],
[0.945, 1.494, 2.202, -1.355, 1.404, -0.418, -0.587],
[0.947, 1.493, 2.209, -1.357, 1.405, -0.408, -0.587],
[0.949, 1.493, 2.215, -1.358, 1.407, -0.397, -0.587],
[0.95, 1.492, 2.222, -1.359, 1.408, -0.386, -0.587],
[0.951, 1.492, 2.23, -1.36, 1.41, -0.375, -0.586],
[0.952, 1.492, 2.24, -1.361, 1.413, -0.362, -0.586],
[0.952, 1.493, 2.251, -1.363, 1.416, -0.35, -0.586],
[0.952, 1.493, 2.263, -1.365, 1.42, -0.336, -0.586],
[0.951, 1.494, 2.275, -1.367, 1.423, -0.323, -0.586],
[0.951, 1.494, 2.287, -1.369, 1.427, -0.308, -0.586],
[0.952, 1.494, 2.298, -1.371, 1.431, -0.294, -0.586],
[0.953, 1.494, 2.309, -1.373, 1.434, -0.279, -0.586],
[0.954, 1.493, 2.32, -1.375, 1.437, -0.264, -0.586],
[0.955, 1.493, 2.331, -1.377, 1.441, -0.249, -0.586],
[0.956, 1.493, 2.341, -1.379, 1.444, -0.234, -0.586],
[0.957, 1.493, 2.352, -1.381, 1.448, -0.218, -0.586],
[0.958, 1.493, 2.363, -1.383, 1.451, -0.203, -0.586],
[0.959, 1.494, 2.374, -1.385, 1.455, -0.187, -0.586],
[0.96, 1.494, 2.385, -1.387, 1.458, -0.172, -0.586],
[0.961, 1.494, 2.396, -1.389, 1.462, -0.156, -0.586],
[0.962, 1.494, 2.407, -1.391, 1.465, -0.14, -0.586],
[0.963, 1.494, 2.418, -1.393, 1.469, -0.124, -0.587],
[0.964, 1.494, 2.43, -1.395, 1.472, -0.108, -0.587],
[0.966, 1.494, 2.441, -1.398, 1.475, -0.092, -0.587],
[0.966, 1.493, 2.452, -1.4, 1.479, -0.076, -0.587],
[0.967, 1.493, 2.463, -1.402, 1.482, -0.06, -0.587],
[0.968, 1.493, 2.474, -1.404, 1.486, -0.044, -0.587],
[0.968, 1.493, 2.484, -1.406, 1.489, -0.028, -0.587],
[0.968, 1.494, 2.495, -1.409, 1.493, -0.012, -0.587],
[0.969, 1.494, 2.506, -1.411, 1.496, 0.005, -0.587],
[0.97, 1.494, 2.517, -1.413, 1.499, 0.021, -0.587],
[0.971, 1.494, 2.529, -1.415, 1.503, 0.037, -0.587],
[0.972, 1.494, 2.54, -1.418, 1.506, 0.053, -0.587],
[0.973, 1.495, 2.552, -1.42, 1.509, 0.069, -0.587],
[0.974, 1.494, 2.563, -1.422, 1.512, 0.085, -0.588],
[0.975, 1.494, 2.574, -1.424, 1.515, 0.1, -0.588],
[0.976, 1.494, 2.584, -1.426, 1.518, 0.115, -0.588],
[0.977, 1.494, 2.593, -1.427, 1.521, 0.13, -0.588],
[0.978, 1.494, 2.603, -1.429, 1.524, 0.143, -0.588],
[0.978, 1.495, 2.612, -1.431, 1.527, 0.156, -0.588],
[0.979, 1.497, 2.621, -1.433, 1.529, 0.169, -0.588],
[0.98, 1.498, 2.629, -1.434, 1.532, 0.18, -0.588],
[0.98, 1.5, 2.637, -1.436, 1.534, 0.191, -0.588],
[0.981, 1.502, 2.643, -1.438, 1.536, 0.202, -0.588],
[0.981, 1.503, 2.649, -1.439, 1.539, 0.211, -0.588],
[0.981, 1.503, 2.654, -1.441, 1.541, 0.22, -0.588],
[0.981, 1.502, 2.658, -1.442, 1.543, 0.229, -0.588],
[0.982, 1.5, 2.661, -1.443, 1.545, 0.237, -0.588],
[0.982, 1.498, 2.663, -1.443, 1.546, 0.244, -0.588],
[0.982, 1.496, 2.666, -1.444, 1.548, 0.25, -0.588],
[0.982, 1.493, 2.669, -1.444, 1.549, 0.256, -0.588],
[0.982, 1.491, 2.672, -1.445, 1.55, 0.26, -0.588],
[0.982, 1.488, 2.675, -1.445, 1.55, 0.264, -0.588],
[0.982, 1.486, 2.677, -1.446, 1.551, 0.268, -0.588],
[0.983, 1.485, 2.68, -1.446, 1.552, 0.271, -0.588],
[0.983, 1.485, 2.683, -1.447, 1.552, 0.273, -0.587],
[0.983, 1.485, 2.685, -1.448, 1.553, 0.276, -0.587],
[0.983, 1.486, 2.687, -1.448, 1.554, 0.278, -0.587],
[0.983, 1.486, 2.689, -1.449, 1.554, 0.28, -0.587],
[0.984, 1.487, 2.69, -1.45, 1.555, 0.282, -0.587],
[0.984, 1.487, 2.691, -1.451, 1.556, 0.283, -0.587],
[0.984, 1.486, 2.691, -1.451, 1.556, 0.285, -0.587],
[0.984, 1.485, 2.691, -1.452, 1.557, 0.286, -0.587],
[0.984, 1.485, 2.691, -1.452, 1.557, 0.287, -0.587],
[0.984, 1.484, 2.691, -1.453, 1.558, 0.288, -0.588],
[0.985, 1.484, 2.692, -1.454, 1.558, 0.289, -0.588],
[0.985, 1.485, 2.692, -1.454, 1.559, 0.29, -0.588],
[0.985, 1.485, 2.692, -1.455, 1.559, 0.29, -0.588],
[0.985, 1.486, 2.693, -1.455, 1.559, 0.291, -0.588],
[0.985, 1.486, 2.693, -1.455, 1.56, 0.291, -0.588],
[0.985, 1.486, 2.693, -1.455, 1.56, 0.291, -0.587],
[0.985, 1.486, 2.693, -1.455, 1.56, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.455, 1.56, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.455, 1.56, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.455, 1.56, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.455, 1.561, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.456, 1.562, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.456, 1.562, 0.291, -0.587],
[0.984, 1.486, 2.693, -1.456, 1.563, 0.291, -0.587],
[0.984, 1.486, 2.693, -1.456, 1.563, 0.291, -0.588],
[0.984, 1.485, 2.693, -1.456, 1.563, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.456, 1.562, 0.291, -0.587],
[0.985, 1.484, 2.693, -1.455, 1.56, 0.291, -0.587],
[0.985, 1.485, 2.693, -1.454, 1.562, 0.291, -0.587],
[0.986, 1.485, 2.693, -1.453, 1.562, 0.292, -0.586],
[0.986, 1.485, 2.693, -1.451, 1.562, 0.292, -0.587],
[0.987, 1.485, 2.693, -1.456, 1.562, 0.292, -0.587],
[0.988, 1.485, 2.693, -1.456, 1.562, 0.293, -0.587],
[0.989, 1.485, 2.693, -1.456, 1.562, 0.294, -0.587],
[0.991, 1.485, 2.693, -1.456, 1.562, 0.295, -0.587],
[0.993, 1.485, 2.693, -1.456, 1.562, 0.296, -0.587],
[0.995, 1.485, 2.692, -1.456, 1.562, 0.297, -0.587],
[0.997, 1.485, 2.692, -1.456, 1.562, 0.299, -0.587],
[1.0, 1.485, 2.692, -1.456, 1.562, 0.3, -0.587],
[1.003, 1.485, 2.692, -1.456, 1.562, 0.302, -0.587]
]
joint_vel_goal_list = [
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.016, 0.023, 0.172, -0.036, 0.06, 0.206, -0.0],
[0.033, 0.036, 0.293, -0.06, 0.1, 0.363, -0.0],
[0.05, 0.038, 0.365, -0.074, 0.12, 0.471, -0.01],
[0.069, 0.029, 0.388, -0.078, 0.121, 0.53, -0.0],
[0.088, 0.009, 0.36, -0.07, 0.1, 0.54, -0.0],
[0.101, -0.012, 0.322, -0.059, 0.077, 0.53, 0.0],
[0.103, -0.023, 0.311, -0.053, 0.067, 0.53, 0.004],
[0.094, -0.025, 0.328, -0.053, 0.07, 0.541, 0.0],
[0.073, -0.018, 0.374, -0.057, 0.086, 0.561, 0.0],
[0.041, -0.001, 0.447, -0.066, 0.116, 0.591, 0.0],
[0.009, 0.016, 0.522, -0.076, 0.148, 0.624, 0.0],
[-0.009, 0.026, 0.575, -0.086, 0.17, 0.654, 0.0],
[-0.015, 0.027, 0.603, -0.093, 0.184, 0.679, 0.003],
[-0.007, 0.021, 0.608, -0.1, 0.188, 0.701, 0.0],
[0.013, 0.007, 0.59, -0.105, 0.184, 0.719, -0.0],
[0.036, -0.008, 0.564, -0.108, 0.176, 0.734, -0.0],
[0.052, -0.016, 0.544, -0.11, 0.171, 0.746, -0.005],
[0.061, -0.018, 0.532, -0.109, 0.169, 0.755, -0.005],
[0.062, -0.014, 0.527, -0.106, 0.169, 0.762, -0.004],
[0.056, -0.003, 0.529, -0.101, 0.173, 0.765, -0.002],
[0.048, 0.009, 0.535, -0.097, 0.176, 0.768, 0.0],
[0.043, 0.015, 0.541, -0.094, 0.178, 0.771, 0.001],
[0.042, 0.017, 0.548, -0.094, 0.178, 0.776, 0.001],
[0.045, 0.014, 0.554, -0.095, 0.176, 0.782, 0.0],
[0.051, 0.005, 0.562, -0.099, 0.173, 0.789, -0.002],
[0.057, -0.004, 0.567, -0.104, 0.169, 0.796, -0.004],
[0.059, -0.009, 0.568, -0.107, 0.167, 0.801, -0.0],
[0.056, -0.012, 0.566, -0.109, 0.167, 0.804, -0.0],
[0.05, -0.011, 0.559, -0.11, 0.168, 0.805, -0.004],
[0.039, -0.007, 0.548, -0.11, 0.17, 0.805, -0.002],
[0.028, -0.002, 0.539, -0.11, 0.173, 0.804, 0.0],
[0.023, 0.003, 0.535, -0.11, 0.174, 0.804, 0.001],
[0.022, 0.006, 0.537, -0.111, 0.174, 0.806, 0.001],
[0.026, 0.009, 0.545, -0.113, 0.172, 0.808, 0.0],
[0.035, 0.012, 0.559, -0.114, 0.169, 0.813, -0.003],
[0.045, 0.013, 0.571, -0.116, 0.165, 0.815, -0.006],
[0.051, 0.011, 0.576, -0.115, 0.162, 0.813, -0.0],
[0.054, 0.008, 0.573, -0.112, 0.159, 0.806, -0.0],
[0.055, 0.002, 0.562, -0.106, 0.157, 0.794, -0.0],
[0.052, -0.005, 0.543, -0.099, 0.155, 0.777, -0.01],
[0.047, -0.01, 0.52, -0.092, 0.152, 0.756, -0.0],
[0.043, -0.006, 0.499, -0.087, 0.149, 0.732, -0.009],
[0.04, 0.006, 0.48, -0.085, 0.145, 0.703, -0.007],
[0.037, 0.027, 0.462, -0.085, 0.139, 0.67, -0.006],
[0.035, 0.056, 0.446, -0.087, 0.132, 0.633, -0.004],
[0.032, 0.081, 0.427, -0.089, 0.126, 0.596, -0.002],
[0.029, 0.091, 0.398, -0.089, 0.12, 0.561, 0.0],
[0.026, 0.084, 0.361, -0.085, 0.115, 0.529, 0.001],
[0.021, 0.062, 0.315, -0.079, 0.112, 0.5, 0.002],
[0.016, 0.023, 0.261, -0.069, 0.109, 0.473, 0.003],
[0.011, -0.02, 0.208, -0.059, 0.106, 0.445, 0.004],
[0.008, -0.056, 0.169, -0.049, 0.099, 0.414, 0.004],
[0.005, -0.086, 0.144, -0.041, 0.09, 0.378, 0.004],
[0.004, -0.109, 0.132, -0.034, 0.078, 0.337, 0.004],
[0.005, -0.126, 0.134, -0.027, 0.063, 0.293, 0.004],
[0.006, -0.133, 0.141, -0.022, 0.048, 0.249, 0.004],
[0.007, -0.127, 0.145, -0.02, 0.038, 0.213, 0.004],
[0.008, -0.11, 0.145, -0.021, 0.031, 0.182, 0.004],
[0.009, -0.08, 0.142, -0.024, 0.029, 0.159, 0.004],
[0.011, -0.039, 0.136, -0.03, 0.031, 0.141, 0.005],
[0.012, 0.001, 0.126, -0.036, 0.034, 0.128, 0.005],
[0.012, 0.026, 0.112, -0.04, 0.036, 0.116, 0.005],
[0.012, 0.034, 0.094, -0.041, 0.036, 0.105, 0.004],
[0.011, 0.026, 0.073, -0.04, 0.035, 0.095, 0.002],
[0.01, 0.003, 0.048, -0.037, 0.032, 0.086, 0.0],
[0.009, -0.023, 0.025, -0.033, 0.029, 0.077, -0.003],
[0.008, -0.036, 0.009, -0.031, 0.027, 0.069, -0.004],
[0.007, -0.039, 0.001, -0.029, 0.025, 0.061, -0.005],
[0.006, -0.029, -0.001, -0.028, 0.025, 0.053, -0.005],
[0.005, -0.008, 0.005, -0.029, 0.024, 0.045, -0.004],
[0.005, 0.014, 0.013, -0.029, 0.024, 0.037, -0.002],
[0.004, 0.027, 0.017, -0.027, 0.022, 0.03, -0.001],
[0.003, 0.029, 0.019, -0.023, 0.019, 0.023, 0.001],
[0.003, 0.023, 0.018, -0.017, 0.014, 0.016, 0.002],
[0.002, 0.006, 0.013, -0.009, 0.009, 0.01, 0.004],
[0.002, -0.012, 0.008, -0.002, 0.004, 0.005, 0.005],
[0.001, -0.021, 0.004, 0.002, 0.003, 0.001, 0.005],
[0.0, -0.023, 0.001, 0.003, 0.006, -0.003, 0.004],
[-0.002, -0.018, 0.0, 0.001, 0.012, -0.005, 0.002],
[-0.003, -0.004, 0.0, -0.003, 0.021, -0.005, -0.001],
[-0.005, 0.01, 0.0, -0.008, 0.029, -0.005, -0.004],
[-0.005, 0.017, 0.0, -0.011, 0.031, -0.005, -0.005],
[-0.004, 0.018, 0.0, -0.01, 0.026, -0.004, -0.005],
[-0.003, 0.012, 0.0, -0.006, 0.016, -0.002, -0.003],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, -0.0, 0.0, 0.009, -0.0, 0.003, 0.005],
[0.0, -0.0, -0.001, 0.0, -0.0, 0.006, 0.0],
[0.0, -0.0, -0.001, 0.0, -0.0, 0.01, 0.02],
[0.0, -0.0, -0.002, 0.0, -0.0, 0.0, 0.03],
[0.0, -0.0, -0.002, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.007, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.01, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
[0.0, -0.0, -0.0, 0.0, -0.0, 0.0, 0.0],
]
point_sum = len(joint_angles_goal_list)
point_now = 0
joint_angles_now = pose_init
joint_angles_now_list = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
joint_velocities_now = udp.controller.limb.joint_velocities()
joint_velocities_now_list = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
z1 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
z2 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
alpha = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
dy_tau = udp.controller.limb.joint_efforts()
cur_time = rospy.get_time()
pre_time = cur_time
Time = 10
count = 1500
ratio = count / Time
step_size = Time / count
output_size = 100
out_ratio = count / output_size
'''作图用'''
'''关节空间'''
joint_effort_display = np.zeros((7, output_size + 1), dtype=float)
joint_actual_pose_display = np.zeros((7, output_size + 1), dtype=float)
joint_req_pose_display = np.zeros((7, output_size + 1), dtype=float)
tout = np.zeros((7, output_size + 1), dtype=float)
'''输出用计数'''
a = 0
cur_time = rospy.get_time()
pre_time = cur_time
# temp = 0
# for key in joint_angles_goal[0]:
# joint_angles_goal_list[temp] = joint_angles_goal[0][key]
# temp += 1
for i in range(0, count):
if not rospy.is_shutdown():
'''得到角度'''
joint_angles_now = udp.controller.limb.joint_angles()
temp = 0
for key in joint_angles_now:
joint_angles_now_list[temp] = joint_angles_now[key]
temp = temp + 1
'''得到速度'''
joint_velocities_now = udp.controller.limb.joint_velocities()
temp = 0
for key in joint_velocities_now:
joint_velocities_now_list[temp] = joint_velocities_now[key]
temp = temp + 1
'''计算出当前应该的目标点'''
if point_now < point_sum:
if i % 6 == 0:
point_now = point_now + 1
# if point_now < point_sum:
# if abs(joint_angles_goal_list[point_now][0] - joint_angles_now_list[0]) < 0.1 and \
# abs(joint_angles_goal_list[point_now][1] - joint_angles_now_list[1]) < 0.1 and\
# abs(joint_angles_goal_list[point_now][2] - joint_angles_now_list[2]) < 0.1 and\
# abs(joint_angles_goal_list[point_now][6] - joint_angles_now_list[6]) < 0.1:
# point_now = point_now + 1
dy_tau = udp.controller.torquecommand(
joint_angles_goal_list[point_now - 1])
'''计算z1'''
# for aaa in range(0, 7):
# z1[aaa] = joint_angles_now_list[aaa] - joint_angles_goal_list[point_now-1][aaa]
#
# '''计算alpha'''
# for aaa in range(0, 7):
# alpha[aaa] = -udp.controller.torController[aaa].get_kp() * z1[aaa] + joint_vel_goal_list[point_now-1][aaa]
#
# '''计算z2'''
# for aaa in range(0, 7):
# z2[aaa] = joint_velocities_now_list[aaa] - alpha[aaa]
# udp.trans_z2_list[aaa] = z2[aaa]
#
# # print z2
# '''得到通信计算的值'''
# temp = 0
# for key in dy_tau:
# dy_tau[key] = -z1[temp] - udp.controller.torController[temp].get_kd() * z2[temp] + udp.real_thread_result[temp]
# temp = temp + 1
#print dy_tau
# if key == "right_s0" or key == "right_s1" or key == "right_e0" or key == "right_e1":
# if dy_tau[key] > 20:
# dy_tau[key] = 20
# elif dy_tau[key] < -20:
# dy_tau[key] = -20
# else:
# pass
# else:
# if dy_tau[key] > 12:
# dy_tau[key] = 12
# elif dy_tau[key] < -12:
# dy_tau[key] = -12
# else:
# pass
if dy_tau['left_s0'] > 5:
dy_tau['left_s0'] = 5
elif dy_tau['left_s0'] < -5:
dy_tau['left_s0'] = -4
else:
pass
if a == 0:
temp = 0
start_time = rospy.get_time()
get_pose = udp.controller.limb.joint_angles()
for key in get_pose:
joint_actual_pose_display[temp, a] = get_pose[key]
joint_effort_display[temp, a] = dy_tau[key]
#joint_effort_display[temp,a] = tau[(0,temp)]
joint_req_pose_display[temp, a] = joint_angles_goal_list[
point_now - 1][temp]
tout[temp, a] = 0
temp = temp + 1
a = a + 1
print point_now
# print dy_tau
udp.controller.limb.set_joint_torques(dy_tau)
#udp.controller.limb.move_to_joint_positions(joint_angles_goal)
'''作图用'''
if i % out_ratio == 0:
display_cur_time = rospy.get_time()
'''关节角度 '''
temp = 0
get_pose = udp.controller.limb.joint_angles()
for key in get_pose:
joint_actual_pose_display[temp, a] = get_pose[key]
joint_effort_display[temp, a] = dy_tau[key]
# joint_effort_display[temp,a] = tau[(0,temp)]
joint_req_pose_display[temp, a] = joint_angles_goal_list[
point_now - 1][temp]
tout[temp, a] = float(display_cur_time - start_time)
temp = temp + 1
a = a + 1
Rate.sleep()
rospy.on_shutdown(udp.controller.shutdown_close)
udp.thread_stop = True
udp.controller.limb.exit_control_mode()
udp.controller.limb.move_to_neutral()
#tout = np.linspace(0, 10, output_size+1)
'''关节空间'''
fig1 = plt.figure(1)
plt.subplot(1, 1, 1)
plt.title("joint_w0")
plt.plot(tout[0].T,
joint_actual_pose_display[0],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[0].T,
joint_req_pose_display[0],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[0] - joint_actual_pose_display[0],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
fig2 = plt.figure(2)
plt.subplot(1, 1, 1)
plt.title("joint_w1")
plt.plot(tout[1].T,
joint_actual_pose_display[1],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[1].T,
joint_req_pose_display[1],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[1] - joint_actual_pose_display[1],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
fig3 = plt.figure(3)
plt.subplot(1, 1, 1)
plt.title("joint_w2")
plt.plot(tout[2].T,
joint_actual_pose_display[2],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[2].T,
joint_req_pose_display[2],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[2] - joint_actual_pose_display[2],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
fig4 = plt.figure(4)
plt.subplot(1, 1, 1)
plt.title("joint_e0")
plt.plot(tout[3].T,
joint_actual_pose_display[3],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[3].T,
joint_req_pose_display[3],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[3] - joint_actual_pose_display[3],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
fig5 = plt.figure(5)
plt.subplot(1, 1, 1)
plt.title("joint_e1")
plt.plot(tout[4].T,
joint_actual_pose_display[4],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[4].T,
joint_req_pose_display[4],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[4] - joint_actual_pose_display[4],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
fig6 = plt.figure(6)
plt.subplot(1, 1, 1)
plt.title("joint_s0")
plt.plot(tout[5].T,
joint_actual_pose_display[5],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[5].T,
joint_req_pose_display[5],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[5] - joint_actual_pose_display[5],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
fig7 = plt.figure(7)
plt.subplot(1, 1, 1)
plt.title("joint_s1")
plt.plot(tout[6].T,
joint_actual_pose_display[6],
linewidth=3,
color="red",
label="actual value")
plt.plot(tout[6].T,
joint_req_pose_display[6],
linewidth=3,
color="green",
label="desired value")
plt.plot(tout[0].T,
joint_req_pose_display[6] - joint_actual_pose_display[6],
linewidth=3,
color="blue",
label="error value")
plt.xlabel("time/s")
plt.ylabel("angle/rad")
plt.legend(loc='best')
plt.show()
fig8 = plt.figure(8)
plt.subplot(1, 1, 1)
plt.title("torques")
plt.plot(tout[0].T, joint_effort_display[0], linewidth=3, label='joint_w0')
plt.plot(tout[0].T, joint_effort_display[1], linewidth=3, label='joint_w1')
plt.plot(tout[0].T, joint_effort_display[2], linewidth=3, label='joint_w2')
plt.plot(tout[0].T, joint_effort_display[3], linewidth=3, label='joint_e0')
plt.plot(tout[0].T, joint_effort_display[4], linewidth=3, label='joint_e1')
plt.plot(tout[0].T, joint_effort_display[5], linewidth=3, label='joint_s0')
plt.plot(tout[0].T, joint_effort_display[6], linewidth=3, label='joint_s1')
plt.xlabel("time/s")
plt.ylabel("torque/Nm")
plt.legend(loc='best', bbox_to_anchor=(1, 0.7))
fig8.savefig('123456.png', transparent=True)
plt.show()
# Callback for dynamic reconfigure requests
def reconfig_callback(config, level):
global imu_yaw_calibration
rospy.loginfo("""Reconfigure request for yaw_calibration: %d""" %(config['yaw_calibration']))
#if imu_yaw_calibration != config('yaw_calibration'):
imu_yaw_calibration = config['yaw_calibration']
rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
return config
rospy.init_node("razor_node")
#We only care about the most recent measurement, i.e. queue_size=1
pub = rospy.Publisher('imu/razor', Imu, queue_size=1)
srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time();
imuMsg = Imu()
# Orientation covariance estimation:
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
# Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
# Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
# cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
# i.e. variance in yaw: 0.0025
# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
# static roll/pitch error of 0.8%, owing to gravity orientation sensing
# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
# so set all covariances the same.
imuMsg.orientation_covariance = [
0.0025 , 0 , 0,
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
rospy.init_node('talker')
pub = rospy.Publisher('chatter', String, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
hello_str = String()
hello_str.data = "hello world %s" % rospy.get_time()
pub.publish(hello_str)
rate.sleep()
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while (rospy.get_time() == 0) and (not rospy.is_shutdown()):
print 'Waiting for clock server to start'
print 'Received first clock message'
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
#self.ros_rate.sleep()
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
rospy.sleep(1.0 / float(self.rate))
self.euler_sp_filt.x = simple_filters.filterPT1(
self.euler_sp_old.x, self.euler_sp.x,
self.roll_reference_prefilter_T, self.Ts,
self.roll_reference_prefilter_K)
self.euler_sp_filt.y = simple_filters.filterPT1(
self.euler_sp_old.y, self.euler_sp.y,
self.pitch_reference_prefilter_T, self.Ts,
self.pitch_reference_prefilter_K)
self.euler_sp_filt.z = self.euler_sp.z
#self.euler_sp.z = simple_filters.filterPT1(self.euler_sp_old.z, self.euler_sp.z, 0.2, self.Ts, 1.0)
self.euler_sp_old = copy.deepcopy(self.euler_sp_filt)
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > (1.0 / self.rate + 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < (1.0 / self.rate - 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < 0.005:
dt_clk = 0.01
# Roll
roll_rate_sv = self.pid_roll.compute(self.euler_sp_filt.x,
self.euler_mv.x, dt_clk)
# roll rate pid compute
roll_rate_output = self.pid_roll_rate.compute(
roll_rate_sv, self.euler_rate_mv.x,
dt_clk) + self.roll_rate_output_trim
# Pitch
pitch_rate_sv = self.pid_pitch.compute(self.euler_sp_filt.y,
self.euler_mv.y, dt_clk)
# pitch rate pid compute
pitch_rate_output = self.pid_pitch_rate.compute(
pitch_rate_sv, self.euler_rate_mv.y,
dt_clk) + self.pitch_rate_output_trim
# Yaw
yaw_rate_sv = self.pid_yaw.compute(self.euler_sp_filt.z,
self.euler_mv.z, dt_clk)
# yaw rate pid compute
yaw_rate_output = self.pid_yaw_rate.compute(
yaw_rate_sv, self.euler_rate_mv.z, dt_clk)
# VPC stuff
vpc_roll_output = -self.pid_vpc_roll.compute(
0.0, roll_rate_output, dt_clk)
# Due to some wiring errors we set output to +, should be -
vpc_pitch_output = -self.pid_vpc_pitch.compute(
0.0, pitch_rate_output, dt_clk)
# Publish attitude
attitude_output = Float64MultiArray()
attitude_output.data = [roll_rate_output, pitch_rate_output, \
yaw_rate_output, vpc_roll_output, vpc_pitch_output]
self.attitude_pub.publish(attitude_output)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
# Publish VPC pid data
self.pub_pid_vpc_roll.publish(self.pid_vpc_roll.create_msg())
self.pub_pid_vpc_pitch.publish(self.pid_vpc_pitch.create_msg())
def image_callback(self, msg):
self.image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
self.ROI = self.image[226:256, 0:876]
left_frame = self.image[160:305, 109:329]
right_frame = self.image[160:305, 547:767]
''' ********** Optical Flow 初始帧 *********** '''
if self.initialize_flag == False:
# 左画面
self.prev_gray_left = cv2.cvtColor(left_frame, cv2.COLOR_BGR2GRAY)
self.prev_left = cv2.goodFeaturesToTrack(self.prev_gray_left, mask = None, **self.feature_params)
# 右画面
self.prev_gray_right = cv2.cvtColor(right_frame, cv2.COLOR_BGR2GRAY)
self.prev_right = cv2.goodFeaturesToTrack(self.prev_gray_right, mask = None, **self.feature_params)
# 长条画面
self.prev_gray_ROI = cv2.cvtColor(self.ROI, cv2.COLOR_BGR2GRAY)
self.prev_point = cv2.goodFeaturesToTrack(self.prev_gray_ROI, mask = None, **self.feature_params)
self.initialize_flag = True
''' *********** Optical Flow 追踪帧 ************ '''
else:
# 左画面
self.prev_gray_left, self.prev_left, self.LY, self.LX = ml.direction_detect(left_frame, \
self.prev_gray_left, self.prev_left, self.feature_params, self.lk_params, self.color)
# 右画面
self.prev_gray_right, self.prev_right, self.RY, self.RX = ml.direction_detect(right_frame, \
self.prev_gray_right, self.prev_right, self.feature_params, self.lk_params, self.color)
# 长条画面
self.prev_gray_ROI, self.prev_point, temp = ml.GetLength(self.ROI, self.prev_gray_ROI, \
self.prev_point, self.feature_params, self.lk_params, self.color)
# 计算光流夹角
if self.LY != 0 and self.LX != 0 and self.RY != 0 and self.RX != 0:
Lx = np.dot(self.H, self.Left_kf_x.predict())[0]
self.Left_kf_x.update(self.LX)
Ly = np.dot(self.H, self.Left_kf_y.predict())[0]
self.Left_kf_y.update(self.LY)
Rx = np.dot(self.H, self.Right_kf_x.predict())[0]
self.Right_kf_x.update(self.RX)
Ry = np.dot(self.H, self.Right_kf_y.predict())[0]
self.Right_kf_y.update(self.RY)
self.Left_Angle = ml.angular([Ly, Lx])
self.Right_Angle = ml.angular([Ry, Rx])
# 获得前进方向 self.RotateFlag (1:右, 2:左, 3:前, 4:后)
self.RotateFlag = ml.GetDirectionOfTravel(self.Left_Angle, self.Right_Angle)
'''******************** 跳过转弯检测的初始5帧 ******************** '''
if self.dir_flag <= 5:
self.now_d, self.pre_d, self.pre_d_sub = self.RotateFlag, self.RotateFlag, self.RotateFlag
self.dir_flag += 1
else:
# 当前判断和上一阶段不同
if self.pre_d != self.RotateFlag:
# 当前判断和上一帧不同
if self.pre_d_sub != self.RotateFlag:
self.count = 0
# 当前判断和上一帧相同
else:
self.count += 1
self.pre_d_sub = self.RotateFlag
# 当前判断和上一阶段相同
else:
self.count = 0
if self.now_d == 1 or self.now_d == 2:
self.TempOfImg[self.OfImgNum] = self.image
self.OfImgNum += 1
'''******************** 再次滤波 连续出现5次相同的变化 ********************'''
if self.count >= 5:
# 计算和前一节点的时间间隔
if self.now_d != 3:
self.UnionTimeStart = rospy.get_time()
self.UnionTimeInterval = self.UnionTimeStart - self.UnionTimeEnd
self.UnionTimeEnd = rospy.get_time()
self.now_d = self.RotateFlag
if self.now_d == 3:
self.cornor_direction = self.pre_d
self.SaveFlagOf = 1
self.count = 0
# 把这一阶段的结果赋值
self.pre_d = self.now_d
# 计算角度 + 整合方向 返回的 self.angle 即相机旋转的角度
self.OF_sum, self.DirectionNow, self.Direction, self.__angle_flag, self.angle = ml.GetRotateAng(self.now_d, \
self.OF_sum, temp, self.__angle_flag, self.angle)
# 开启时，保存图像等信息
if self.SaveFlagOf == 1:
self.FileNum, self.TempOfImg, self.OfCount, self.SaveFlagOf, self.OFImgSet, self.Obj, self.MovePartten, self.UnionTimeInterval \
= ml.SaveDirectionImg(self.FileNum, \
self.angle, self.cornor_direction, self.UnionTimeInterval, self.TempOfImg, self.OfCount, self.SaveFlagOf)
self.OfImgNum = 0
'''
self.OFImgSet: 图像集合
self.Obj: 此处物体类型
self.MovePartten: 此处运动方式
self.UnionTimeTnterval: 距离上一个节点的时间间隔
'''
# 开始发布消息
self.ImgNum_pub.publish(len(self.OFImgSet))
for i in range(1, len(self.OFImgSet)+1):
self.image_pub.publish(self.bridge.cv2_to_imgmsg(self.OFImgSet[i], "bgr8"))
self.OBJ_Info_pub.publish(self.Obj)
self.MovePartten_info_pub.publish(self.MovePartten)
self.Interval_pub.publish(int(self.UnionTimeInterval))
def commucate(self):
while not self.thread_stop:
self._cur_time = rospy.get_time()
dt = self._cur_time - self._pre_time
'''需要传递的角度'''
self.angles = self.limb.joint_angles()
temp = 0
for key in self.angles:
self.trans_angles_list[temp] = self.angles[key]
temp = temp + 1
self.real_trans_angles_list[0] = self.trans_angles_list[0]
self.real_trans_angles_list[1] = self.trans_angles_list[1]
self.real_trans_angles_list[2] = self.trans_angles_list[5]
self.real_trans_angles_list[3] = self.trans_angles_list[6]
self.real_trans_angles_list[4] = self.trans_angles_list[2]
self.real_trans_angles_list[5] = self.trans_angles_list[3]
self.real_trans_angles_list[6] = self.trans_angles_list[4]
# print self.real_trans_angles_list
for i in range(0, 7):
self.real_trans_angles_list[
i] = self.real_trans_angles_list[i] * 1000.0 + 32768.0
'''需要传递的速度'''
self.velocities = self.limb.joint_velocities()
temp = 0
for key in self.velocities:
self.trans_velocities_list[temp] = self.velocities[key]
temp = temp + 1
self.real_trans_velocities_list[0] = self.trans_velocities_list[0]
self.real_trans_velocities_list[1] = self.trans_velocities_list[1]
self.real_trans_velocities_list[2] = self.trans_velocities_list[5]
self.real_trans_velocities_list[3] = self.trans_velocities_list[6]
self.real_trans_velocities_list[4] = self.trans_velocities_list[2]
self.real_trans_velocities_list[5] = self.trans_velocities_list[3]
self.real_trans_velocities_list[6] = self.trans_velocities_list[4]
# print self.real_trans_velocities_list
for i in range(0, 7):
self.real_trans_velocities_list[
i] = self.real_trans_velocities_list[i] * 1000.0 + 32768.0
'''需要传递的z2'''
self.real_z2_alphas_list[0] = self.trans_z2_list[0]
self.real_z2_alphas_list[1] = self.trans_z2_list[1]
self.real_z2_alphas_list[2] = self.trans_z2_list[5]
self.real_z2_alphas_list[3] = self.trans_z2_list[6]
self.real_z2_alphas_list[4] = self.trans_z2_list[2]
self.real_z2_alphas_list[5] = self.trans_z2_list[3]
self.real_z2_alphas_list[6] = self.trans_z2_list[4]
for i in range(0, 7):
self.real_z2_alphas_list[
i] = self.real_z2_alphas_list[i] * 1000.0 + 32768.0
self.msg = struct.pack("H", self.start)
self.msg += struct.pack(
"7H", self.real_trans_angles_list[0],
self.real_trans_angles_list[1], self.real_trans_angles_list[2],
self.real_trans_angles_list[3], self.real_trans_angles_list[4],
self.real_trans_angles_list[5], self.real_trans_angles_list[6])
self.msg += struct.pack("7H", self.real_trans_velocities_list[0],
self.real_trans_velocities_list[1],
self.real_trans_velocities_list[2],
self.real_trans_velocities_list[3],
self.real_trans_velocities_list[4],
self.real_trans_velocities_list[5],
self.real_trans_velocities_list[6])
self.msg += struct.pack(
"7H", self.real_z2_alphas_list[0], self.real_z2_alphas_list[1],
self.real_z2_alphas_list[2], self.real_z2_alphas_list[3],
self.real_z2_alphas_list[4], self.real_z2_alphas_list[5],
self.real_z2_alphas_list[6])
self.s.sendto(self.msg, ('10.1.1.21', 8001))
data, addr = self.s.recvfrom(1024)
for i in range(0, 7):
self.thread_result[i] = (
(ord(data[2 * i]) * 256 + ord(data[2 * i + 1])) -
32768.0) / 1000.0
self.real_thread_result[0] = self.thread_result[0]
self.real_thread_result[1] = self.thread_result[1]
self.real_thread_result[2] = self.thread_result[4]
self.real_thread_result[3] = self.thread_result[5]
self.real_thread_result[4] = self.thread_result[6]
self.real_thread_result[5] = self.thread_result[2]
self.real_thread_result[6] = self.thread_result[3]
# print "self.real_thread_result"
#print self.real_thread_result
for i in range(0, 7):
self.pre_alphas[i] = self.cur_alphas[i]
self._pre_time = self._cur_time
self.Rate.sleep()
def __init__(self):
# 共用时间
self.UnionTimeEnd = rospy.get_time()
self.UnionTimeStart = 0
self.UnionTimeInterval = 0
# Landmark 初始变量
self.bridge = cv_bridge.CvBridge()
self.BBinfo = rospy.Subscriber("darknet_ros/bounding_boxes", BoundingBoxes, self.callback_boundingbox)
self.img = rospy.Subscriber('/image_changed', Image, self.image_callback)
self.image_pub = rospy.Publisher('/image_node', Image)
self.OBJ_Info_pub = rospy.Publisher('/node_info_obj', String)
self.MovePartten_info_pub = rospy.Publisher('/node_info_mp', String)
self.Interval_pub = rospy.Publisher('/node_info_inter', Int32)
self.ImgNum_pub = rospy.Publisher('/img_num', Int32)
self.SetYoloTime = 1
self.YoloTimeEnd = 0
self.YoloTimeInterval = 0
self.StartTime = rospy.get_time()
self.Start_flag = 0
self.TrashDict = {}
self.IndicDict = {}
self.TempImgDict = {}
self.ImgCount = 0
self.ImgNum = 0
self.FileNum = 0
self.OBJNameListFlag = 1
self.TempObjNum = []
self.DictNum = []
# Optical Flow 初始变量
self.feature_params = dict(maxCorners = 300, qualityLevel = 0.2, minDistance = 5, blockSize = 7)
self.lk_params = dict(winSize = (15,15), maxLevel = 2, criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
self.initialize_flag = False
self.color = (0, 255, 0)
self.LX, self.LY, self.RX, self.RY = 0, 0, 0, 0
self.dt = 1.0/600
self.F = np.array([[1, self.dt], [0, 1]])
self.H = np.array([1, 0]).reshape(1, 2)
self.Q = np.array([[0.05, 0.0], [0.0, 0.0]])
self.R = np.array([0.5]).reshape(1, 1)
self.Left_kf_x = ml.KalmanFilter(F = self.F, H = self.H, Q = self.Q, R = self.R)
self.Left_kf_y = ml.KalmanFilter(F = self.F, H = self.H, Q = self.Q, R = self.R)
self.Right_kf_x = ml.KalmanFilter(F = self.F, H = self.H, Q = self.Q, R = self.R)
self.Right_kf_y = ml.KalmanFilter(F = self.F, H = self.H, Q = self.Q, R = self.R)
self.__angle_flag = 0
self.dir_flag = 1
self.count = 0
self.pre_text = 'forward'
self.Direction = 'forward'
self.DirectionNow = 'forward'
self.cornor_direction = 'forward'
self.TempOfImg = {}
self.OfImgNum = 0
self.SaveFlagOf = 0
self.OfCount = 0
self.angle = 0
self.OF_sum = 0
self.OFImgSet = {}
self.DictImgSURF = {}
# 追加的sb实验
self.IndicatorLeft = {}
self.IndicatorRight = {}
self.TrashLeft = {}
self.TrashRight = {}
def callback_boundingbox(self, message):
if self.Start_flag == 0:
self.StartInterval = rospy.get_time() - self.StartTime
if self.StartInterval > 5:
self.Start_flag = 1
else:
num = len(message.bounding_boxes)
OBJNameList = []
self.YoloTimeStart = rospy.get_time()
self.YoloTimeInterval = self.YoloTimeStart - self.YoloTimeEnd
# 满足时间条件时，判断是否为真节点，根据结果保存图像
if self.YoloTimeInterval >= self.SetYoloTime:
self.TrashLeft, self.TrashRight, self.IndicatorLeft, self.IndicatorRight, self.TempImgDict, self.ImgNum, self.TempObjNum, self.ImgCount, self.FileNum, \
LMset, Obj, nowd, SendFlag = ml.CheckSaveOrPass(self.ImgCount, self.TrashLeft, self.TrashRight, self.IndicatorLeft, self.IndicatorRight, \
self.ImgNum, self.TempObjNum, self.TempImgDict, self.FileNum, self.now_d)
if SendFlag == 1:
# 计算和前一节点的时间间隔
self.UnionTimeStart = rospy.get_time()
self.UnionTimeInterval = self.UnionTimeStart - self.UnionTimeEnd
self.UnionTimeEnd = rospy.get_time()
# 初始节点时间距离归0
if self.FileNum == 1:
self.UnionTimeInterval = 0
'''
LMset: 图像集合
Obj: 此处物体类型
nowd: 此处运动方式
self.UnionTimeTnterval: 距离上一个节点的时间间隔
'''
# 发布消息
self.ImgNum_pub.publish(len(LMset))
for i in range(1, len(LMset)+1):
self.image_pub.publish(self.bridge.cv2_to_imgmsg(LMset[i], "bgr8"))
self.OBJ_Info_pub.publish(Obj)
self.MovePartten_info_pub.publish(ml.DirTransform(nowd))
self.Interval_pub.publish(int(self.UnionTimeInterval))
# 还是同一节点
for i in range(num):
# 获取物体名
OBJName = message.bounding_boxes[i].Class
# 获取物体坐标
x = (message.bounding_boxes[i].xmax + message.bounding_boxes[i].xmin)
y = (message.bounding_boxes[i].ymax + message.bounding_boxes[i].ymin)
coordinate = [x/2, y/2]
# 位置筛选
if coordinate[1] > 110 and coordinate[1] < 360:
if coordinate[0] > 100 and coordinate[0] < 328:
OBJNameList.append(OBJName + '+Left')
self.DictNum = num
self.TempObjNum.append(self.DictNum)
elif coordinate[0] > 548 and coordinate[0] < 776:
OBJNameList.append(OBJName + '+Right')
self.DictNum = num
self.TempObjNum.append(self.DictNum)
# 如果次帧为空列表，跳出循环
if OBJNameList == []:
self.OBJNameListFlag = 0
break
# 读取这一次列表中物体的个数
LTrashNum, RTrashNum, LIndicatorNum, RIndicatorNum = ml.NumCount(OBJNameList)
self.TrashLeft, self.TrashRight, self.IndicatorLeft, self.IndicatorRight = ml.UpdateDict\
([LTrashNum, RTrashNum, LIndicatorNum, RIndicatorNum], self.TrashLeft, self.TrashRight, self.IndicatorLeft, self.IndicatorRight)
self.OBJNameListFlag = 1
if self.OBJNameListFlag == 1:
self.TempImgDict[self.ImgNum] = (self.DictNum, self.image)
self.YoloTimeEnd = rospy.get_time()
self.ImgNum += 1
self.ImgCount += 1