def robot_state_publisher(self):
if self.current_robot_status['ready']:
state_num = 0
if self.current_robot_status['busy']:
state_num = 1
if self.current_robot_status['direct_teaching']:
state_num = 2
if self.current_robot_status['collision']:
state_num = 3
if self.current_robot_status['emergency']:
state_num = 4
status_msg = GoalStatusArray()
status_msg.header.stamp = rospy.Time.now()
status = GoalStatus()
status.goal_id.stamp = rospy.Time.now()
status.goal_id.id = ""
status.status = state_num
status.text = ROBOT_STATE[state_num]
status_msg.status_list = [status]
self.indy_state_pub.publish(status_msg)
def __init__(self):
self.ang_fwd_threshold = rospy.get_param('~ang_fwd_threshold', 0.5)
self.ang_vel_intersection = rospy.get_param('~ang_vel_intersection',
1.82)
self.dist_alarm_1 = rospy.get_param('~dist_alarm_1', 5.0)
self.dist_alarm_2 = rospy.get_param('~dist_alarm_2', 3.0)
self.spin_cycle = rospy.Duration(rospy.get_param('~spin_cycle', 0.3))
self.plan_cycle = rospy.Duration(rospy.get_param('~plan_cycle', 1.0))
print(C_YELLO + '\rTask planner, GVG 서비스 확인중...' + C_END)
rospy.wait_for_service('gvg/nearest')
rospy.wait_for_service('gvg/neighbors')
rospy.wait_for_service('gvg/node')
self.get_nearest = rospy.ServiceProxy('gvg/nearest', Nearest)
self.get_neighbors = rospy.ServiceProxy('gvg/neighbors', Neighbors)
self.get_node = rospy.ServiceProxy('gvg/node', Node)
print(C_YELLO + '\rTask planner, GVG 서비스 확인 완료' + C_END)
print(C_YELLO + '\rTask planner, 자율주행 서비스 확인중...' + C_END)
self.publisher_cmd_vel = rospy.Publisher('cmd_vel',
Twist,
queue_size=1)
self.publisher_robot_motion = rospy.Publisher('interf/robot/motion',
RobotMotion,
queue_size=1)
self.publisher_nav_cue = rospy.Publisher('interf/nav_cue',
NavCue,
queue_size=1)
print(C_YELLO + '\rTask planner, Action 서비스 확인중...' + C_END)
self.move_result = GoalStatus()
self.move_result.status = 3
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
self.percussion_time = rospy.get_time()
print(C_YELLO + '\rTask planner, 토픽 구독중...' + C_END)
rospy.Subscriber('interf/cmd/assist', CmdAssist, self.percussion)
rospy.Subscriber('robot/pose', PoseWithCovarianceStamped,
self.update_robot_pose)
rospy.Subscriber('move_base/result', MoveBaseActionResult,
self.update_move_result)
self.time_start = rospy.Time.now()
rospy.Subscriber('time/start', Time, self.update_time)
print(C_YELLO + '\rTask planner, 초기자세로 이동중...' + C_END)
self.robot_state = S_INDIRECT_WAIT
self.mount_gvg()
print(C_GREEN + '\rTask planner, 자율주행 서비스 초기화 완료' + C_END)
print(C_YELLO + '\rTask planner, BCI 서비스 확인중...' + C_END)
rospy.wait_for_service('interf/nav2cmd')
self.get_cmd = rospy.ServiceProxy('interf/nav2cmd', Nav2Cmd)
print(C_YELLO + '\rTask planner, BCI 서비스 확인 완료' + C_END)
print(C_YELLO + '\rTask planner, 기동중...' + C_END)
rospy.Timer(self.plan_cycle, self.explosion)
print(C_GREEN + '\rTask planner, 초기화 완료\n' + C_END)
def PubCancleRequest(self, ID, data): #pub cancle request to planner
CanclePub = rospy.Publisher('/%s/cancle' % self.ControlBase,
GoalStatus,
queue_size=1)
PubData = GoalStatus()
PubData.goal_id = int(ID.split('/')[1])
PubData.status = data
CanclePub.publish(PubData)
def make_goal_array(self):
for path in range(0, len(self.path_points) - 1):
new_goal = GoalStatus()
new_goal.goal_id.id = str(path)
new_goal.status = 0
new_goal.text = 'PENDING'
self.goal_arr.status_list.append(new_goal)
def waitOnGripperSuccess(self, lr): msg = GoalStatus() if lr == 'left' or lr == 'l': while(self.left_gripper_result != msg.SUCCEEDED): rospy.sleep(0.1) self.left_gripper_result = -1 if lr == "right" or lr == 'r': while(self.right_gripper_result != msg.SUCCEEDED): rospy.sleep(0.1) self.right_gripper_result = -1
def move_goal(self, point, frame_time=0):
'''
指定されたゴール地点に移動する
Parameters
----------
point : geometry_msgs/Point
カメラ座標系のオブジェクト位置
Returns
-------
client.get_result : Bool
actionlibの実行結果
'''
# tfを取得
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
try:
# カメラ → ベースリンクへの座標変換を取得
trans1 = tfBuffer.lookup_transform('base_link', self.cam_frame,
rospy.Time(0),
rospy.Duration(3.0))
# ベースリンク → マップへの座標変換を取得
trans2 = tfBuffer.lookup_transform('map', 'base_link',
rospy.Time(0),
rospy.Duration(3.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print(e)
else:
goal = self.transform_pose(
self.calc_goal(self.transform_point(point, trans1)), trans2)
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.stamp = rospy.Time.now()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose = goal
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
#rospy.loginfo('connected to server')
client.send_goal(goal_pose)
rospy.loginfo('Send goal pos : [%5.3f, %5.3f, %5.3f]',
goal.position.x, goal.position.y, goal.position.z)
self.set_marker(goal_pose)
#client.wait_for_result()
client.wait_for_result(rospy.Duration(10))
rospy.loginfo(client.get_state())
#http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
Status = GoalStatus()
if client.get_state() != Status.SUCCEEDED:
client.cancel_goal()
rospy.loginfo('Failed to Move')
else:
rospy.loginfo('Succeed to Move')
return #client.get_state()
def print_status(status):
g_status = GoalStatus()
if status == g_status.PREEMPTED:
print("PREEMPTED")
elif status == g_status.ABORTED:
print("ABORTED")
elif status == g_status.REJECTED:
print("REJECTED")
elif status == g_status.SUCCEEDED:
print("SUCCEEDED")
def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn,
send_cancel_fn):
self.action_goal = action_goal
self.transition_cb = transition_cb
self.feedback_cb = feedback_cb
self.send_goal_fn = send_goal_fn
self.send_cancel_fn = send_cancel_fn
self.state = CommState.WAITING_FOR_GOAL_ACK
self.mutex = threading.RLock()
self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
self.latest_result = None
def __init__(self):
self.feedback = ProjectedGraspingFeedback()
self.result = ProjectedGraspingResult()
self.goal_received = False
self.object_to_grasp = False
self.action_name = 'projected_grasping_server'
self.action_server = actionlib.ActionServer(self.action_name,
ProjectedGraspingAction,
self.action_callback,
self.cancel_cb,
auto_start=False)
self.action_server.start()
self.action_status = GoalStatus()
def navToPose(goal):
print
"Starting Navigation!"
# get path from A*
while not stopDrive:
# find the global path
globalPathServ = getGlobalPath(navBot.cur.pose, goal.pose)
globalPath = globalPathServ.path
if globalPath.poses: # if there's something in the list of poses
print
" Global Navigation"
navBot.rotateTowardPose(
globalPath.poses[0]) # rotate to update local map
localPathServ = getLocalPath(navBot.cur.pose,
globalPath.poses[0].pose)
localPath = localPathServ.path
while not localPath.poses: # if there's nothing in the list of poses
print
" - blocked global waypoint, moving to next"
del globalPath.poses[0]
if not globalPath.poses:
print
" - no valid path, ending navigation"
return
localPathServ = getLocalPath(navBot.cur.pose,
globalPath.poses[0].pose)
localPath = localPathServ.path
print
" Local Navigation"
navBot.goToPose(localPath.poses[0])
else:
print
" - goal not navigable, ending navigation"
break
gridDist = math.sqrt((globalPath.poses[-1].pose.position.x -
navBot.cur.pose.position.x)**2 +
(globalPath.poses[-1].pose.position.y -
navBot.cur.pose.position.y)**2)
if (len(globalPath.poses) == 1 and len(localPath.poses)
== 1) or gridDist <= navBot.distThresh * 3:
print
" Finished All Navigation!"
break
navBot.rotateTo(getAngleFromPose(goal.pose))
statuses = GoalStatusArray()
status = GoalStatus()
status.status = 3
statuses.status_list.append(status)
status_pub.publish(statuses)
def __init__(self, name, anonymous=False):
super(CalibrationMasterNode, self).__init__(name, anonymous=anonymous)
self._replies = {}
self._received = 0
self._status = GoalStatus()
self._result = CalibrationResult()
self._server = actionlib.SimpleActionServer("/calibration/server",
CalibrationAction,
execute_cb=self._process,
auto_start=False)
self._server.register_preempt_callback(self._cancel)
self._server.start()
def move_pose(self, pose_map):
'''
指定されたゴール姿勢に移動する
Parameters
----------
pose_map : geometry_msgs/Pose
map座標系のポーズ
Returns
-------
ret: int
actionlibの実行結果 0:succeed 1:failed
'''
goal = pose_map
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.stamp = rospy.Time.now()
goal_pose.target_pose.header.frame_id = self.map_frame
goal_pose.target_pose.pose = goal
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
# rospy.loginfo('connected to server')
client.send_goal(goal_pose)
rospy.loginfo('Send goal pos : [%5.3f, %5.3f, %5.3f]', goal.position.x,
goal.position.y, goal.position.z)
self.set_marker(goal_pose)
client.wait_for_result(rospy.Duration(10))
# http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
Status = GoalStatus()
while (client.get_state() != Status.SUCCEEDED):
if ((client.get_state() != Status.ACTIVE)
or (client.get_state() != Status.SUCCEEDED)):
client.wait_for_result(rospy.Duration(5)) # timeout時間を設定
if (client.get_state() != Status.ACTIVE):
break
client.wait_for_result(rospy.Duration(1))
if (client.get_state() == Status.SUCCEEDED):
rospy.loginfo('Succeed to Move')
ret = 0
else:
client.cancel_goal()
rospy.loginfo('Failed to Move')
ret = 1
return ret
def __init__(self):
self._feedback = RequestTrajectoryFeedback()
self._result = RequestTrajectoryResult()
self.goal_received = False
self._action_name = 'request_trajectory'
self.action_server = actionlib.ActionServer(self._action_name,
RequestTrajectoryAction,
self.action_callback,
self.cancel_cb,
auto_start=False)
self.action_server.start()
self.action_status = GoalStatus()
# Variable definition
self.grip_left = 'left_gripper_tool_frame'
self.grip_right = 'right_gripper_tool_frame'
self.gripper = self.grip_right
self.frame_base = 'base_link'
self.nJoints = 8 + 3
self.arm = 'left'
self.joint_values = np.empty([self.nJoints])
self.joint_values_kdl = kdl.JntArray(self.nJoints)
self.eef_pose = kdl.Frame()
self.far = False
# self.old_error = [1.0, 1.0, 1.0]
# TODO: find appropriate max acceleration
self.accel_max = np.array(
[0.05, 0.05, 0.1, 0.01, 0.64, 0.64, 0.75, 0.75, 0.75, 1.05, 1.05])
#self.accel_max = np.array([0.3, 0.3, 0.3, 1.02, 1.9, 1.9, 1.95, 1.95, 1.95, 2.05, 2.05])
joint_topic = rospy.get_param('joint_topic')
rospy.Subscriber(joint_topic, JointState, self.joint_callback)
self.pub_clock = rospy.Publisher('/simulator/projection_clock',
ProjectionClock,
queue_size=3)
self.pub_velocity = rospy.Publisher('/simulator/commands',
JointState,
queue_size=3)
self.pub_plot = rospy.Publisher('/data_to_plot',
PoseArray,
queue_size=10)
print 'Ready'
# Wait until goal is received
rospy.Timer(rospy.Duration(0.05), self.generate_trajectory)
def rasterDone(self, status, result):
if status == GoalStatus().SUCCEEDED:
if result.max_concentration > self.max_conc_val:
self.max_conc_val = result.max_concentration
self.max_conc_at.x, self.max_conc_at.y, self.max_conc_at.z = result.max_concentration_point
self.raster_scan_complete = True
if self.initial_scan_complete:
rospy.loginfo(
"Non initial Raster Scan Complete. Following Wind")
self.algorithm = self.FOLLOW_WIND
self.lost_plume_max = 1
else:
rospy.loginfo("Initial Raster scan complete")
self.initial_scan_complete = True
else:
rospy.logerr("Raster Scan not completed. Status = %s" %
status.text)
def action_status_callback(msg):
global action_current_status
global action_current_id
status_list = GoalStatus()
status_list = msg.status_list
# print(str("*** CB --- ACTION --- LENGTH OF A GOAL STATUS LIST IS:" + str(len(status_list))))
if (len(status_list) != 1):
# assuming that there will always be 1 goal status
# print(str("*** CB --- ACTION --- LENGTH OF A GOAL STATUS LIST IS NOT 1 but " + str(len(status_list))))
return
id_goal_str = status_list[0].goal_id.id
# print(str("*** CB --- id_goal_str:" + str(id_goal_str) ))
# tuck_arm action is executed first, robot will wait until its finish
idx = id_goal_str.find("tuck_arm")
if (idx != -1):
# found
action_current_id = "tuck_arm"
action_current_status = status_list[0].status
def updateGripperPosition(self, msg):
print "Commanding", self.arm_name, 'gripper to', str(msg.data)
self.pause_control_loop = True # pause the control loop to free the serial bus
rospy.sleep(1.0)
angles = {self.arm_name+'_joint1':0.0, self.arm_name+'_joint2':0.0,self.arm_name+'_joint3':0.0, \
self.arm_name+'_joint4':0.0,self.arm_name+'_joint5':0.0,self.arm_name+'_joint6':0.0,self.arm_name+'_gripper':0.0}
vels = {self.arm_name+'_joint1':0.0, self.arm_name+'_joint2':0.0,self.arm_name+'_joint3':0.0, \
self.arm_name+'_joint4':0.0,self.arm_name+'_joint5':0.0,self.arm_name+'_joint6':0.0,self.arm_name+'_gripper':0.0}
angles[self.arm_name+"_gripper"] = int(msg.data)
vels[self.arm_name+"_gripper"] = 20.0
msg = GoalStatus()
msg.status = msg.ACTIVE
self.gripper_result_pub.publish(msg)
self.commandAllJointAngles(angles, vels)
msg.status = msg.SUCCEEDED
self.gripper_result_pub.publish(msg)
rospy.sleep(0.1)
msg.status = msg.PENDING
self.gripper_result_pub.publish(msg)
def main():
'''
Description: This function captures data on a given object class and
instance by having the robot randomly pick a goal and then
make a route to that goal to take a picture of the object.
Input: None
Return: None
'''
# Set up command line arguments
parser = argparse.ArgumentParser(description='Fetch Data Capture')
parser.add_argument('-c',
'--class',
dest='class_name',
action='store',
help='object class name: mug, stapler, book, etc...')
parser.add_argument('-n',
'--num',
'--number',
dest='class_number',
action='store',
help='object number in data set: mug # 6, 7, 8, etc..')
args = parser.parse_args()
num_necessary_args = 5
if (len(sys.argv) > num_necessary_args):
print "Need to provide command line arguments, use \"-help\" to see examples."
exit()
# Initialize variables
class_name = args.class_name
instance_name = class_name + '_' + str(args.class_number)
rospy.init_node('data_collector', anonymous=True)
num_published_points = 4
sample_min_radius = .6
sample_max_radius = 1
max_spine_height = .386
min_spine_height = 0.00313
spine_offset = 0.0
# set starting_image_index back to 0 when doing a new object
starting_image_index = 0
desired_num_images = 80
# Relevant paths
results_path = "/home/mccallm/catkin_ws/src/lifelong_object_learning/data_captured/"
base_filepath = results_path + class_name + "/" + instance_name
image_filepath = results_path + class_name + "/" + instance_name + "/images/"
circle_image_filepath = results_path + class_name + "/" + instance_name + "/circle_images/"
image_data_filepath = results_path + class_name + "/" + instance_name + "/metadata/"
# Path dir creation
if not os.path.exists(results_path):
os.makedirs(results_path)
if not os.path.exists(base_filepath):
os.makedirs(base_filepath)
if not os.path.exists(image_filepath):
os.makedirs(image_filepath)
if not os.path.exists(circle_image_filepath):
os.makedirs(circle_image_filepath)
if not os.path.exists(image_data_filepath):
os.makedirs(image_data_filepath)
# ROS paths
image_topic = "/head_camera/rgb/image_rect_color"
camera_info_topic = "/head_camera/rgb/camera_info"
map_frame = "/map"
camera_frame = "/head_camera_rgb_optical_frame"
published_point_num_topic = "/object_point_num"
published_point_base_topic = "/object_point"
torso_movement_topic = "/torso_controller/follow_joint_trajectory"
head_movement_topic = "/head_controller/point_head"
node = Node(image_topic, camera_info_topic, camera_frame,
published_point_num_topic, published_point_base_topic,
torso_movement_topic, head_movement_topic,
num_published_points, max_spine_height, min_spine_height,
spine_offset)
count_pub = rospy.Publisher('data_capture_index', String, queue_size=10)
camera_model = PinholeCameraModel()
while node.camera_info is None: # wait for camera info
continue
camera_model.fromCameraInfo(node.camera_info)
while (len(node.points_registered) != node.num_published_points):
rospy.loginfo(str(len(node.points_registered)))
continue
# find center of object
x_center = 0.0
y_center = 0.0
z_center = 0.0
for i in range(node.num_published_points):
x_center += node.published_points[i][0]
y_center += node.published_points[i][1]
z_center += node.published_points[i][2]
x_center = x_center / node.num_published_points
y_center = y_center / node.num_published_points
z_center = z_center / node.num_published_points
rospy.loginfo("x center: " + str(x_center))
rospy.loginfo("y center: " + str(y_center))
rospy.loginfo("z center: " + str(x_center))
# send first goal
goalID = starting_image_index
num_images_captured = starting_image_index
position = node.sample_position(x_center, y_center, sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal")
node.move_base_to(goal_x, goal_y, goal_theta)
g_status = GoalStatus()
image_file_index = starting_image_index
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if (node.base_client.get_state() == g_status.PREEMPTED) or (
(node.base_client.get_state() == g_status.ABORTED) or
(node.base_client.get_state() == g_status.REJECTED)):
position = node.sample_position(x_center, y_center,
sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
count_pub.publish("New goal ID is " + str(goalID))
rospy.loginfo("New goal ID is " + str(goalID))
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal")
node.move_base_to(goal_x, goal_y, goal_theta)
# when base reaches position, adjust spine and point camera at object
if (node.base_client.get_state() == g_status.SUCCEEDED):
# adjust spine height
spine_height = position[3]
result = node.move_torso(spine_height)
rospy.loginfo("Adjusting spine")
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Spine adjustment succeeded")
# make robot look at object
rospy.sleep(1)
rospy.loginfo("Turning head")
node.look_at("/map", x_center, y_center, z_center)
result = node.point_head_client.wait_for_result()
rospy.loginfo(result)
if result == True:
rospy.loginfo("Head turn succeeded")
rospy.sleep(.1)
# capture and save image
img_cur = node.get_img()
rospy.sleep(.1)
if (img_cur is not None) and (
len(node.points_registered)
== node.num_published_points):
rospy.loginfo("Capturing image")
# find pixel coordinates of points of interest
# tl, tr, bl, br
ref_points = node.published_points
height, width, channels = img_cur.shape
points_to_write = []
for ref_point in ref_points:
ps = PointStamped()
ps.header.frame_id = map_frame
ps.header.stamp = node.tf.getLatestCommonTime(
camera_frame, ps.header.frame_id)
ps.point.x = ref_point[0]
ps.point.y = ref_point[1]
ps.point.z = ref_point[2]
ps_new = node.tf.transformPoint(
camera_frame, ps)
(u, v) = camera_model.project3dToPixel(
(ps_new.point.x, ps_new.point.y,
ps_new.point.z))
points_to_write.append(
[int(round(u)),
int(round(v))])
image_file = image_filepath + instance_name + \
"_" + str(image_file_index) + '.png'
circle_image_file = circle_image_filepath + \
instance_name + "_" + str(image_file_index) + '.png'
text_file = image_data_filepath + instance_name + \
"_" + str(image_file_index) + '.txt'
f = open(text_file, 'w')
f.write(image_file + "\n")
f.write(str(height) + "\t" + str(width) + "\n")
for point in points_to_write:
f.write(str(point[0]) + "\t")
f.write(str(point[1]) + "\n")
f.write(str(goal_x) + "\n")
f.write(str(goal_y) + "\n")
f.write(str(position[3]) + "\n") # spine height
f.close()
circle_img = img_cur.copy()
# visualize
for point in points_to_write:
cv2.circle(circle_img, (point[0], point[1]), 2,
(0, 0, 255), 3)
cv2.imwrite(circle_image_file, circle_img)
cv2.imwrite(image_file, img_cur)
image_file_index += 1
rospy.loginfo("Metadata and Image saved")
rospy.loginfo("Num images captured: " +
str(image_file_index))
# update goal id
goalID += 1
num_images_captured += 1
# Send next position
position = node.sample_position(x_center, y_center,
sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
# exit if we have tried all positions
if num_images_captured == desired_num_images:
rospy.loginfo("Total images captured: " +
str(image_file_index))
return
# move to next position
count_pub.publish("New goal ID is " + str(goalID))
rospy.loginfo("New goal ID is " + str(goalID))
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal...")
node.move_base_to(goal_x, goal_y, goal_theta)
rate.sleep()
def fake_auto_demo():
rospy.init_node('autonomous_demo23')
#Requirements
pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
pub2 = rospy.Publisher('/imu/data', Imu, queue_size=10)
pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size=10)
pub4 = rospy.Publisher('/gps_waypoint_handler/status/gps/fix',
String,
queue_size=10)
pub5 = rospy.Publisher('/pass_gate_topic', String, queue_size=10)
#Autonomous movement
#pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10)
#Image Detect
pub6 = rospy.Publisher('/artag_detect_topic', String, queue_size=10)
#Image Reach
pub7 = rospy.Publisher('/artag_reach_topic', String, queue_size=10)
pub8 = rospy.Publisher('/done_app_topic', String, queue_size=10)
rate = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
print("0: All sensors are good.")
print("1: All sensors except encoder are good.")
print("2: Damaged Sensors")
print("3: Got Waypoint")
print("4: Did not get any waypoint")
print("5: Reached to way point")
print("6: Did not Reached to way point")
print("7: Detected AR Tag")
print("8: Did not detect AR Tag")
print("9: Reached AR Tag")
print("10: Did not reached AR Tag")
imuMsg = Imu()
imuMsg.orientation.x = 5
imuMsg.orientation.y = 5
gpsMsg = NavSatFix()
gpsMsg.latitude = 5
gpsMsg.longitude = 5
encoderMsg = Odometry()
encoderMsg.pose.pose.position.x = 5
encoderMsg.pose.pose.position.y = 5
wpStatusMsgLow = GoalStatus()
wpStatusMsgLow.status = 3
wpStatusArray = []
wpStatusArray.append(wpStatusMsgLow)
wpStatusMsg = GoalStatusArray()
wpStatusMsg.status_list = wpStatusArray
userInput = raw_input()
if userInput == "0": #All sensors are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
pub3.publish(encoderMsg)
elif userInput == "1": # All sensors except encoder are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
#pub3.publish("0")
elif userInput == "2": #Damaged Sensors
pub1.publish(gpsMsg)
#pub2.publish("0")
pub3.publish(encoderMsg)
elif userInput == "3": # Got Waypoint
pub4.publish("1")
elif userInput == "4": #Did not get any waypoint
pub4.publish("0")
elif userInput == "5": #Reached to way point
#pub5.publish(wpStatusMsg)
pub4.publish("2")
#elif userInput == "6": #Did not reached to way point
#pub5.publish("0")
elif userInput == "7": #Detected Image
pub6.publish("1")
#pub5.publish("1")
elif userInput == "8": #Did not Detect Image
pub6.publish("0")
elif userInput == "9": #Reached Image
pub7.publish("1")
elif userInput == "10": #Did not reached image
pub7.publish("0")
elif userInput == "11":
pub5.publish("1")
elif userInput == "12":
pub8.publish("1")
else:
print("Invalid entry")
rate.sleep()
