+neem_init

Author: daniel86

launch/openease.launch

@@ -13,6 +13,8 @@
     <arg name="websocket_external_port" value="80" />
   </include>
 
+  <node pkg="tf2_web_republisher" type="tf2_web_republisher" name="tf2_web_republisher" />
+  
   <node pkg="knowrob_openease" name="image_saver" type="image_saver.py" output="screen">
   </node>
 

src/__init__.pl

@@ -2,10 +2,13 @@
 :- register_ros_package(knowrob_openease).
 :- register_ros_package(knowrob).
 
-% load query tunnling
+% query answering
 :- use_module('./query_handler.pl').
 :- use_module('./query_history.pl').
 
+% handling of NEEMs
+:- use_module('./neems.pl').
+
 % load marker visualization
 :- use_directory('marker_vis').
 % load IDE interface

src/backends/canvas.pl

@@ -8,27 +8,21 @@
 		triple(Interval,soma:hasIntervalBegin,Time0),
 		triple(Interval,soma:hasIntervalEnd,Time1)
 	]))),
-	writeln(set_goal(Time0,Time1)),
-	tf_plugin:tf_republish_set_goal(Time0,Time1),
-	
-	get_time(Time),
-    marker_plugin:show_markers(Time).
-	%data_vis_tf(Time0, Time1,
-	%	[title: 'Scene']).
+	tf_plugin:tf_republish_set_goal(Time0,Time1).
 
-data_vis_tf(StartTime, EndTime, Options) :-
-	% gather data
-	findall([[Frame,Time],[RefFrame,X,Y,Z,QX,QY,QZ,QW]],
-		tf_plugin:tf_mng_range(StartTime,EndTime,
-			Frame,
-			[RefFrame,[X,Y,Z],[QX,QY,QZ,QW]],Time,_),
-		ArrayData),
-	% generate ID for the chart
-	atomic_list_concat(
-		['tf',StartTime,EndTime],'_',ID),
-	% publish the message
-    data_vis(tf(ID),
-    	[array_data: ArrayData | Options]).
+%data_vis_tf(StartTime, EndTime, Options) :-
+%	% gather data
+%	findall([[Frame,Time],[RefFrame,X,Y,Z,QX,QY,QZ,QW]],
+%		tf_plugin:tf_mng_range(StartTime,EndTime,
+%			Frame,
+%			[RefFrame,[X,Y,Z],[QX,QY,QZ,QW]],Time,_),
+%		ArrayData),
+%	% generate ID for the chart
+%	atomic_list_concat(
+%		['tf',StartTime,EndTime],'_',ID),
+%	% publish the message
+%    data_vis(tf(ID),
+%    	[array_data: ArrayData | Options]).
 
 %query_handler:openease_gen_answer(event,Evts) :-
 %	% TODO: handle multiple events

src/neems.pl

@@ -0,0 +1,20 @@
+:- module(neems,
+	[ neem_init/1
+	]).
+
+neem_init(NEEM_id) :-
+	% assign DB collection prefix
+	set_setting(mng_client:collection_prefix, NEEM_id),
+	% load URDF files referred to in triple store
+	urdf_init,
+writeln(load_init_tf),
+	% initialize position of each frame for tf publishing
+	tf_tree:initial_transforms(InitialTransforms),
+length(InitialTransforms,NInitialTransforms),
+writeln(initial_transforms(NInitialTransforms)),
+	forall(
+		member([Ref,Frame,Pos,Rot],InitialTransforms),
+		tf_plugin:tf_republish_set_pose(Frame,[Ref,Pos,Rot])
+	),
+	% publish object marker messages
+	marker_plugin:republish.