def output_decision(self):
if not self.on:
if self.bag_open:
self.bag.close()
self.bag_open = False
return
else:
if not self.bag_open:
filename = datetime.datetime.today().strftime("%Y%m%d_%H%M%S") + '.bag'
rosparam.set_param("/current_bag_file", filename)
self.bag = rosbag.Bag(filename, 'w')
self.bag_open = True
s = self.sensor_values
a = self.cmd_vel
e = Event()
lf = int((-math.pi*30.0/180 - s.angle_min)/s.angle_increment);
rf = int((math.pi*30.0/180 - s.angle_min)/s.angle_increment);
ls = int((-math.pi*90.0/180 - s.angle_min)/s.angle_increment);
rs = int((math.pi*90.0/180 - s.angle_min)/s.angle_increment);
e.left_side = 5000.0 if math.isnan(s.ranges[ls]) else s.ranges[ls]*1000;
e.right_side = 5000.0 if math.isnan(s.ranges[rs]) else s.ranges[rs]*1000;
e.left_forward = 5000.0 if math.isnan(s.ranges[lf]) else s.ranges[lf]*1000;
e.right_forward = 5000.0 if math.isnan(s.ranges[rf]) else s.ranges[rf]*1000;
e.linear_x = a.linear.x
e.angular_z = a.angular.z
print(e)
self._decision.publish(e)
self.bag.write('/event', e)
def insert_goal(self, pos_x, pos_y, pos_z):
# 插入一个新点
# target_points在ORB_SLAM/World坐标系下
q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
q = Quaternion(*q_angle)
waypoint = Pose(Point(pos_x, pos_y, pos_z), q)
waypoint_stamped = PoseStamped()
waypoint_stamped.header.frame_id = "map"
waypoint_stamped.header.stamp = rospy.Time(0)
waypoint_stamped.pose = waypoint
self.waypoints.append(waypoint_stamped)
# 转至ORB_SLAM/World坐标系
rospy.loginfo("获取TF map->ORB_SLAM/World")
tf_flag = False
while not tf_flag and not rospy.is_shutdown():
try:
t = rospy.Time(0)
self.listener.waitForTransform("map", "ORB_SLAM/World", t,
rospy.Duration(1.0))
tf_flag = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as e:
tf_flag = False
rospy.logwarn("获取TF失败 map->ORB_SLAM/World")
rospy.logwarn(e)
rospy.loginfo("获取TF 成功 map->ORB_SLAM/World")
target_point = self.listener.transformPose(
"ORB_SLAM/World", waypoint_stamped)
target_point.header.stamp = rospy.Time.now()
self.target_points.append(target_point)
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def set_depth(self):
data = []
data.append(self.s_d_P.get() * 10**float(self.combo_d_P.get()))
data.append(self.s_d_I.get() * 10**float(self.combo_d_I.get()))
data.append(self.s_d_D.get() * 10**float(self.combo_d_D.get()))
print data
rosparam.set_param('/PIDpara/depth', str(data))
def output_decision(self):
if not self.on:
if self.bag_open:
self.bag.close()
self.bag_open = False
return
else:
if not self.bag_open:
filename = datetime.datetime.today().strftime(
"%Y%m%d_%H%M%S") + '.bag'
rosparam.set_param("/current_bag_file", filename)
self.bag = rosbag.Bag(filename, 'w')
self.bag_open = True
s = self.sensor_values
a = self.cmd_vel
e = Event()
e.left_side = s.left_side
e.right_side = s.right_side
e.left_forward = s.left_forward
e.right_forward = s.right_forward
e.linear_x = a.linear.x
e.angular_z = a.angular.z
self._decision.publish(e)
self.bag.write('/event', e)
def add_ok_event(self):
# Get name for this marker and add to set
marker_name = self._widget.fiducial_name_edit.text()
frame = self.current_unknown_marker
fid = self.id_from_frame(frame)
self.current_unknown_marker = None
rospy.loginfo("Adding marker #{} [{}]".format(fid, frame))
# Add to "seen" list
self.markers_seen += [fid]
self.marker_frames[frame] = marker_name
# Update visualized marker
self.recolor_marker(fid, [0.0, 1.0, 0.0])
# Add marker to rosparam
rosparam_marker_name = "instructor_landmark/{}".format(marker_name)
rosparam.set_param(rosparam_marker_name, frame)
# GUI: Append to the list box
list_txt = "{} ({})".format(marker_name, frame)
self._widget.fiducial_list.addItem(list_txt)
# GUI: Reset text field
self._widget.fiducial_name_edit.setText("")
self._widget.found_fiducial_id_label_2.setText(
"Found fiducial with ID: NULL")
# GUI Remove the "edit name" section of the gui
self._widget.fiducial_found_widget.hide()
self._widget.add_fiducial_widget.hide()
def img_callback(self, data):
img = self.bridge.imgmsg_to_cv2(data)
cv2.namedWindow("img", cv2.WINDOW_NORMAL)
self.draw_objects(img)
cv2.imshow('img', img)
if cv2.waitKey(1) & 0xFF == 32:
rosparam.set_param("no_man", True)
def execute(self, userdata):
try:
rospy.loginfo('Executing state NAVIGATION')
ap_result = userdata.result_message
coord_list = userdata.coord_in
m6Control(0.3)
rosparam.set_param('/move_base/DWAPlannerROS/xy_goal_tolerance', str(0.5))
print rosparam.get_param('/move_base/DWAPlannerROS/xy_goal_tolerance')
rospy.sleep(0.1)
result = navigationAC(coord_list)
print result
if self.result == True:
m6Control(0.4)
speak('I came close to person')
#ap_result = result
#userdata.result_message.data = ap_result
userdata.result_message.data = self.result
return 'arrive'
else:
speak('I can`t came close to person')
#ap_result = result
#userdata.result_message.data = ap_result
userdata.result_message.data = self.result
return 'not_arrive'
except rospy.ROSInterruptException:
rospy.loginfo('**Interrupted**')
pass
def set_altitude(self):
data = []
data.append(self.s_a_P.get() * 10**float(self.combo_a_P.get()))
data.append(self.s_a_I.get() * 10**float(self.combo_a_I.get()))
data.append(self.s_a_D.get() * 10**float(self.combo_a_D.get()))
print data
rosparam.set_param('/PIDpara/altitude', str(data))
def insert_goal(self, pos_x, pos_y, pos_z):
# 插入一个新点
# target_points在ORB_SLAM/World坐标系下
q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
q = Quaternion(*q_angle)
waypoint = Pose(Point(pos_x, pos_y, pos_z), q)
waypoint_stamped = PoseStamped()
waypoint_stamped.header.frame_id = "map"
waypoint_stamped.header.stamp = rospy.Time(0)
waypoint_stamped.pose = waypoint
self.waypoints.append(waypoint_stamped)
# 转至ORB_SLAM/World坐标系
rospy.loginfo("获取TF map->ORB_SLAM/World")
tf_flag = False
while not tf_flag and not rospy.is_shutdown():
try:
t = rospy.Time(0)
self.listener.waitForTransform("map", "ORB_SLAM/World", t,
rospy.Duration(1.0))
tf_flag = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as e:
tf_flag = False
rospy.logwarn("获取TF失败 map->ORB_SLAM/World")
rospy.logwarn(e)
rospy.loginfo("获取TF 成功 map->ORB_SLAM/World")
target_point = self.listener.transformPose("ORB_SLAM/World",
waypoint_stamped)
target_point.header.stamp = rospy.Time.now()
self.target_points.append(target_point)
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def add_ok_event(self):
# Get name for this marker and add to set
marker_name = self._widget.fiducial_name_edit.text()
frame = self.current_unknown_marker
fid = self.id_from_frame(frame)
self.current_unknown_marker = None
rospy.loginfo("Adding marker #{} [{}]".format(fid, frame))
# Add to "seen" list
self.markers_seen += [fid]
self.marker_frames[frame] = marker_name
# Update visualized marker
self.recolor_marker(fid, [0.0, 1.0, 0.0])
# Add marker to rosparam
rosparam_marker_name = "instructor_landmark/{}".format(marker_name)
rosparam.set_param(rosparam_marker_name, frame)
# GUI: Append to the list box
list_txt = "{} ({})".format(marker_name, frame)
self._widget.fiducial_list.addItem(list_txt)
# GUI: Reset text field
self._widget.fiducial_name_edit.setText("")
self._widget.found_fiducial_id_label_2.setText("Found fiducial with ID: NULL")
# GUI Remove the "edit name" section of the gui
self._widget.fiducial_found_widget.hide()
self._widget.add_fiducial_widget.hide()
def tidy_up(self):
"""
Run when this flight state is finished
Overides pyx4 'tidy_up' method
"""
rospy.loginfo('tidying up torf_ros.py')
self.cwssim.tidy_up()
rospy.logwarn('tidy up torf_ros.py completed')
rosparam.set_param('cwssim_test_complete', "true")
def set_yaw(self):
data = []
data.append(self.s_y.get() * 10**float(self.combo_y.get()))
data.append(self.s_y_P.get() * 10**float(self.combo_a_P.get()))
data.append(self.s_y_I.get() * 10**float(self.combo_a_I.get()))
data.append(self.s_y_D.get() * 10**float(self.combo_a_D.get()))
print data
rosparam.set_param('/PIDpara/yaw', str(data))
def callback_param_client(self, command, result):
""" Callback When a Parameter is Fetched from Client and Needs to Be Set on Server
Args:
command: sent command of Type Command -- see Command Class
result: returned argument from client
"""
if result is not None:
result = json.loads(result)
param_value = result["values"]["value"]
rosparam.set_param(command.wrapper.name, param_value)
def execute(self,userdata):
#パラメータ初期化
self.enemy_pos_from_lider={"enemy_pos":Point(),"is_topic_receive":False}
#移動時の向きを逃走用(後ろ向き)に設定
rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "4")
#逃げる位置計算
enemy_pos=userdata.enemy_pos_in
print("enemy_pos",enemy_pos)
#escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v1(enemy_pos.x,enemy_pos.y)#中心挟んで相手の反対地点に逃げるパターン
#escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v2(enemy_pos.x,enemy_pos.y)#相手の位置によって反対側の決まった地点に逃げるパターン
#escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v3(enemy_pos.x,enemy_pos.y)#相手の位置によって反対側の決まった地点に逃げるパターン
escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v4(enemy_pos.x,enemy_pos.y,self.my_pose.pos.x,self.my_pose.pos.y)#相手の位置によって反対側の決まった地点に逃げるパターン
#print(escape_pos_x,escape_pos_y,escape_yaw)
#ゴール設定
my_move_base.setGoal(self.move_base_client,escape_pos_x,escape_pos_y,escape_yaw)
# rospy終了か、ゴールに着いたらループ抜ける。
self.is_stop_receive=False
r = rospy.Rate(5)
while (not rospy.is_shutdown()) and \
self.move_base_client.get_state() in [ actionlib_msgs.msg.GoalStatus.ACTIVE,\
actionlib_msgs.msg.GoalStatus.PENDING]:
#逃げる途中で,ストップトピックを受け取った場合
if self.is_stop_receive == True:
#ループ抜ける
#目的地設定キャンセル
self.move_base_client.cancel_goal()
#移動停止
for _ in range(5):
self.vel_pub.publish(Twist())
r.sleep()
self.is_stop_receive=False
rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "1")#(None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)
return 'is_receiveStopSig'
#逃げる途中で、敵と遭遇した場合
if self.enemy_pos_from_lider["is_topic_receive"]==True:
#前回の敵位置との距離を計算
dist=self.get_distance( enemy_pos.x, enemy_pos.y,\
self.enemy_pos_from_lider["enemy_pos"].x,self.enemy_pos_from_lider["enemy_pos"].y)
# print("dist",dist)
if dist >= self.CHANGE_ESCAPE_POS_TH: #しきい値を上回っていた場合
userdata.enemy_pos_out=self.enemy_pos_from_lider["enemy_pos"]
self.move_base_client.cancel_goal()
rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "1")#(None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)
return 'is_NearEnemyFound' #抜ける。再計算されて、目的地が変更される。
#TODO:ゴールが壁の中になった時の対応
r.sleep()
#目的地についた場合
rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "1")#(None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)
return 'is_Gone'
def callback(data):
cv_msgs = msg_list_to_cv(data.camera_msg)
aruco_detector.set_data(cv_msgs)
aruco_detector.detect_markers(aruco_dict, aruco_parametrs)
single_markers, duplicates = aruco_detector.find_duplicates()
aruco_detector.estimate_pose_single_markers_list(single_markers, dx=dx, dy=dy)
aruco_detector.estimate_pose_duplicates(duplicates, dx=dx, dy=dy)
aruco_detector.create_pairs_of_markers()
distance_between_markers_in_pair = aruco_detector.get_mean_markers_pair_distance()
if distance_between_markers_in_pair:
rosparam.set_param("pix_between_markers_pair_centers", str(distance_between_markers_in_pair))
markers_list = aruco_detector.pairs_of_markers
msg = prepare_msg(markers_list, duplicates.keys())
aruco_publisher.publish(msg)
def _launch_node(self):
package = 'minisim'
executable = 'minisim_srv' if not self.randomize_maps else 'minisim_srv_standalone'
node = roslaunch.core.Node(package,
executable,
required=True,
name=self.sim_name,
namespace=self.sim_name,
output='screen')
if self.randomize_maps:
rosparam.set_param("/{0}/{0}/map_dir".format(self.sim_name),
MinisimEnv.map_dir)
return MinisimEnv.roslaunch_node_starter.launch(node)
def __init__(self):
rospy.loginfo("start init")
self.loop_rate = rospy.Rate(1)
self.h_idx=np.zeros(1)
self.publisher = rospy.Publisher('hypothesis', String, queue_size=1)
self.TRAIN_DATA_FILES = ['dead_end', 'left', 'right', 'straight', 'threeway_left', 'threeway_center', 'threeway_right']
NUM_CLASSES = len(self.TRAIN_DATA_FILES)
self.MAX_LASER_DISTANCE = 30.0
rosparam.set_param("model_full_path", "/home/docker/catkin_ws/src/intersection_ws/model/NN.h5")
model_full_path = rosparam.get_param("tensorflow/model_full_path")
self.model = keras.models.load_model(model_full_path)
rospy.loginfo("finish init")
def start_ros(): # pragma: no cover
"""
Starts ROS utilities and cleanup thread in the app process.
"""
root_logger.info("Starting ROS node")
# Block until connection to ROS master is established, and initialize a backend node
rospy.init_node("nrp_backend")
rosparam.set_param("/use_sim_time", "true")
rospy_thread = Thread(target=rospy.spin)
rospy_thread.setDaemon(True)
rospy_thread.start()
root_logger.info("Starting cleanup thread")
cleanup_thread = Thread(target=clean_simulations)
cleanup_thread.setDaemon(True)
cleanup_thread.start()
def table_coeffs_cb(data):
openni_frame = data.header.frame_id
ps = PointStamped()
ps.header.frame_id = openni_frame
#print data.values
h_openni = -data.values[3]/data.values[2]
ps.point.z = h_openni
try:
listener.waitForTransform('base_footprint', openni_frame, rospy.Time.now(), rospy.Duration(4.0))
#(trans, rot) = listener.lookupTransform('/base_link', openni_frame, rospy.Time())
ps = listener.transformPoint('base_footprint', ps)
except:
return
if math.isnan(ps.point.z) or np.isnan(ps.point.z):
return
h_floor = ps.point.z + OFFSET
print 'table height: ', h_floor
rosparam.set_param('/table_height', str(h_floor))
call(['rosnode', 'kill', 'table_detect_node'])
rospy.signal_shutdown('table height set')
def table_coeffs_cb(data):
openni_frame = data.header.frame_id
ps = PointStamped()
ps.header.frame_id = openni_frame
#print data.values
h_openni = -data.values[3] / data.values[2]
ps.point.z = h_openni
try:
listener.waitForTransform('base_footprint', openni_frame,
rospy.Time.now(), rospy.Duration(4.0))
#(trans, rot) = listener.lookupTransform('/base_link', openni_frame, rospy.Time())
ps = listener.transformPoint('base_footprint', ps)
except:
return
if math.isnan(ps.point.z) or np.isnan(ps.point.z):
return
h_floor = ps.point.z + OFFSET
print 'table height: ', h_floor
rosparam.set_param('/table_height', str(h_floor))
call(['rosnode', 'kill', 'table_detect_node'])
rospy.signal_shutdown('table height set')
def __init__(self, name, filters=OrderedDict()):
self.name = name
self.node = Node("ros_vision", "filter_chain_node.py", name)
self.reset_params()
self.node.run()
create_filter_srv_name = '/%s/create_filter' % name
rospy.wait_for_service(create_filter_srv_name)
self.create_filter = rospy.ServiceProxy(create_filter_srv_name, CreateFilter)
i = 0
for filter_name in filters.keys():
i += 1
if 'type' in filters[filter_name].keys():
filter_type = filters[filter_name]['type']
del filters[filter_name]['type']
for parameter_name in filters[filter_name].keys():
rosparam.set_param('/%s/%s/%s' % (name, filter_name, parameter_name), str(filters[filter_name][parameter_name]))
self.create_filter(filter_name, filter_type, i)
def logger(self):
if not self.mode:
if self.bagfile_open:
self.bag.close()
self.bagfile_open = False
return
else:
print("教示中・・・")
if not self.bagfile_open:
name = "PFoE_" + datetime.datetime.today().strftime(
"%Y_%m_%d_%H_%M_%S") + ".bag"
rosparam.set_param("/bagfile", name)
self.bag = rosbag.Bag(name, 'w')
self.bagfile_open = True
self.e.left_forward = self.sensor_values.left_forward
self.e.left_side = self.sensor_values.left_side
self.e.right_side = self.sensor_values.right_side
self.e.right_forward = self.sensor_values.right_forward
self.e.linear_x = self.cmd_vel.linear.x
self.e.angular_z = self.cmd_vel.angular.z
self.event_pub.publish(self.e)
self.bag.write('/event', self.e)
def main():
global unload_controller_service,load_controller_service,switch_controller_service,shutdown_timeout
args = parse_args(rospy.myargv()[1:])
wait_for_topic = args.wait_for
autostart = 1 if not args.stopped else 0
robot_namespace = args.namespace or ""
timeout = args.timeout
shutdown_timeout = args.shutdown_timeout
rospy.init_node('spawner', anonymous=True)
# add a '/' to the namespace if needed
if robot_namespace and robot_namespace[-1] != '/':
robot_namespace = robot_namespace+'/'
# set service names based on namespace
load_controller_service = robot_namespace+"controller_manager/load_controller"
unload_controller_service = robot_namespace+"controller_manager/unload_controller"
switch_controller_service = robot_namespace+"controller_manager/switch_controller"
try:
# loader
rospy.loginfo("Controller Spawner: Waiting for service "+load_controller_service)
rospy.wait_for_service(load_controller_service, timeout=timeout)
load_controller = rospy.ServiceProxy(load_controller_service, LoadController)
# switcher
rospy.loginfo("Controller Spawner: Waiting for service "+switch_controller_service)
rospy.wait_for_service(switch_controller_service, timeout=timeout)
switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController)
# unloader
# NOTE: We check for the unloader's existence here, although its used on shutdown because shutdown
# should be fast. Further, we're interested in knowing if we have a compliant controller manager from
# early on
rospy.loginfo("Controller Spawner: Waiting for service "+unload_controller_service)
rospy.wait_for_service(unload_controller_service, timeout=timeout)
except rospy.exceptions.ROSException:
rospy.logwarn("Controller Spawner couldn't find the expected controller_manager ROS interface.")
return
if wait_for_topic:
# This has to be a list since Python has a peculiar mechanism to determine
# whether a variable is local to a function or not:
# if the variable is assigned in the body of the function, then it is
# assumed to be local. Modifying a mutable object (like a list)
# works around this debatable "design choice".
wait_for_topic_result = [None]
def wait_for_topic_cb(msg):
wait_for_topic_result[0] = msg
rospy.logdebug("Heard from wait-for topic: %s" % str(msg.data))
rospy.Subscriber(wait_for_topic, Bool, wait_for_topic_cb)
started_waiting = time.time()
# We might not have received any time messages yet
warned_about_not_hearing_anything = False
while not wait_for_topic_result[0]:
time.sleep(0.01)
if rospy.is_shutdown():
return
if not warned_about_not_hearing_anything:
if time.time() - started_waiting > timeout:
warned_about_not_hearing_anything = True
rospy.logwarn("Controller Spawner hasn't heard anything from its \"wait for\" topic (%s)" % \
wait_for_topic)
while not wait_for_topic_result[0].data:
time.sleep(0.01)
if rospy.is_shutdown():
return
# find yaml files to load
controllers = []
for name in args.controllers:
if os.path.exists(name):
# load yaml file onto the parameter server, using the namespace specified in the yaml file
rosparam.set_param("",open(name))
# list the controllers to be loaded
name_yaml = yaml.load(open(name))
for controller in name_yaml:
controllers.append(controller)
else:
controllers.append(name)
# load controllers
for name in controllers:
rospy.loginfo("Loading controller: "+name)
resp = load_controller(name)
if resp.ok != 0:
loaded.append(name)
else:
time.sleep(1) # give error message a chance to get out
rospy.logerr("Failed to load %s" % name)
rospy.loginfo("Controller Spawner: Loaded controllers: %s" % ', '.join(loaded))
if rospy.is_shutdown():
return
# start controllers is requested
if autostart:
resp = switch_controller(loaded, [], 2)
if resp.ok != 0:
rospy.loginfo("Started controllers: %s" % ', '.join(loaded))
else:
rospy.logerr("Failed to start controllers: %s" % ', '.join(loaded))
rospy.spin()
def main():
global unload_controller_service, load_controller_service, switch_controller_service
args = parse_args(rospy.myargv()[1:])
wait_for_topic = args.wait_for
robot_namespace = args.namespace or ""
timeout = args.timeout
rospy.init_node('spawner', anonymous=True)
start_controllers = rospy.get_param("~start_controllers", [])
stop_controllers = rospy.get_param("~stop_controllers", [])
if start_controllers:
rospy.loginfo("Controller spawner: Starting controllers -- " +
str(start_controllers))
if stop_controllers:
rospy.loginfo("Controller spawner: Stopping controllers -- " +
str(stop_controllers))
if args.shutdown_timeout is not None:
rospy.logwarn("DEPRECATION warning: --shutdown-timeout has no effect.")
# add a '/' to the namespace if needed
if robot_namespace and robot_namespace[-1] != '/':
robot_namespace = robot_namespace + '/'
# set service names based on namespace
load_controller_service = robot_namespace + "controller_manager/load_controller"
unload_controller_service = robot_namespace + "controller_manager/unload_controller"
switch_controller_service = robot_namespace + "controller_manager/switch_controller"
try:
# loader
rospy.loginfo("Controller Spawner: Waiting for service " +
load_controller_service)
rospy.wait_for_service(load_controller_service, timeout=timeout)
load_controller = rospy.ServiceProxy(load_controller_service,
LoadController)
# switcher
rospy.loginfo("Controller Spawner: Waiting for service " +
switch_controller_service)
rospy.wait_for_service(switch_controller_service, timeout=timeout)
switch_controller = rospy.ServiceProxy(switch_controller_service,
SwitchController)
# unloader
# NOTE: We check for the unloader's existence here, although its used on shutdown because shutdown
# should be fast. Further, we're interested in knowing if we have a compliant controller manager from
# early on
rospy.loginfo("Controller Spawner: Waiting for service " +
unload_controller_service)
rospy.wait_for_service(unload_controller_service, timeout=timeout)
except rospy.exceptions.ROSException:
rospy.logwarn(
"Controller Spawner couldn't find the expected controller_manager ROS interface."
)
return
if wait_for_topic:
# This has to be a list since Python has a peculiar mechanism to determine
# whether a variable is local to a function or not:
# if the variable is assigned in the body of the function, then it is
# assumed to be local. Modifying a mutable object (like a list)
# works around this debatable "design choice".
wait_for_topic_result = [None]
def wait_for_topic_cb(msg):
wait_for_topic_result[0] = msg
rospy.logdebug("Heard from wait-for topic: %s" % str(msg.data))
rospy.Subscriber(wait_for_topic, Bool, wait_for_topic_cb)
started_waiting = time.time()
# We might not have received any time messages yet
warned_about_not_hearing_anything = False
while not wait_for_topic_result[0]:
time.sleep(0.01)
if rospy.is_shutdown():
return
if not warned_about_not_hearing_anything:
if time.time() - started_waiting > timeout:
warned_about_not_hearing_anything = True
rospy.logwarn("Controller Spawner hasn't heard anything from its \"wait for\" topic (%s)" % \
wait_for_topic)
while not wait_for_topic_result[0].data:
time.sleep(0.01)
if rospy.is_shutdown():
return
# hook for unloading controllers on shutdown
rospy.on_shutdown(shutdown)
# find yaml files to load
controllers = []
for name in start_controllers + stop_controllers:
if os.path.exists(name):
# load yaml file onto the parameter server, using the namespace specified in the yaml file
rosparam.set_param("", open(name))
# list the controllers to be loaded
name_yaml = yaml.load(open(name))
for controller in name_yaml:
controllers.append(controller)
else:
controllers.append(name)
# load controllers
for name in controllers:
rospy.loginfo("Loading controller: " + name)
resp = load_controller(name)
if resp.ok != 0:
loaded.append(name)
else:
time.sleep(1) # give error message a chance to get out
rospy.logerr("Failed to load %s" % name)
rospy.loginfo("Controller Spawner: Loaded controllers: %s" %
', '.join(loaded))
if rospy.is_shutdown():
return
# start controllers is requested
start_controllers = [l for l in loaded if l in start_controllers]
resp = switch_controller(start_controllers, [], 2)
if resp.ok != 0:
rospy.loginfo("Started controllers: %s" % ', '.join(start_controllers))
else:
rospy.logerr("Failed to start controllers: %s" %
', '.join(start_controllers))
rospy.spin()
"""
ros_param.py
python script that will load ROS Parameters
from a YAML file, parse them and will upload
them to ROS 'Parametr Server'.
"""
import rospy
import rosparam
import yaml
with open("validation_params.yaml", 'r') as stream:
data = yaml.load(stream)
while len(data) > 0:
parameter = data.popitem()
rosparam.set_param(parameter[0], str(parameter[1]))
def set_yaw(self):
data = self.s_y.get() * 10**float(self.combo_y.get())
print data
rosparam.set_param('/tune/yaw', str(data))
def torf_logic(self):
"""
Main logic for torf model used in conjunction with the CWSSIM view matching pipeline.
Handles CWSSIM image processing depending on the flight state
"""
while not rospy.is_shutdown() and self.node_alive:
# Get flight state to see what phase of the mission we are in
this_state_label = self.commander._flight_instruction.state_label
# if we are in the outbound/learning flight phase then add new images to our memory database
if this_state_label == 'Head_out':
self.cwssim_state = CWSSIM_STATE.OUTBOUND
if not self.image_q.empty():
rospy.logdebug('adding image')
self.cwssim.add_image(self.image_q.get())
self.publish_cwssim_status()
# check that we are not lagging behind the image processing task
if self.image_q.qsize() > 5:
rospy.logwarn(
'getting behind on image processing - queue size is: {}'.format(self.image_q.qsize()))
# if we are in the inbound/homing flight phase then interrogate new images for familiarity against the
# images in our memory database
elif this_state_label == 'Sweep_state':
self.cwssim_state = CWSSIM_STATE.INBOUND
if self.new_image_evt:
self.new_image_evt = False
if self.use_multiprocessing:
if self.multiprocessing_status == MULTIPROCESSING_STATUS.INITIALISED:
self.cwssim_score, self.cwssim_score_idx = \
self.cwssim.max_query_image_with_index_mp(self.this_image)
else:
rospy.logwarn_throttle(1, "memory bank not initialised yet - can't do multiprocessing")
if self.multiprocessing_status == MULTIPROCESSING_STATUS.NOT_INIT:
rospy.logwarn_throttle(1, "trying to initialised memory bank but this should have "
"already been done - there is an error in the code")
self.initialise_memory_bank_for_multiprocessing()
else:
rospy.logwarn_throttle(5, 'not using multiprocessing, fewer images possible')
self.cwssim_score, self.cwssim_score_idx = \
self.cwssim.max_query_image_with_index_mp(self.this_image)
self.publish_cwssim_status()
elif (this_state_label == 'Turn around time') or (this_state_label == 'Return to start'):
self.cwssim_state = CWSSIM_STATE.TURNING_AROUND
self.new_image_evt = False # clear the last image
if self.multiprocessing_status == MULTIPROCESSING_STATUS.NOT_INIT:
if self.need2reverse_image_idxs:
rospy.logwarn("flip images before preparing memory bank for multiprocessing")
else:
rospy.loginfo("preparing memory bank for multiprocessing")
self.initialise_memory_bank_for_multiprocessing()
else:
self.cwssim_state = CWSSIM_STATE.INTERUPTION
# if we n
if self.need2reverse_image_idxs and \
(this_state_label=='Turn around time' or this_state_label=='Return to start'):
rospy.logwarn('started reversing CWSSIM image ids')
self.cwssim.reverse_image_ids()
self.need2reverse_image_idxs = False
rospy.logwarn('Completed reversing CWSSIM image ids')
try:
self.ros_rate.sleep()
except:
# prevent gabble being printed to the terminal
break
# do not do tidy up - if pyx4 also calls this it seems to cause problems
# self.tidy_up()
rospy.logwarn('setting mission complete parameter to true')
rosparam.set_param('cwssim_test_complete', "true")
def __set_param(self, name, value):
rosparam.set_param(name, str(value))
def register_dnsmasq(self, IP):
self.dnsmasqIP = IP
rosparam.set_param("{}/dnsmasqIP".format(self.master_handle), IP)
rospy.loginfo("Registered dnsmasq server at {}".format(IP))
def map_mux():
rospy.init_node("map_mux", anonymous=True)
# Topic Subscriber
rospy.Subscriber("map1", OccupancyGrid, addMap1)
rospy.Subscriber("map2", OccupancyGrid, addMap2)
rospy.Subscriber("map3", OccupancyGrid, addMap3)
# Topic Publisher
topic = rospy.resolve_name("map")
pub = rospy.Publisher("map", OccupancyGrid)
topic = rospy.resolve_name("map_metadata")
pub_metadata = rospy.Publisher("map_metadata", MapMetaData)
initialPosePub = rospy.Publisher("initialpose", PoseWithCovarianceStamped)
global change_map
global flag
# Service Servers
s = rospy.Service('change_map', ChangeMap, changeMapfunc)
s = rospy.Service('static_map', GetMap, staticMapfunc)
# Service Clients
service_floor_switch = rospy.ServiceProxy("floor_switch", ChangeMap)
rp = rospkg.RosPack()
try:
path = rp.get_path("map_mux") + "/src"
except rospkg.ResourceNotFound:
print "package not found"
print path
print "waiting for floor_switch to come up"
#rospy.wait_for_service("floor_switch")
r = rospy.Rate(.1)
old_change_map = 0
while not rospy.is_shutdown():
if( map1 == None and map2 == None and map3 == None):
print "you need to provide 3 maps on topics map1 map2 map3 from a launch file."
#if (change_map != old_change_map):
if (1):
if (change_map == 1 and map1 != None):
rospy.loginfo("changing to 1")
pub.publish(map1)
pub_metadata.publish(map1.info)
if (change_map == 2 and map2 != None and flag ==1 ):
rospy.loginfo("changing to 2")
pub.publish(map2)
pub_metadata.publish(map2.info)
#print str(type(initalPositionFloor2))
try:
initialPosePub.publish(initalPositionFloor2)
rosparam.set_param("/segbot_logical_navigator/door_file",
rp.get_path("bwi_kr") + "/config/multi_map/atrium_doors.yaml")
rosparam.set_param("/segbot_logical_navigator/location_file",
rp.get_path("bwi_kr") + "/config/multi_map/atrium_locations.yaml")
rosparam.set_param("/segbot_logical_navigator/object_file",
rp.get_path("bwi_kr") + "/config/multi_map/atrium_objects.yaml")
rosparam.set_param("/segbot_logical_navigator/map_file",
rp.get_path("map_mux") + "/maps/atrium_with_elevators.yaml")
resp_floor_switch = service_floor_switch(change_map)
except rospy.ServiceException:
print "floor switch service call failed"
print "done with floor 2"
flag = 0
if (change_map == 3 and map3 != None and flag ==1 ):
rospy.loginfo("changing to 3")
pub.publish(map3)
pub_metadata.publish(map3.info)
try:
initialPosePub.publish(initalPositionFloor3)
rosparam.set_param("/segbot_logical_navigator/door_file",
rp.get_path("bwi_kr") + "/config/multi_map/3ne_doors.yaml")
rosparam.set_param("/segbot_logical_navigator/location_file",
rp.get_path("bwi_kr") + "/config/multi_map/3ne_locations.yaml")
rosparam.set_param("/segbot_logical_navigator/object_file",
rp.get_path("bwi_kr") + "/config/multi_map/3ne_objects.yaml")
rosparam.set_param("/segbot_logical_navigator/map_file",
rp.get_path("map_mux") + "/maps/map_whole2_with_elevators.yaml")
resp_floor_switch = service_floor_switch(change_map)
except rospy.ServiceException:
print "floor switch service call failed"
print "done with floor 3"
flag = 0
old_change_map = change_map
def load_targets_task(self):
if not os.path.exists(self.nav_points_file):
self.target_points = []
self.waypoints = list()
with open(self.nav_points_file, "r") as nav_data_file:
nav_data_str = nav_data_file.readline()
self.target_points = []
while len(nav_data_str) != 0:
pos_x = float(nav_data_str.split(" ")[0])
pos_y = float(nav_data_str.split(" ")[1])
pos_z = float(nav_data_str.split(" ")[2])
self.target_points.append([pos_x, pos_y, pos_z])
nav_data_str = nav_data_file.readline()
self.waypoints = list()
# nav_path_points = []
# with open("/home/xiaoqiang/slamdb/path.csv", "r") as nav_data_file:
# nav_data_str = nav_data_file.readline()
# while len(nav_data_str) != 0:
# pos_x = float(nav_data_str.split(" ")[0])
# pos_y = float(nav_data_str.split(" ")[1])
# pos_z = float(nav_data_str.split(" ")[2])
# nav_path_points.append([pos_x, pos_y, pos_z])
# nav_data_str = nav_data_file.readline()
# nav_path_points_2d = [[point[0], point[2]]
# for point in nav_path_points]
for point in self.target_points:
pose_in_world = PoseStamped()
pose_in_world.header.frame_id = "ORB_SLAM/World"
pose_in_world.header.stamp = rospy.Time(0)
pose_in_world.pose.position = Point(point[0], point[1], point[2])
# axis = self.get_target_direction(
# [point[0], point[2]], nav_path_points_2d)
# q_angle = quaternion_from_euler(
# math.pi / 2, -math.atan2(axis[1], axis[0]), 0, axes='sxyz')
q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
pose_in_world.pose.orientation = Quaternion(*q_angle)
# 转至map坐标系
rospy.loginfo("获取ORB_SLAM/World->TF map")
tf_flag = False
while not tf_flag and not rospy.is_shutdown(
) and self.running_flag:
try:
t = rospy.Time(0)
self.listener.waitForTransform("ORB_SLAM/World", "map", t,
rospy.Duration(1.0))
tf_flag = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as e:
tf_flag = False
rospy.logwarn("获取TF失败 ORB_SLAM/World->map")
rospy.logwarn(e)
if not self.running_flag:
self.load_targets_exited_flag = True
return
rospy.loginfo("获取TF 成功 ORB_SLAM/World->map")
waypoint = self.listener.transformPose("map", pose_in_world)
self.waypoints.append(waypoint)
self.load_targets_exited_flag = True
# 通过全局规划器，计算目标点的朝向
rospy.loginfo("waiting for move_base/make_plan service")
rospy.wait_for_service("/move_base/make_plan")
rospy.loginfo("waiting for move_base/make_plan service succeed")
make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
for waypoint in self.waypoints:
req = GetPlanRequest()
req.start = self.waypoints[0]
if waypoint == self.waypoints[0]:
req.start = self.waypoints[1]
req.goal = waypoint
req.tolerance = 0.1
res = make_plan(req)
if len(res.plan.poses) > 10:
res.plan.poses = res.plan.poses[:-4]
plan_path_2d = [[point.pose.position.x, point.pose.position.y]
for point in res.plan.poses]
angle = self.get_target_direction(
[waypoint.pose.position.x, waypoint.pose.position.y],
plan_path_2d)
q_angle = quaternion_from_euler(0,
0,
math.atan2(angle[1], angle[0]) +
math.pi,
axes='sxyz')
waypoint.pose.orientation = Quaternion(*q_angle)
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def reset_goals(self):
self.current_goal_id = -1
self.goal_status = "FREE"
self.load_targets_task()
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def start(self):
self.delay = rospy.get_param('~delay', 10)
self.prefilter_size = rospy.get_param('~prefilter_size', '23')
self.prefilter_cap = rospy.get_param('~prefilter_cap', '33')
self.correlation_window_size = rospy.get_param(
'~correlation_window_size', '41')
self.min_disparity = rospy.get_param('~min_disparity', '44')
self.disparity_range = rospy.get_param('~disparity_range', '64')
self.uniqueness_ratio = rospy.get_param('~uniqueness_ratio', '15.0')
self.texture_threshold = rospy.get_param('~texture_threshold', '10')
self.speckle_size = rospy.get_param('~speckle_size', '356')
self.speckle_range = rospy.get_param('~speckle_range', '7')
self.restore_time = rospy.get_param('~restore_time', 5)
self.heartbeat_period = rospy.get_param('~heartbeat_period', 5.0)
self.heartbeat_timeout = rospy.get_param('~heartbeat_timeout', 10.0)
rospy.loginfo(
"\n"
"=================================================================\n"
"Started 3DPC Extractor with the following paramterers:\n"
"=================================================================\n"
"ID : " + self.id + "\n"
"Delay (s) : " + str(self.delay) + "\n"
"Prefilter size : " + self.prefilter_size + "\n"
"Prefilter cap : " + self.prefilter_cap + "\n"
"Correlation Window Size : " + self.correlation_window_size + "\n"
"Min. Disparity : " + self.min_disparity + "\n"
"Disparity Range : " + self.disparity_range + "\n"
"Uniqueness Ratio : " + self.uniqueness_ratio + "\n"
"Texture Threshold : " + self.texture_threshold + "\n"
"Speckle Size : " + self.speckle_size + "\n"
"Speckle Range : " + self.speckle_range + "\n"
"Bond Restore Time (s) : " + str(self.restore_time) + "\n"
"Heartbeat Period (s) : " + str(self.heartbeat_period) + "\n"
"Heartbeat Timeout (s) : " + str(self.heartbeat_timeout) + "\n"
"New 3DPC Msg Queue size : " + str(self.new_3dpc_msg_queue_size) +
"\n"
"=================================================================\n"
)
time.sleep(self.delay)
self.node_publisher = rospy.Publisher(
"/new_3dpc_extractor",
New3DPCExtractor.New3DPCExtractor,
queue_size=self.new_3dpc_msg_queue_size)
node = roslaunch.core.Node(
package="stereo_image_proc",
node_type="stereo_image_proc",
name=self.id,
machine_name="localhost",
args="",
respawn=True,
remap_args=[("/left/image_color", self.id + "/left/image_color"),
("/left/image_mono", self.id + "/left/image_mono"),
("/left/image_rect", self.id + "/left/image_rect"),
("/left/image_rect_color",
self.id + "/left/image_rect_color"),
("/right/image_color", self.id + "/right/image_color"),
("/right/image_mono", self.id + "/right/image_mono"),
("/right/image_rect", self.id + "/right/image_rect"),
("/right/image_rect_color",
self.id + "/right/image_rect_color")],
env_args=None,
output=None,
cwd=None,
launch_prefix=None,
required=False)
self.roslaunch_instance = roslaunch.scriptapi.ROSLaunch()
self.roslaunch_instance.start()
param_path = "/" + self.id + "/"
rosparam.set_param(param_path + "prefilter_size", self.prefilter_size)
rosparam.set_param(param_path + "prefilter_cap", self.prefilter_cap)
rosparam.set_param(param_path + "correlation_window_size",
self.correlation_window_size)
rosparam.set_param(param_path + "min_disparity", self.min_disparity)
rosparam.set_param(param_path + "disparity_range",
self.disparity_range)
rosparam.set_param(param_path + "uniqueness_ratio",
self.uniqueness_ratio)
rosparam.set_param(param_path + "texture_threshold",
self.texture_threshold)
rosparam.set_param(param_path + "speckle_size", self.speckle_size)
rosparam.set_param(param_path + "speckle_range", self.speckle_range)
self.roslaunch_instance.launch(node)
new_extractor_msg = New3DPCExtractor.New3DPCExtractor()
new_extractor_msg.id = self.id
new_extractor_msg.topic = self.id + "/bond"
self.node_publisher.publish(new_extractor_msg)
bond = bondpy.Bond(self.id + "/bond", self.id)
bond.set_heartbeat_period(self.heartbeat_period)
bond.set_heartbeat_timeout(self.heartbeat_timeout)
rospy.Timer(rospy.Duration(self.restore_time), self.restore_bond)
bond.start()
def test_rosparam(self,s):
rosparam.set_param("/dadayeh_param",s)
package_name = sys.argv[1] + "_support"
command = sys.argv[2]
if command == "-l":
list_work_objects(package_name)
exit()
else:
work_name = command
rospy.init_node("publish_work")
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(1.0) # wait for the above things to setup
remove_all_objects(scene)
work = parse_urdf_file(package_name, work_name)
publish_parsed_urdf(work, scene)
# parse and load task to parameter server
rospack = rospkg.RosPack()
filename = work_name + ".irl"
task_path = rospack.get_path(package_name) + REL_TASK_PATH + filename
rosparam.set_param("/planning_task_path", task_path)
task = parse_file(task_path)
plotter = Plotter(ref_frame="/world")
# plotter = Plotter(ref_frame="/work") # TODO fix setup 3
show_task(plotter, task)
print("Done!")
def load_targets_task(self):
if not os.path.exists(self.nav_points_file):
self.target_points = []
self.waypoints = list()
with open(self.nav_points_file, "r") as nav_data_file:
nav_data_str = nav_data_file.readline()
self.target_points = []
while len(nav_data_str) != 0:
pos_x = float(nav_data_str.split(" ")[0])
pos_y = float(nav_data_str.split(" ")[1])
pos_z = float(nav_data_str.split(" ")[2])
self.target_points.append([pos_x, pos_y, pos_z])
nav_data_str = nav_data_file.readline()
self.waypoints = list()
# nav_path_points = []
# with open("/home/xiaoqiang/slamdb/path.csv", "r") as nav_data_file:
# nav_data_str = nav_data_file.readline()
# while len(nav_data_str) != 0:
# pos_x = float(nav_data_str.split(" ")[0])
# pos_y = float(nav_data_str.split(" ")[1])
# pos_z = float(nav_data_str.split(" ")[2])
# nav_path_points.append([pos_x, pos_y, pos_z])
# nav_data_str = nav_data_file.readline()
# nav_path_points_2d = [[point[0], point[2]]
# for point in nav_path_points]
for point in self.target_points:
pose_in_world = PoseStamped()
pose_in_world.header.frame_id = "ORB_SLAM/World"
pose_in_world.header.stamp = rospy.Time(0)
pose_in_world.pose.position = Point(point[0], point[1], point[2])
# axis = self.get_target_direction(
# [point[0], point[2]], nav_path_points_2d)
# q_angle = quaternion_from_euler(
# math.pi / 2, -math.atan2(axis[1], axis[0]), 0, axes='sxyz')
q_angle = quaternion_from_euler(
0, 0, 0, axes='sxyz')
pose_in_world.pose.orientation = Quaternion(*q_angle)
# 转至map坐标系
rospy.loginfo("获取ORB_SLAM/World->TF map")
tf_flag = False
while not tf_flag and not rospy.is_shutdown() and self.running_flag:
try:
t = rospy.Time(0)
self.listener.waitForTransform("ORB_SLAM/World", "map", t,
rospy.Duration(1.0))
tf_flag = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception) as e:
tf_flag = False
rospy.logwarn("获取TF失败 ORB_SLAM/World->map")
rospy.logwarn(e)
if not self.running_flag:
self.load_targets_exited_flag = True
return
rospy.loginfo("获取TF 成功 ORB_SLAM/World->map")
waypoint = self.listener.transformPose(
"map", pose_in_world)
self.waypoints.append(waypoint)
self.load_targets_exited_flag = True
# 通过全局规划器，计算目标点的朝向
rospy.loginfo("waiting for move_base/make_plan service")
rospy.wait_for_service("/move_base/make_plan")
rospy.loginfo("waiting for move_base/make_plan service succeed")
make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
for waypoint in self.waypoints:
req = GetPlanRequest()
req.start = self.waypoints[0]
if waypoint == self.waypoints[0]:
req.start = self.waypoints[1]
req.goal = waypoint
req.tolerance = 0.1
res = make_plan(req)
if len(res.plan.poses) > 10:
res.plan.poses = res.plan.poses[:-4]
plan_path_2d = [[point.pose.position.x, point.pose.position.y]
for point in res.plan.poses]
angle = self.get_target_direction(
[waypoint.pose.position.x, waypoint.pose.position.y], plan_path_2d)
q_angle = quaternion_from_euler(0, 0, math.atan2(
angle[1], angle[0]) + math.pi, axes='sxyz')
waypoint.pose.orientation = Quaternion(*q_angle)
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def reset_goals(self):
self.current_goal_id = -1
self.goal_status = "FREE"
self.load_targets_task()
rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
