[TeamTalk 8]: [545] TeamTalk/Agents: Added code for PenDecoder and Backend handling of setpos.

[tk@edam.speech.cs.cmu.edu]

An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20061109/7a29be6f/attachment.html
-------------- next part --------------
Modified: TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/DrawingCanvas.java
===================================================================
--- TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/DrawingCanvas.java	2006-11-09 19:17:31 UTC (rev 544)
+++ TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/DrawingCanvas.java	2006-11-09 20:10:38 UTC (rev 545)
@@ -92,6 +92,25 @@
     }
   }
   
+  /**
+   * this informs the bot that it should be at the screen position and orientation 
+   * indicated by the point
+   */
+  public void placeSelectedBot(Point p, float theta) {
+    screen_to_map.transform(p, p);
+    if(shapes != null) {
+      Iterator<Shape> i = shapes.iterator();
+      while(i.hasNext()) {
+        Shape shape = i.next();
+        if(shape.isSelected()) {
+          BotShape selectedBot = (BotShape) shape;
+          drawingPad.placeBot(shape.getName(), p, theta);
+          return;
+        }
+      }
+    }
+  }
+  
   public void search(RectangleShape shape) {
     //find translator bot and do translation here
     BotShape bot = BotOf(opTraderFrameHolder); //[2005-10-02] (tk): hack

Modified: TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/DrawingPad.java
===================================================================
--- TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/DrawingPad.java	2006-11-09 19:17:31 UTC (rev 544)
+++ TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/DrawingPad.java	2006-11-09 20:10:38 UTC (rev 545)
@@ -152,6 +152,13 @@
     }
   }
   
+  public void placeBot(String name, Point p, float theta) {
+      ListIterator<PenDecoderServer> i = servers.listIterator();
+      while(i.hasNext()) {
+          i.next().placeBot(name, p.x, p.y, theta);
+      }
+  }
+  
   public void selectBot(String name) {
     drawingCanvas.selectBot(name);
   }

Modified: TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderServer.java
===================================================================
--- TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderServer.java	2006-11-09 19:17:31 UTC (rev 544)
+++ TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderServer.java	2006-11-09 20:10:38 UTC (rev 545)
@@ -194,6 +194,15 @@
     sayToBot("Commands", "[HumanReportCommand]", rname);
   }
   
+  public void placeBot(String name, float x, float y, float theta) {
+    System.err.println("placing bot " + name + ": (" + x + " " + y + ") " + theta);
+    Map<String, String> pose = new HashMap<String, String>();
+    pose.put("[x]", Double.toString(x));
+    pose.put("[y]", Double.toString(y));
+    pose.put("[angle]", Double.toString(theta));
+    sayToBot("Commands", "[SetPoseCommand]", pose);
+  }
+  
   public void search(RectangleShape shape) {
     System.err.println("Searching...");
     Map<String, String> loc = new HashMap<String, String>();

Modified: TeamTalk/Agents/PrimitiveComm/robot_packet.cpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_packet.cpp	2006-11-09 19:17:31 UTC (rev 544)
+++ TeamTalk/Agents/PrimitiveComm/robot_packet.cpp	2006-11-09 20:10:38 UTC (rev 545)
@@ -580,4 +580,10 @@
 
 double MsgCmdSetPos::getX() const {return x_;}
 double MsgCmdSetPos::getY() const {return y_;}
-double MsgCmdSetPos::getAngle() const {return angle_;}
\ No newline at end of file
+double MsgCmdSetPos::getAngle() const {return angle_;}
+
+string MsgCmdSetPos::action_string() const {
+	ostringstream out;
+	out << "setpos (" << x_ << ' ' << y_ << ") " << angle_;
+	return out.str();
+}

Modified: TeamTalk/Agents/PrimitiveComm/robot_packet.hpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_packet.hpp	2006-11-09 19:17:31 UTC (rev 544)
+++ TeamTalk/Agents/PrimitiveComm/robot_packet.hpp	2006-11-09 20:10:38 UTC (rev 545)
@@ -95,6 +95,7 @@
 	double getX() const;
 	double getY() const;
 	double getAngle() const;
+	string action_string() const;
 };
 
 class MsgCmdHaltPP : public Msg {

Modified: TeamTalk/Agents/TeamTalkBackend/gal_be.cpp
===================================================================
--- TeamTalk/Agents/TeamTalkBackend/gal_be.cpp	2006-11-09 19:17:31 UTC (rev 544)
+++ TeamTalk/Agents/TeamTalkBackend/gal_be.cpp	2006-11-09 20:10:38 UTC (rev 545)
@@ -1197,6 +1197,17 @@
 #endif
 }
 
+static void set_pose_command(const char* robot, const Gal_Frame& inframe)
+{
+	GalUtil_Error("got turn command");
+	const char* x = Gal_GetString(inframe, ":x");
+	const char* y = Gal_GetString(inframe, ":y");
+	const char* a = Gal_GetString(inframe, ":angle");
+	MsgCmdSetPos set_pos(atof(x), atof(y), atof(a));
+	Boeing::RobotClient *r = p_client->find(robot);
+	if(r) sendAction(r, &set_pos);
+}
+
 Gal_Frame launch_query(Gal_Frame f, void *server_data) 
 {
 	Gal_Object aStr;
@@ -1261,6 +1272,7 @@
 		search_or_explore(query, robot, inframe);
 	else if(!strcmp(query, "move_to_goal_command")) move_to_goal_command(robot, inframe);
 	else if(!strcmp(query, "turn_command")) turn_command(robot, inframe);
+	else if(!strcmp(query, "set_pose_command")) set_pose_command(robot, inframe);
 	else GalUtil_Error("unhandled query: %s", query);
 	Gal_SetProp(f, ":outframe", aStr);