def errorCheck(pub): r = rospy.Rate(10) global count precount = 0 thread_unloop_count = 0 while True: error_flag = Bool() if (count - precount) > 0: thread_unloop_count = 0 precount = count elif (count - precount) == 0: thread_unloop_count += 1 precount = count else: thread_unloop_count = 0 precount = 0 if thread_unloop_count > 2: error_flag.data = True print'********++++++++++------------------AMU is rebooting now!!------------------++++++++******' else: error_flag.data = False #commented out by hagiwara 0224 #print'********++++++++++------------------AMU is OK!!------------------++++++++******' pub.publish(error_flag) r.sleep()
def publishOrderBegun():
pubBegunMsg = Bool()
if(orderProcess == True):
pubBegunMsg.data = 1
else:
pubBegunMsg.data = 0
global pubOrderBegun
pubOrderBegun.publish(pubBegunMsg)
def run_dryer(self, run_it=True):
cmd = Bool()
if run_it == True:
cmd.data = True
else:
cmd.data = False
"Updating dryer"
self.dryer_pub.publish(cmd)
def pauseSystem(self, pausing):
learningStatus = Bool()
controlStatus = Bool()
if not pausing:
self.expStatus = "monitor"
self.backToInit = not pausing
learningStatus.data = not pausing
self.learningMF_pub.publish(learningStatus)
self.learningMB_pub.publish(learningStatus)
self.learningMB2_pub.publish(learningStatus)
controlStatus.data = not pausing
self.control_pub.publish(controlStatus)
def ft_sleeve_cb(self, msg):
if not self.ft_sleeve_biased:
rospy.sleep(0.5)
self.time_since_last_cb = rospy.Time.now()
self.ft_sleeve_bias_x = msg.wrench.force.x
self.ft_sleeve_bias_y = msg.wrench.force.y
self.ft_sleeve_bias_z = msg.wrench.force.z
self.ft_sleeve_bias_t_x = msg.wrench.torque.x
self.ft_sleeve_bias_t_y = msg.wrench.torque.y
self.ft_sleeve_bias_t_z = msg.wrench.torque.z
self.ft_sleeve_biased = True
if rospy.Time.now().to_sec() - self.time_since_last_cb.to_sec() > 0.05:
print 'The force-torque sensor callback is too slow. That is potentially very bad. Aborting everything!!!'
self.z.estop()
self.z = None
self.time_since_last_cb = rospy.Time.now()
x_force = msg.wrench.force.x-self.ft_sleeve_bias_x
y_force = msg.wrench.force.y-self.ft_sleeve_bias_y
z_force = msg.wrench.force.z-self.ft_sleeve_bias_z
x_torque = msg.wrench.torque.x-self.ft_sleeve_bias_t_x
y_torque = msg.wrench.torque.y-self.ft_sleeve_bias_t_y
z_torque = msg.wrench.torque.z-self.ft_sleeve_bias_t_z
# pos = self.z.get_position()
if self.recording:
t = rospy.Time.now() - self.start_record_time
self.sleeve_file.write(''.join([str(t.to_sec()), ' %f %f %f %f %f %f %f \n' %
(self.zenither_pose,
x_force,
y_force,
z_force,
x_torque,
y_torque,
z_torque)]))
self.array_to_save[self.array_line] = [t.to_sec(), self.zenither_pose, x_force, y_force, z_force]
self.array_line += 1
if self.pulling:
threshold = self.pulling_force_threshold
else:
threshold = self.reset_force_threshold
# mag_force = np.linalg.norm(np.abs([x_force, y_force, z_force]))
if (np.linalg.norm(np.abs([x_force, y_force, z_force])) > threshold):
out = Bool()
out.data = True
self.force_threshold_exceeded_pub.publish(out)
else:
out = Bool()
out.data = False
self.output_stuck = 0
self.force_threshold_exceeded_pub.publish(out)
def test1(self):
msg = Bool()
msg.data = True
# wait until advertise and receive
time.sleep(0.5)
self._pub.publish(msg)
time.sleep(0.1)
self.assertTrue(self._true)
self.assertFalse(self._false)
msg.data = False
self._pub.publish(msg)
time.sleep(0.1)
self.assertTrue(self._false)
self.assertFalse(self._true)
def prepare_std_msg(self):
rospy.loginfo("Preparing std_msgs......")
'''
Fill Bool message
'''
bool_msg = Bool()
bool_msg.data = False
'''
Fill Float32 message
'''
float32_msg = Float32()
float32_msg.data = 1.0
'''
Fill Int32 message
Please, do it your self for practice
'''
'''
Fill String message
Please, do it your self for practice
'''
rospy.loginfo("std_msgs preparation done......")
def newSample(self, msg): newFFT = self.stft.ingestSample(msg.data) if newFFT is not None: self.FFT = newFFT # Mask and average data guard1 = np.mean(newFFT[self.G1_mask, :][:, self.channel_mask]) alpha = np.mean(newFFT[self.Al_mask, :][:, self.channel_mask]) guard2 = np.mean(newFFT[self.G2_mask, :][:, self.channel_mask]) detected = self.movavg.step(alpha > (guard1 + guard2)*1.1) > 0.5 if detected and not self.ignore.test(): self.movavg.reset() self.ignore.reset(4) else: detected = False # Publish messages msg = Float32() msg.data = guard1 self.pub_guard1.publish(msg) msg = Float32() msg.data = alpha self.pub_alpha.publish(msg) msg = Float32() msg.data = guard2 self.pub_guard2.publish(msg) msg = Bool() msg.data = detected self.pub_eyes.publish(msg)
def model_stateCB(self, msg):
if not self.active:
return
quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w]
euler = tf.transformations.euler_from_quaternion(quaternion_msg)
self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2])
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
if angle_to_destination < 3 * (math.pi/180):
print "correct straight line"
if distance_to_destination<= self.threshold_proximity:
print "ready to grab"
self.gazebo_cmd(0,0)
self.active = False
self.gripper_cmd(0,0.5)
msg = Bool()
msg.data = True
self.pub_grab_object.publish(msg)
else:
self.gazebo_cmd(self.speed,0)
else:
print "correct orientation"
w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5
self.gazebo_cmd(0,w)
def do_security():
global STATUS_LOCK, CAR_UPDATE_FLAG, CAMERA_UPDATE_FLAG
global CAMERA_CURRENT_TIME, CAR_CURRENT_TIME
global GLOBAL_MOVE_PUB, VEL_PUB
global NAV_FLAG_PUB, ENABLE_MOVE_FLAG, BAR_FLAG
time_now = rospy.Time.now()
STATUS_LOCK.acquire()
if CAR_UPDATE_FLAG:
time1_diff = time_now - CAR_CURRENT_TIME
if CAMERA_UPDATE_FLAG:
time2_diff = time_now - CAMERA_CURRENT_TIME
# 视觉丢失超时保险
# 里程计丢失超时保险
if (CAMERA_UPDATE_FLAG and time2_diff.to_sec() > 180 and not BAR_FLAG) or time1_diff.to_sec() > 5.:
# 发布全局禁止flag
global_move_flag = Bool()
global_move_flag.data = False
GLOBAL_MOVE_PUB.publish(global_move_flag)
car_twist = Twist()
VEL_PUB.publish(car_twist)
CAMERA_UPDATE_FLAG = False
CAR_UPDATE_FLAG = False
CAMERA_CURRENT_TIME = rospy.Time.now()
CAR_CURRENT_TIME = rospy.Time.now()
STATUS_LOCK.release()
def talker():
rospy.init_node("frobit_test_set_speed")
deadman_pub = rospy.Publisher("/fmSignals/deadman", Bool)
deadman = Bool()
cmd_vel_left_pub = rospy.Publisher("/fmSignals/cmd_vel_left", TwistStamped)
twist_left = TwistStamped()
cmd_vel_right_pub = rospy.Publisher("/fmSignals/cmd_vel_right", TwistStamped)
twist_right = TwistStamped()
r = rospy.Rate(node_upd_freq) # set talker update frequency
while not rospy.is_shutdown():
deadman.data = 1 # publish deadman topic
deadman_pub.publish(deadman)
twist_left.header.stamp = rospy.Time.now()
twist_left.twist.linear.x = spd_left
# publish cmd_vel topic for left wheel
cmd_vel_left_pub.publish(twist_left)
twist_right.header.stamp = rospy.Time.now()
twist_right.twist.linear.x = spd_right
# publish cmd_vel topic for right wheel
cmd_vel_right_pub.publish(twist_right)
r.sleep()
def pub_e( self , enable ):
""" pub enable status """
msg = Bool()
msg.data = enable
self.pub_enable.publish( msg )
def run(self):
while not rospy.is_shutdown():
if time.time() - self.current_time > self.time:
shutoff = Bool()
shutoff.data = True
self.pub_shutoff.publish(shutoff)
time.sleep(1)
def navToPose(goal):
"""Drive to a goal subscribed to from /move_base_simple/goal"""
#compute angle required to make straight-line move to desired pose
global xPosition
global yPosition
global theta
#capture desired x and y positions
desiredY = goal.pose.position.y
desiredX = goal.pose.position.x
#capture desired angle
quat = goal.pose.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, yaw = euler_from_quaternion(q)
desiredT = yaw * (180.0/math.pi)
#compute distance to target
goalDistance = math.sqrt(math.pow((desiredX - xPosition),2) + math.pow((desiredY-yPosition),2))#Distance formula
#compute initial turn amount
initialTurn = math.atan((desiredY-yPosition)/(desiredX-xPosition))
print "spin!" #turn to calculated angle
rotate(initialTurn)
print "move!" #move in straight line specified distance to new pose
driveStraight(0.25, goalDistance)
rospy.sleep(2)
print "spin!" #spin to final angle
rotate(desiredT)
print "done"
arrival = Bool()
arrival.data = True
arrival_pub.publish(arrival)
def model_stateCB(self, msg):
if not self.active:
return
quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w]
euler = tf.transformations.euler_from_quaternion(quaternion_msg)
self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2])
# print self.robot_pose
# print 'robot_pose:', self.robot_pose#, 'waypoints:', self.waypoints
self.destination_pose = self.circleIntersect(self.lookahead_distance)
if self.destination_pose == None:
self.active = False
print "approach destination "
self.gazebo_cmd(0,0)
msg = Bool()
msg.data = True
self.pub_finish.publish(msg)
else:
# print 'destination_pose:', self.destination_pose
# print 'waypoint_index:', self.current_waypoint_index
self.speed = self.default_speed
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
# self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination)
# print "robot_head",euler[2]*180/math.pi,"angle_to_destination", angle_to_destination*180/math.pi
w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5
self.gazebo_cmd(self.speed,w)
def status_updated_callback(self, msg): # GoalStatusArray
intermediate_statuses = [GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.RECALLING, GoalStatus.PREEMPTING]
for goal in msg.status_list:
# status = GoalManager.status_list[goal.status]
# rospy.logerr("Goal %s. %s" % (status, goal.text))
goal_idx = self.get_index_of(goal.goal_id.id)
if goal.status == GoalStatus.SUCCEEDED: # do a little dance to celebrate
rospy.loginfo("Goal %s reached!" % goal.goal_id.id)
last_goal_reached = Bool()
last_goal_reached.data = not self.loop_goals and goal_idx + 1 == len(self.goals)
self.done_pub.publish(last_goal_reached)
self.wait_for_goal()
if goal_idx + 1 == len(self.goals):
if self.loop_goals:
rospy.loginfo("Now looping back to first goal")
self.reset_goal_list()
self.next_goal()
else:
self.next_goal(goal_idx + 1)
elif goal.status in intermediate_statuses: # transitional state, so just wait
pass
elif goal.status == GoalStatus.PREEMPTED:
rospy.logwarn("Goal %s was preempted. %s" % (goal.goal_id.id, goal.text))
else: # the robot's dead
status = GoalManager.status_list[goal.status]
rospy.logerr("Goal %s %s. %s" % (goal.goal_id.id, status, goal.text))
def receive_1_cb(self, msg):
#print 'message received'
#print msg
im = self.br.imgmsg_to_cv2(msg, desired_encoding='passthrough')
for i in range(0, im.shape[0]):
for j in range(0, im.shape[1]):
#if not (im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0):
if im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0:
#print 'Detect an intruder!'
msg_sended = Bool()
msg_sended.data = True
send = FunctionUnit.create_send(self, self._send_topic, Bool)
send.send(msg_sended)
virtual_msg = Bool()
virtual_msg.data = False
self._virtual_send.send(virtual_msg)
def callback_manipulator(self, controller):
if controller.killswitch != self.controller.killswitch:
shut_down = Bool()
shut_down.data = self.controller.killswitch == 1
self.callback_auto_shut_down(shut_down)
self.controller = controller
self.send_manipulator = True
def cbJoy(self, joy_msg): model_state_msg = Twist() model_state_msg.linear.x = joy_msg.axes[1] * self.v_gain model_state_msg.linear.y = 0 model_state_msg.linear.z = 0 model_state_msg.angular.x = 0 model_state_msg.angular.y = 0 model_state_msg.angular.z = -joy_msg.axes[3] * self.w_gain self.pub_gazebo.publish(model_state_msg) model_state_msg.linear.x = 0 model_state_msg.angular.z = 0 if joy_msg.buttons[0] == 1: model_state_msg.angular.z = -1 elif joy_msg.buttons[1] == 1: model_state_msg.angular.z = 1 self.pub_gazebo_gripper.publish(model_state_msg) if (joy_msg.buttons[7] == 1): reset_msg = Bool() reset_msg.data = True self.pub_encoder_reset.publish(reset_msg)
def grab_close_ball(self):
self.switch_to_cam("arm")
mode = Bool()
mode.data = True
self.arm_mode_pub.publish(mode)
req = False
self.grab_request(req)
def talker(options):
pub = rospy.Publisher("pr2_etherCAT/motors_halted", Bool)
rospy.init_node(NAME, anonymous=True)
while not rospy.is_shutdown():
out = Bool()
out.data = options.halt
pub.publish(out)
rospy.sleep(1.0)
def pauseSystem(self, pausing): learningStatus = Bool() controlStatus = Bool() #if not pausing: # self.expStatus = "monitor" #self.backToInit = not pausing learningStatus.data = not pausing self.learningMF_pub.publish(learningStatus) self.learningMB_pub.publish(learningStatus) self.learningMB2_pub.publish(learningStatus) controlStatus.data = not pausing self.control_pub.publish(controlStatus) self.askStopPlanning = not pausing planDecide = CommandSignal() planDecide.decide = not pausing self.plde_pub.publish(planDecide)
def pub_e( self , enable ):
""" pub actuator cmd """
msg = Bool()
msg.data = enable
self.pub_enable.publish( msg )
def send_move_status(msg):
"""
Send a movement status message.
:param msg: A bool, true if moving, false if stopped.
"""
global move_status_pub
status_msg = Bool()
status_msg.data = msg
move_status_pub.publish(status_msg)
def vacuum_state_cb(msg, arm):
print("state call back %8s %7.2f %s"%(arm, g_force[arm], msg.data))
pub_msg = Bool(False)
if g_force[arm] > 15 and msg.data is True:
pub_msg.data = True
if arm == "left":
left_vacuum_pub.publish(pub_msg)
elif arm == "right":
right_vacuum_pub.publish(pub_msg)
def halt_test():
rospy.init_node('halt_test')
bool_msg = Bool()
bool_msg.data = True
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub_halt = rospy.Publisher('/collide/halt', Bool, queue_size = 10)
print 'published successfully!'
rate.sleep()
pub_halt.publish(bool_msg)
def load(self):
rospy.loginfo("Sending load cmd")
cmd = Bool()
cmd.data = True
self.loader_pub.publish(cmd)
# might need to delay here
print "letting screw turn for 1 second"
time.sleep(2)
# turn dryer on
self.dryer_pub.publish(cmd)
print "Turning dryer on for 3 seconds"
time.sleep(6)
# turn dryer off
cmd.data = False
self.dryer_pub.publish(cmd)
def shutdown(self):
rospy.loginfo("Stopping the robot...")
# Stop the robot
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("enable BarDetectFlag")
# enable BarDetectFlag
flag=Bool()
flag.data=True
self.barDetectFlag_pub.publish(flag)
rospy.sleep(0.5)
def _stop_teleop_cb(self): ''' Sends the stop teleop command IGC ''' msg = Bool() msg.data = True ret = QMessageBox.question(self._widget, "Stop Arm Teleop", 'Do you want to stop the arm teleoperation?', QMessageBox.Ok, QMessageBox.Cancel) if ret == QMessageBox.Ok: self.teleop_done_pub.publish(msg)
def laser_callback(self, laser_scan):
if self.ping_index_ < 0:
self.set_scan_properties(laser_scan)
# Assume min/max angle are symmetrical and set up evenly on robot
# This is the ping index that is straight ahead
self.ping_index_ = int(
(0 - self.angle_min_) / self.angle_increment_)
rospy.loginfo("LIDAR setup: ping_index = %d", self.ping_index_)
# Set the beginning and end of our scan zone
self.min_index = self.ping_index_ - int(self.ALARM_SCAN_WIDTH / 2)
if self.min_index < 0:
rospy.logwarn("Lidar alarm scan width is too large!")
self.min_index = 0
self.max_index = self.ping_index_ + int(self.ALARM_SCAN_WIDTH / 2)
if self.max_index > len(laser_scan.ranges):
rospy.logwarn("Lidar alarm scan width is too large")
self.max_index = len(laser_scan.ranges)
rospy.loginfo("min index is %d", self.min_index)
rospy.loginfo("max index is %d", self.max_index)
count = 0
num_pings = 0
average = 0
for x in range(self.min_index, self.max_index):
if laser_scan.ranges[x] > self.range_min_:
if laser_scan.ranges[x] < self.range_max_:
num_pings += 1
rospy.logdebug("Ping %d with range %f", num_pings,
laser_scan.ranges[x])
average += laser_scan.ranges[x]
if laser_scan.ranges[x] < self.MIN_SAFE_DISTANCE:
count += 1
rospy.loginfo(
"%d valid lidar pings, %d dangerous pings, average distance %f",
num_pings, count, float(average) / float(num_pings))
if count > (num_pings * self.ALARM_TRIP_PERCENTAGE):
self.laser_alarm = True
rospy.loginfo("LIDAR ALARM! DANGER!")
else:
self.laser_alarm = False
lidar_alarm_msg = Bool()
lidar_alarm_msg.data = self.laser_alarm
self.lidar_alarm_publisher.publish(lidar_alarm_msg)
lidar_dist_msg = Float32()
lidar_dist_msg.data = self.ping_dist_in_front_
self.lidar_dist_publisher_.publish(lidar_dist_msg)
def compute_people(self, _data):
b = Bool()
b.data = _data
self.people_compute.publish(b)
rospy.loginfo("Detecting People: %s" % str(b.data))
def set_wrench_body_orientation_absolute(self, absolute):
"""Apply body wrench using body orientation (relative/False) or reference frame (absolute/True)"""
m = Bool()
m.data = absolute
self.__set_wrench_body_orientation_absolute_pub.publish(m)
def compute_objects(self, _data):
b = Bool()
b.data = _data
self.obj_compute.publish(b)
rospy.loginfo("Detecting Objects: %s" % str(b.data))
def master(sx=2., sy=2., gx=2., gy=2.):
# ---------------------------------- INITS ----------------------------------------------
# --- init node ---
rospy.init_node('hector_motion')
# --- cache global vars / constants ---
global msg_motion, turtle_motion, turtle_stop, height
msg_motion = None
turtle_motion = None
turtle_stop = None
height = None
# --- Publishers ---
pub_target = rospy.Publisher('/hector/target', PointStamped, latch=True, queue_size=1)
msg_target = PointStamped()
msg_target.header.frame_id = "map"
msg_target_position = msg_target.point
msg_target_position.x = sx
msg_target_position.y = sy
pub_target.publish(msg_target)
pub_state = rospy.Publisher('/hector/state', Int8, latch=True, queue_size=1)
msg_state = Int8()
msg_state.data = STATE_TAKEOFF
pub_state.publish(msg_state)
pub_stop = rospy.Publisher('/hector/stop', Bool, latch=True, queue_size=1)
msg_stop = Bool()
msg_stop.data = False
pub_stop.publish(msg_stop) # ping to others it is ready
# --- Subscribers ---
rospy.Subscriber('/hector/motion', EE4308MsgMotion, subscribe_motion, queue_size=1)
rospy.Subscriber('/turtle/motion', EE4308MsgMotion, subscribe_turtle_motion, queue_size=1)
rospy.Subscriber('/turtle/stop', Bool, subscribe_turtle_stop, queue_size=1)
rospy.Subscriber('/hector/sonar_height', Range, subscribe_sonar, queue_size=1)
while (height is None or msg_motion is None or turtle_stop is None or \
turtle_motion is None or rospy.get_time() == 0) and not rospy.is_shutdown():
pass
if rospy.is_shutdown():
return
######################################################
t = rospy.get_time()
while not rospy.is_shutdown():
if rospy.get_time() > t:
# --- FSM ---
# check if close enough to targets and goals with CLOSE_ENOUGH
## from project 1
if check_distance: # check if close enough to target
Di = target_x - msg_motion.x
Dj = target_y - msg_motion.y
if Di*Di + Dj*Dj <= CLOSE_ENOUGH_SQ:# if target reached
target_idx += 1
if target_idx < num_targets:# still have targets remaining
previous_x.append(target_x)
previous_y.append(target_y)
target_x += Dx
target_y += Dy
msg_target_position.x = target_x
msg_target_position.y = target_y
pub_target.publish(msg_target) # publish new target
else:
turnpt_idx -= 1
if turnpt_idx >= 0:
update_turnpoint = True
else:
update_goalpoint = True
## from project 1
# generate targets with TARGET_SEPARATION
# fly to CRUISE_ALTITUDE (you decide value) after takeoff
# use STATE_... constants for states
# --- Publish state ---
msg_state.data = STATE_TURTLE
pub_state.publish(msg_state)
# --- Publish target ---
msg_target_position.x = target_x
msg_target_position.y = target_y
msg_target_position.z = target_z
msg_target.header.seq += 1
pub_target.publish(msg_target)
# --- Timing ---
et = rospy.get_time() - t
t += ITERATION_PERIOD
if et > ITERATION_PERIOD:
print('[HEC MASTER] {} OVERSHOOT'.format(int(et*1000)))
and visual_pose_timeout != 0.0):
logger.info('Visual pose data is too old')
rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
local_pose_callback)
rospy.Subscriber('/mavros/setpoint_position/local', PoseStamped,
setpoint_position_callback)
rospy.Subscriber('/mavros/setpoint_raw/local', PositionTarget,
setpoint_raw_callback)
rospy.Subscriber('/mavros/state', State, state_callback)
rospy.Subscriber('/mavros/distance_sensor/rangefinder', Range, laser_callback)
emergency_pub = rospy.Publisher('/emergency', Bool, queue_size=10)
rospy.Service('emergency_land', Trigger, emergency_land_service)
rospy.Timer(rospy.Duration(0.5), watchdog_callback)
while not rospy.is_shutdown():
emergency_msg = Bool()
emergency_msg.data = emergency
emergency_pub.publish(emergency_msg)
rate.sleep()
def done_publish():
msg = Bool()
msg.data = True
done_pub.publish(msg)
return
def set_gravity_compensation(self, gravity_compensation):
"Turn on/off gravity compensation in cartesian effort mode"
g = Bool()
g.data = gravity_compensation
self.__set_gravity_compensation_pub.publish(g)
def init_ros(self):
# Create ROS Timer for vehicle heartbeat
self.heartbeat_received = False
self.last_heartbeat_time = datetime.datetime.now()
self.heartbeat_timer = self.node.create_timer(1,
self.heartbeat_callback)
# Create QOS profiles
qos_be = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
qos_klr_v = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)
qos_klr_tl = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
# Create Subscribers
self.sub_heartbeat = self.node.create_subscription(
msg_type=String,
topic='heartbeat',
callback=self.update_heartbeat,
qos_profile=qos_klr_v)
self.sub_camera_status = self.node.create_subscription(
msg_type=String,
topic='camera_status',
callback=self.update_camera_status,
qos_profile=qos_klr_tl)
self.sub_gps_status = self.node.create_subscription(
msg_type=String,
topic='gps_status',
callback=self.update_gps_status,
qos_profile=qos_klr_tl)
self.sub_storage_status = self.node.create_subscription(
msg_type=String,
topic='storage_status',
callback=self.update_storage_status,
qos_profile=qos_klr_tl)
self.sub_system_time = self.node.create_subscription(
msg_type=String,
topic='system_time',
callback=self.update_system_time,
qos_profile=qos_klr_tl)
self.sub_recording_status = self.node.create_subscription(
msg_type=String,
topic='recording_status',
callback=self.update_recording_status,
qos_profile=qos_klr_tl)
self.sub_recorded_samples = self.node.create_subscription(
msg_type=String,
topic='recorded_samples',
callback=self.update_recorded_samples,
qos_profile=qos_klr_tl)
self.sub_video_left = self.node.create_subscription(
msg_type=CompressedImage,
topic='camera_left/image_raw/compressed',
callback=self.update_video_image,
qos_profile=qos_be)
# Create publisher
self.pub_req_enable_recording = self.node.create_publisher(
msg_type=Bool,
topic='recording_enabled_target',
qos_profile=qos_klr_tl)
# Set initial recording enable target
msg = Bool()
msg.data = self.recording_enabled_target
self.pub_req_enable_recording.publish(msg)
def main():
print "INITIALIZING TORSO NODE IN SIMULATION MODE BY MARCOSOFT..."
###Connection with ROS
global pubGoalReached
rospy.init_node("torso")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["arm_lift_joint"]
jointStates.position = [0.0]
rospy.Subscriber("/hardware/torso/goal_pose", Float32, callbackGoalPose)
rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray,
callbackRelPose)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubCurrentPose = rospy.Publisher("/hardware/torso/current_pose",
Float32,
queue_size=1)
pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached",
Bool,
queue_size=1)
loop = rospy.Rate(30)
global goalSpine
global spine
global newGoal
spine = 0.10
goalSpine = spine
# goalWaist = waist
# goalShoulders = shoulders
speedSpine = 0.005
speedWaist = 0.1
speedShoulders = 0.1
msgCurrentPose = Float32()
msgGoalReached = Bool()
msgCurrentPose.data = [0, 0, 0]
newGoal = False
while not rospy.is_shutdown():
deltaSpine = goalSpine - spine
# deltaWaist = goalWaist - waist;
# deltaShoulders = goalShoulders - shoulders;
if deltaSpine > speedSpine:
deltaSpine = speedSpine
if deltaSpine < -speedSpine:
deltaSpine = -speedSpine
# if deltaWaist > speedWaist:
# deltaWaist = speedWaist;
# if deltaWaist < -speedWaist:
# deltaWaist = -speedWaist;
# if deltaShoulders > speedShoulders:
# deltaShoulders = speedShoulders;
# if deltaShoulders < -speedShoulders:
# deltaShoulders = -speedShoulders;
spine += deltaSpine
# waist += deltaWaist
# shoulders += deltaShoulders
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [spine]
pubJointStates.publish(jointStates)
msgCurrentPose.data = spine
pubCurrentPose.publish(msgCurrentPose)
#if newGoal and abs(goalSpine - spine) < 0.02 and abs(goalWaist - waist) < 0.05 and abs(goalShoulders - shoulders) < 0.05:
if newGoal and abs(goalSpine - spine) < 0.02:
newGoal = False
msgGoalReached.data = True
pubGoalReached.publish(msgGoalReached)
loop.sleep()
def main():
print "INITIALIZING TORSO..."
global simul
global pubGoalReached
portName1 = "/dev/justinaTorso"
###Connection with ROS
rospy.init_node("torso")
if rospy.has_param('~simul'):
simul = rospy.get_param('~simul')
else:
simul = True
if rospy.has_param('~port'):
portName1 = rospy.get_param('~port')
elif not simul:
print_help()
sys.exit()
jointStates = JointState()
jointStates.name = [
"spine_connect", "waist_connect", "shoulders_connect",
"shoulders_left_connect", "shoulders_right_connect"
]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose",
Float32MultiArray,
queue_size=1)
pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached",
Bool,
queue_size=1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubStop = rospy.Publisher("robot_state/stop", Empty, queue_size=1)
pubEmergencyStop = rospy.Publisher("robot_state/emergency_stop",
Bool,
queue_size=1)
subRelativeHeight = rospy.Subscriber("/hardware/torso/goal_rel_pose",
Float32MultiArray, callbackRelative)
subAbsoluteHeight = rospy.Subscriber("/hardware/torso/goal_pose",
Float32MultiArray, callbackAbsolute)
subStop = rospy.Subscriber("robot_state/stop", Empty, callbackStop)
subEmergencyStop = rospy.Subscriber("robot_state/emergency_stop", Bool,
callbackEmergency)
subTorsoUp = rospy.Subscriber("/hardware/torso/torso_up", String,
callbackTorsoUp)
subTorsoDown = rospy.Subscriber("/hardware/torso/torso_down", String,
callbackTorsoDown)
subSimul = rospy.Subscriber("/simulated", Bool, callbackSimul)
rate = rospy.Rate(ITER_RATE)
global valueRel
global valueAbs
global absH
global relH
global stop
global torsoUp
global torsoDown
global eme_stop
global new_eme_msg_recv
valueAbs = False
valueRel = False
torsoUp = False
torsoDown = False
torsoPos = 0
bumper = 0
stop = False
msgCurrentPose = Float32MultiArray()
msgGoalReached = Bool()
msgCurrentPose.data = [0, 0, 0]
msgMotor = None
initTimeMtrMsg = datetime.now()
initTimeSnrMsg = datetime.now()
timeoutSnr = 0
timeoutMtr = 0
if not simul:
ArdIfc = comm.Comm(portName1)
msgSensor = comm.Msg(comm.ARDUINO_ID, comm.MOD_SENSORS,
comm.OP_GETCURRENTDIST, [], 0)
ArdIfc.send(msgSensor)
goalPose = 0
new_eme_msg_recv = False
eme_stop = Bool()
eme_stop.data = False
while not rospy.is_shutdown():
try:
initTorso = torsoPos
if not simul:
timeoutSnr = datetime.now() - initTimeSnrMsg
if timeoutSnr.microseconds > MSG_SENSOR_TIMEOUT:
ArdIfc.send(msgSensor)
initTimeSnrMsg = datetime.now()
newMsg = ArdIfc.recv()
if newMsg != None:
if newMsg.mod == comm.MOD_SENSORS:
if newMsg.op == comm.OP_GETCURRENTDIST:
torsoPos = newMsg.param[0]
#rospy.loginfo("Torso-> Arduino ack GET CURRENT DIST msg received.")
if newMsg.mod == comm.MOD_SYSTEM:
if newMsg.op == comm.OP_PING:
rospy.loginfo(
"Torso-> Arduino ack PING msg received.")
if newMsg.op == comm.OP_STOP:
rospy.loginfo(
"Torso-> Arduino Emercenty STOP system received. "
)
if eme_stop.data != bool(newMsg.param[0]):
eme_stop.data = newMsg.param[0]
pubEmergencyStop.publish(eme_stop)
if newMsg.mod == comm.MOD_MOTORS:
if newMsg.op == comm.OP_SETTORSOPOSE:
msgMotor_ack_received = True
rospy.loginfo(
"Torso-> Arduino ack SETTORSOPOSE msg received."
)
if newMsg.op == comm.OP_GOUP:
msgMotor_ack_received = True
rospy.loginfo(
"Torso-> Arduino ack GOUP msg received.")
if newMsg.op == comm.OP_GODOWN:
msgMotor_ack_received = True
rospy.loginfo(
"Torso-> Arduino ack GODOWN msg received.")
if newMsg.op == comm.OP_STOP_MOTOR:
msgMotor_ack_received = True
#rospy.loginfo("Torso-> Arduino ack STOP MOTOR msg received.")
#until ack received
timeoutMtr = datetime.now() - initTimeMtrMsg
if msgMotor != None and timeoutMtr.microseconds > MSG_MOTOR_TIMEOUT and not msgMotor_ack_received:
ArdIfc.send(msgMotor)
initTimeMtrMsg = datetime.now()
if valueAbs and not eme_stop.data and absH >= DIST_LIM_INF and absH <= DIST_LIM_SUP:
msgMotor_ack_received = False
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_SETTORSOPOSE, int(absH), 1)
ArdIfc.send(msgMotor)
valueAbs = False
goalPose = absH
initTimeMtrMsg = datetime.now()
elif valueRel and not eme_stop.data and (
torsoPos + relH) >= DIST_LIM_INF and (
torsoPos + relH) <= DIST_LIM_SUP:
msgMotor_ack_received = False
absCalH = torsoPos + relH
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_SETTORSOPOSE, int(absCalH), 1)
ArdIfc.send(msgMotor)
goalPose = absCalH
valueRel = False
initTimeMtrMsg = datetime.now()
elif (valueAbs and
(absH < DIST_LIM_INF or absH > DIST_LIM_SUP)) or (
valueRel and (torsoPos + relH > DIST_LIM_SUP
or torsoPos + relH < DIST_LIM_INF)):
rospy.logerr("Torso-> Can not reach te position.")
valueAbs = False
valueRel = False
goalPose = torsoPos
elif torsoUp and not eme_stop.data:
rospy.loginfo("Torso-> Moving torso up.")
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_GOUP, [], 0)
ArdIfc.send(msgMotor)
torsoUp = False
msgMotor_ack_received = False
initTimeMtrMsg = datetime.now()
goalPose = torsoPos
elif torsoDown and not eme_stop.data:
rospy.loginfo("Torso-> Moving torso down.")
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_GODOWN, [], 0)
ArdIfc.send(msgMotor)
torsoDown = False
msgMotor_ack_received = False
initTimeMtrMsg = datetime.now()
goalPose = torsoPos
elif eme_stop.data and new_eme_msg_recv:
rospy.loginfo("Torso-> Stop message.")
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_STOP_MOTOR, [], 0)
ArdIfc.send(msgMotor)
msgMotor_ack_received = False
initTimeMtrMsg = datetime.now()
new_eme_msg_recv = False
torsoDown = False
torsoUp = False
valueAbs = False
valueRel = False
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [(torsoPos - TORSO_ADJUSTMENT) / 100.0,
0.0, 0.0, 0.0, 0.0]
pubJointStates.publish(jointStates)
msgCurrentPose.data[0] = (torsoPos - TORSO_ADJUSTMENT) / 100.0
msgCurrentPose.data[1] = 0.0
msgCurrentPose.data[2] = 0.0
pubTorsoPos.publish(msgCurrentPose)
msgGoalReached.data = abs(goalPose - torsoPos) < THR_DIFF_POS
pubGoalReached.publish(msgGoalReached)
else:
if valueAbs and not stop:
torsoPos = absH
valueAbs = False
elif valueRel and not stop:
torsoPos = torsoPos + relH
valueRel = False
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [(torsoPos * 0.352) / 169733, 0.0, 0.0,
0.0, 0.0]
pubJointStates.publish(jointStates)
msgCurrentPose.data[0] = (
torsoPos *
0.352) / 169733 #-------------------pulsos a metros
msgCurrentPose.data[1] = 0.0
msgCurrentPose.data[2] = 0.0
pubTorsoPos.publish(msgCurrentPose)
msgGoalReached.data = abs(initTorso - torsoPos) < 200
pubGoalReached.publish(msgGoalReached)
rate.sleep()
except:
rospy.logerr(
"Torso-> Oopps...we have some issues in this node :( ")
def gripperClose(self, bool):
msg = Bool()
msg.data = bool
self.gripper_pub.publish(msg)
def dron_sate(msg):
armed = Bool()
armed.data = msg.armed
armed_pub.publish(armed)
def publish_hotspot_state(self, activated):
msg = Bool()
msg.data = activated
self.hotspot_state_publisher.publish(msg)
def calibrated_cb(self, req):
r = Bool()
r.data = True
self.calibrated_pub.publish(r)
return EmptyResponse()
except Exception as e:
print("second load marge error:", e.args)
if "recipe" in Config:
srcpath = re.subn(r".*?/", "/", thispath[::-1], 1)[0][::-1]
dirpath = srcpath + Config["recipe"]["dir"]
linkpath = srcpath + Config["recipe"]["link"]
print("dirpath", dirpath)
print("linkpath", linkpath)
try:
dictlib.merge(Config, rospy.get_param("/config/dashboard"))
except Exception as e:
print("get_param exception:", e.args)
####Bools
mTrue = Bool()
mTrue.data = True
mFalse = Bool()
####sub pub
rospy.Subscriber("~load", String, cb_load)
rospy.Subscriber("/message", String, functools.partial(cb_mbox_push, 0))
rospy.Subscriber("/error", String, functools.partial(cb_mbox_push, 2))
rospy.Subscriber("/shutdown", Bool, cb_shutdown)
pub_Y3 = rospy.Publisher("~loaded", Bool, queue_size=1)
pub_E3 = rospy.Publisher("~failed", Bool, queue_size=1)
pub_msg = rospy.Publisher("/message", String, queue_size=1)
pub_redraw = rospy.Publisher("/request/redraw", Bool, queue_size=1)
####Layout####
normalfont = (Config["font"]["family"], Config["font"]["size"], "normal")
boldfont = (Config["font"]["family"], Config["font"]["size"], "bold")
def cb_notif(event):
ret = Bool()
ret.data = True
pub_Y2.publish(ret)
def stopMove(self,flag):
mes = Bool()
mes.data = flag
for _ in range(15):
self.stop.publish(mes)
def report_readiness(self, ready_bool):
msg = Bool()
msg.data = ready_bool
self.controller_ready_pub.publish(msg)
def navigation(self):
pose = Pose()
bool = Bool()
# STATE 1: GO TO THE OTHER SIDE OF THE COURT BYPASSING THE NET
#-------------------------------------------------------------------
if self.state == 1:
if self.is_ball_on_court == True:
if np.sign(self.pose_rob.y) != np.sign(self.pose_ball_y):
self.x_wp = 6.5*np.sign(self.poserob.x)
self.y_wp = 0.0
else:
self.state = 2
else :
self.state = 3
#-------------------------------------------------------------------
# STATE 2: GO THE NEXT BALL TO COLLECT IT
#-------------------------------------------------------------------
if self.state == 2:
if self.is_ball_on_court == True:
if np.sign(self.pose_rob.y) != np.sign(self.pose_ball_y):
state = 1
else:
self.x_wp = self.pose_ball_x
self.y_wp = self.pose_ball_y
else:
self.state = 3
#-------------------------------------------------------------------
# STATE 3: BRING THE BALL INTO THE COLLECTING AREAS
#-------------------------------------------------------------------
if self.state == 3:
if self.is_ball_collected == True:
self.x_wp = 7.0*np.sign(self.pose_rob.y)
self.y_wp = 14.0*np.sign(self.pose_rob.y)
else:
self.state = 2
#-------------------------------------------------------------------
# STATE 0: IDLE
#-------------------------------------------------------------------
else: # state == 0
if self.is_ball_on_court == True:
self.state = 2
self.move == True
else:
if 6.0 < abs(self.pose_rob.x) < 7.0 and abs(self.pose_rob.y) < .5:
# stop robot
self.move = False
else:
self.state = 1
self.move = True
#-------------------------------------------------------------------
# PUBLISHING WAYPONT (POSE) AND MOVING COMMAND (BOOL)
#-------------------------------------------------------------------
pose.pose.position.x = self.x_wp
pose.pose.position.y = self.y_wp
self.wp_pub.publish(pose)
self.get_logger().info("Publishing: {}, {}, {}".format(pose.x, pose.y))
bool.data = self.move
self.move_pub.publish(bool)
def path_follow(pose_msg_in):
# First assign the incoming message
global estimated_pose, path_segment_spec
estimated_pose = pose_msg_in
pose_xy = np.array([estimated_pose.x, estimated_pose.y])
pose_theta = np.array([estimated_pose.theta])
# Set up global variables
global initialize_psi_world, estimated_pose_previous, estimated_x_local_previous, estimated_theta_local_previous, path_segment_curvature_previous, estimated_psi_world_previous, estimated_segment_completion_fraction_previous
# Create a vector variable for the Origin of the path segment
path_segment_Origin = np.array(
[path_segment_spec.x0, path_segment_spec.y0])
# Forward distance relative to a Line path is computed along the Forward axis.
# Therefore Define the Forward Axis:
path_segment_y0_vector = np.array(
[-np.sin(path_segment_spec.theta0),
np.cos(path_segment_spec.theta0)])
# local X is computed perpendicular to the segment.
# Therefore define the Perpendicular axis.
path_segment_x0_vector = np.array([
-np.sin(path_segment_spec.theta0 - np.pi / 2),
np.cos(path_segment_spec.theta0 - np.pi / 2)
])
# Define path curvature
path_segment_curvature = 1 / path_segment_spec.Radius
# =============================================================================
# ### First Compute the robot's position relative to the path (x, y, theta)
# ### and the local path properties (curvature 1/R and segment completion percentage)
# =============================================================================
# Line (Infinite radius)
if np.isinf(path_segment_spec.Radius):
path_segment_endpoint = path_segment_Origin + path_segment_spec.Length * path_segment_y0_vector
# compute position relative to Segment:
estimated_xy_rel_to_segment_origin = (
pose_xy - path_segment_Origin
) # XY vector from Origin of segment to current location of robot.
# Projection of vector from path origin to current position
estimated_segment_forward_pos = estimated_xy_rel_to_segment_origin.dot(
path_segment_y0_vector)
# The fraction completion can be estimated as the path length the robot has gone through, as a fraction of total path length on this segment
estimated_segment_completion_fraction = estimated_segment_forward_pos / path_segment_spec.Length
# Position of the robot to the Right of the segment Origin
estimated_segment_rightward_pos = estimated_xy_rel_to_segment_origin.dot(
path_segment_x0_vector)
estimated_y_local = 0 # y_local = 0 by definition: Local coords are defined relative to the closest point on the path.
estimated_x_local = estimated_segment_rightward_pos
estimated_theta_local = pose_theta - path_segment_spec.theta0
# Arc
else:
curve_sign = np.sign(path_segment_spec.Radius)
path_segment_circle_center = path_segment_Origin + path_segment_spec.Radius * -path_segment_x0_vector
# determine the angular displacement of this arc. SIGNED quantity!
path_segment_angular_displacement = path_segment_spec.Length / path_segment_spec.Radius
path_segment_ThetaEnd = path_segment_spec.theta0 + path_segment_angular_displacement
estimated_xy_rel_to_circle_center = (pose_xy -
path_segment_circle_center)
# Compute angle of a vector from circle center to Robot, in the world frame, relative to the +Yworld axis.
# Note how this definition affects the signs of the arguments to "arctan2"
estimated_psi_world = np.arctan2(-estimated_xy_rel_to_circle_center[0],
estimated_xy_rel_to_circle_center[1])
# unwrap the angular displacement
if initialize_psi_world:
estimated_psi_world_previous = estimated_psi_world
initialize_psi_world = False
while estimated_psi_world - estimated_psi_world_previous > np.pi: # was negative, is now positive --> should be more negative.
estimated_psi_world += -2 * np.pi
while estimated_psi_world - estimated_psi_world_previous < -np.pi: # was positive and is now negative --> should be more positive.
estimated_psi_world += 2 * np.pi
# update the "previous angle" memory.
estimated_psi_world_previous = estimated_psi_world
# The local path forward direction is perpendicular (clockwise) to this World frame origin-to-robot angle.
estimated_path_theta = estimated_psi_world + np.pi / 2 * curve_sign
# The fraction completion can be estimated as the path angle the robot has gone through, as a fraction of total angular displacement on this segment
estimated_segment_completion_fraction = (
estimated_path_theta -
path_segment_spec.theta0) / path_segment_angular_displacement
estimated_y_local = 0 # by definition of local coords
# x_local is positive Inside the circle for Right turns, and Outside the circle for Left turns
estimated_x_local = curve_sign * (
np.sqrt(np.sum(np.square(estimated_xy_rel_to_circle_center))) -
np.abs(path_segment_spec.Radius))
estimated_theta_local = pose_theta - estimated_path_theta
## Whether Line or Arc, update the "local" coordinate state and path properties:
estimated_theta_local = m439rbt.fix_angle_pi_to_neg_pi(
estimated_theta_local)
# Update the "previous" values
estimated_pose_previous = estimated_pose
estimated_x_local_previous = estimated_x_local
estimated_theta_local_previous = estimated_theta_local
path_segment_curvature_previous = path_segment_curvature
estimated_segment_completion_fraction_previous = estimated_segment_completion_fraction
# =============================================================================
# ### CONTROLLER for path tracking based on local position and curvature.
# # parameters for the controller are
# # Vmax: Maximum allowable speed,
# # and controller gains:
# # Beta (gain on lateral error, mapping to lateral speed)
# # gamma (gain on heading error, mapping to rotational speed)
# # and a control variable for the precision of turns,
# # angle_focus_factor
# =============================================================================
global Vmax, Beta, gamma, angle_focus_factor
# First set the speed with which we want the robot to approach the path
xdot_local_desired = -Beta * estimated_x_local
# limit it to +-Vmax
xdot_local_desired = np.min([np.abs(xdot_local_desired),
abs(Vmax)]) * np.sign(xdot_local_desired)
# Next set the desired theta_local
theta_local_desired = -np.arcsin(xdot_local_desired / Vmax)
## Next SET SPEED OF ROBOT CENTER.
## G. Cook 2011 says just use constant speed all the time,
Vc = Vmax
## But, that drives farther from the path at first if it is facing away.
## This FIX causes the speed to fall to zero if the robot is more than 90 deg from the heading we want it to have.
## The parameter "angle_focus_factor" can make it even more restrictive if needed (e.g. angle_focus_factor = 2 --> 45 deg limit).
## Value of 1.0 uses a straight cosine of the angle.
Vc = Vmax * np.cos(angle_focus_factor *
m439rbt.fix_angle_pi_to_neg_pi(theta_local_desired -
estimated_theta_local))
## Could also limit it to only forward. Try with and without this!
Vc = np.max([Vc, 0])
# Finally set the desired angular rate
omega = gamma * m439rbt.fix_angle_pi_to_neg_pi(
theta_local_desired - estimated_theta_local) + Vc * (
path_segment_curvature /
(1 + path_segment_curvature * estimated_x_local)
) * np.cos(estimated_theta_local)
# Finally, use the "robot" object to translate these into wheel speeds
global robot
robot.set_wheel_speeds_from_robot_velocities(Vc, omega)
# Check if the path is complete
# If so, stop!
global path_is_complete
if path_is_complete:
robot.set_wheel_speeds(0., 0.)
# ... and Publish it
global pub_speeds
# Set up the message that will go on that topic.
msg_speeds = ME439WheelSpeeds()
msg_speeds.v_left = robot.left_wheel_speed
msg_speeds.v_right = robot.right_wheel_speed
pub_speeds.publish(msg_speeds)
print(estimated_segment_completion_fraction)
# Finally, if the segment is complete, publish that fact
if estimated_segment_completion_fraction >= 1.0:
global pub_segment_complete
msg_seg_complete = Bool()
msg_seg_complete.data = True
pub_segment_complete.publish(msg_seg_complete)
# and clear the memory of angle wrapping:
initialize_psi_world = True
# reinsforcement learning
new_zone, new_ray, new_angle = zone_boat(
zone_to_stay, psi, np.array([pos_x, pos_y]))
if new_zone != prev_zone:
#Chose an action
thetabar, act = chose_action(epsilon, new_ray, new_angle,
Q_table)
# Update the Q-table
reward = rewards(prev_zone, new_zone, act)
Q_table[prev_angle, prev_ray, act] = Q_table[
prev_angle, prev_ray, act] + learning_rate * (
(reward + actualization_rate *
(np.argmax(Q_table[new_angle, new_ray, :]))) -
Q_table[prev_angle, prev_ray, act])
u = cl.control_station_keeping(thetabar, fut_x, psi)
# rospy.loginfo(" Zone {}, ray de {}, angle de {}".format(new_zone,new_ray,new_angle))
prev_zone, prev_ray, prev_angle = new_zone, new_ray, new_angle
u_rudder_msg.data = u[0, 0]
u_sail_msg.data = u[1, 0]
pub_send_u_rudder.publish(u_rudder_msg)
pub_send_u_sail.publish(u_sail_msg)
pub_send_reset.publish(reset_msg)
if reset_msg.data == True:
reset_msg.data = False
rate.sleep()
nbr_ite += 1
rospy.loginfo("Iteration nbr {}/{}".format(nbr_ite, nbr_episodes))
save_data(Q_table)
rospy.loginfo("End of the training")
def close(self):
message = Bool()
message.data = True
self.RS.publish(message)
time.sleep(self.main_state.configs["RS_WAIT"])
super().close()
pub_Y2 = rospy.Publisher("~solved", Bool, queue_size=1)
pub_busy = rospy.Publisher("~stat", Bool, queue_size=1)
pub_saved = rospy.Publisher("~saved", Bool, queue_size=1)
pub_loaded = rospy.Publisher("~loaded", Bool, queue_size=1)
pub_score = rospy.Publisher("~score", Float32MultiArray, queue_size=1)
rospy.Subscriber("~clear", Bool, cb_clear)
rospy.Subscriber("~solve", Bool, cb_solve)
rospy.Subscriber("~save", Bool, cb_save)
rospy.Subscriber("~load", Bool, cb_load)
rospy.Subscriber("~redraw", Bool, cb_master)
pub_msg = rospy.Publisher("/message", String, queue_size=1)
pub_err = rospy.Publisher("/error", String, queue_size=1)
###std_msgs/Bool
mTrue = Bool()
mTrue.data = True
mFalse = Bool()
mFalse.data = False
###TF
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
broadcaster = tf2_ros.StaticTransformBroadcaster()
###data
Scene = [None] * len(Config["scenes"])
Model = [None] * len(Config["scenes"])
tfReg = []
tfSolve = []
rospy.Timer(rospy.Duration(5), cb_load, oneshot=True)
cro = Bool() cir = Bool() tri = Bool() l1 = Bool() r1 = Bool() l3 = Bool() r3 = Bool() opt = Bool() ps = Bool() pad = Bool() keypad = Twist() keypad.linear.x = 0 keypad.linear.y = 0 squ.data = False cro.data = False cir.data = False tri.data = False l1.data = False r1.data = False l3.data = False r3.data = False opt.data = False ps.data = False pad.data = False reverse_fac = 1.0 speeding_fac = 1.0 left_mode = 1.0
def end_experiment_pub(self, flag):
msg = Bool()
msg.data = flag
self.pub_end_experiment.publish(msg)
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from drive_by_wire.msg import Cart_values
from std_msgs.msg import Bool
# import keyboard
pub = rospy.Publisher('manual', Bool, queue_size=10)
rospy.init_node('keyboard_input', anonymous=True)
rate = rospy.Rate(30) # 10hz
val = Bool()
val.data = False
def keyboard_input():
global val
while not rospy.is_shutdown():
text = raw_input()
# if 'w' in text:
# if vel_msg.linear.x < vel_max_linear:
# vel_msg.linear.x += vel_linear_inc
# if 's' in text:
# if vel_msg.linear.x > -1*vel_max_linear:
# vel_msg.linear.x -= vel_linear_inc
# if 'd' in text:
# if vel_msg.angular.z > -1*vel_max_angular:
# vel_msg.angular.z -= vel_angular_inc
# if 'a' in text:
# if vel_msg.angular.z < vel_max_angular:
def __init__(self):
# Init attributes
self.default_speed = 0.5
self.speed = self.default_speed
self.steering_angle = 0
self.robot_length = 0.22
self.robot_pose = None#(-0.3, -0.1789, -0.0246)
self.destination_pose = None
self.stop_point = None
self.waypoints = rospy.get_param('~path')
#self.waypoints = [(0, 0),(2,2),(-1,1),(-3,3),(-3,0),(1,-2),(0,0)]
self.current_waypoint_index = 0
self.distance_from_path = None
self.lookahead_distance = rospy.get_param("~lookahead")
#self.lookahead_distance_adjust = self.lookahead_distance
self.threshold_proximity = 0.2 # How close the robot needs to be to the final waypoint to stop driving
self.active = True
self.start = True
# Init subscribers and publishers
self.pub_gazebo = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.pub_lookahead = rospy.Publisher('/pure_pursuit/lookahead', Point, queue_size=1)
self.pub_finish = rospy.Publisher('/pure_pursuit/finished', Bool, queue_size=1)
while True:
rospy.wait_for_service('/gazebo/get_model_state')
try:
get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
model_state = get_model_state('duckiebot',"")
msg = model_state
#print msg
except rospy.ServiceException, e:
pass
if not self.active:
#break
return
finish = Bool()
finish.data = False
self.pub_finish.publish(finish)
quaternion_msg = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]
euler = tf.transformations.euler_from_quaternion(quaternion_msg)
#print euler[2]
#print
self.robot_pose = (msg.pose.position.x, msg.pose.position.y, euler[2])
# print self.robot_pose
#print 'robot_pose:', self.robot_pose#, 'waypoints:', self.waypoints
self.destination_pose = self.pure_pursuit()
if self.destination_pose == None:
self.active = False
print "approach destination "
self.gazebo_cmd(0,0)
msg = Bool()
msg.data = True
self.pub_finish.publish(msg)
else:
self.publish_lookhead(self.destination_pose)
self.speed = self.default_speed
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
# self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination)
# print "robot_head",euler[2]*180/math.pi,"angle_to_destination", angle_to_destination*180/math.pi
#print angle_to_destination
w = 3*((angle_to_destination + math.pi) / (2 * math.pi) - 0.5)
'''if w > 0:
w = 1
else:
w = -1'''
#print angle_to_destination
self.gazebo_cmd(self.speed,w)
cmd.acceleration.x = 0
cmd.acceleration.y = 0
cmd.acceleration.z = 0
cmd.jerk.x = 0
cmd.jerk.y = 0
cmd.jerk.z = 0
cmd.yaw = 0
cmd.yaw_dot = 0
cmd.kx = [1, 1, 1]
cmd.kv = [.5, .5, .5]
return cmd
if __name__ == "__main__":
rospy.init_node('hover_cmd')
pub = rospy.Publisher('position_cmd', PositionCommand, queue_size=10)
rate = rospy.Rate(20) # 10hz
motorPub = rospy.Publisher('motors', Bool, queue_size=10)
enable = Bool()
enable.data = True
rospy.sleep(1)
motorPub.publish(enable)
while not rospy.is_shutdown():
pub.publish(getCommand())
rate.sleep()
# when the python interpreter runs this script
# this is the section that gets run as main
if __name__ == '__main__':
# let the master know about our name
rospy.init_node("riss")
# define and fill out some messages
publisherDataType = rospy.get_param("~publisher_data_type")
waitDuration = rospy.get_param("~wait_duration")
# define a duration of 1 second
myDuration = rospy.Duration(waitDuration, 0)
if (publisherDataType == "bool"):
msg = Bool()
msg.data = 0
myPublisher = rospy.Publisher('~my_ros_topic', Bool, queue_size=5)
elif (publisherDataType == "uint16"):
msg = UInt16()
msg.data = 10
myPublisher = rospy.Publisher('~my_ros_topic', UInt16, queue_size=5)
else:
msg = String()
msg.data = "publisher data type was not bool or uint16."
myPublisher = rospy.Publisher('~my_ros_topic', String, queue_size=5)
# wait for Ctrl-C and loop until then
while not rospy.is_shutdown():
# print a message for feedback
print "looping..."
def send_exe_camera(self, BOOL):
exe_msg = Bool()
exe_msg.data = BOOL
self.pub_camera.publish(exe_msg)
