From 7cb8c70eeebcfbacabf362b98c558ff9c6c52874 Mon Sep 17 00:00:00 2001
From: Timothy Stack <stack@flux.utah.edu>
Date: Tue, 31 May 2005 13:47:23 +0000
Subject: [PATCH] Change the MTP_CREATE_OBSTACLE packet type to include a robot
 ID for obstacles that represent a robot.

---
 robots/mtp/mtp.c | 20 ++++++++++++++++++++
 robots/mtp/mtp.x | 13 ++++++++++++-
 2 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/robots/mtp/mtp.c b/robots/mtp/mtp.c
index 0305ef26c..566b87881 100755
--- a/robots/mtp/mtp.c
+++ b/robots/mtp/mtp.c
@@ -420,6 +420,9 @@ mtp_error_t mtp_init_packetv(struct mtp_packet *mp,
 		    va_arg(args, struct obstacle_config *);
 		break;
 	    case MTP_CREATE_OBSTACLE:
+		mp->data.mtp_payload_u.create_obstacle.config =
+		    *(va_arg(args, struct obstacle_config *));
+		break;
 	    case MTP_UPDATE_OBSTACLE:
 		mp->data.mtp_payload_u.update_obstacle =
 		    *(va_arg(args, struct obstacle_config *));
@@ -467,6 +470,10 @@ mtp_error_t mtp_init_packetv(struct mtp_packet *mp,
 		mp->data.mtp_payload_u.request_report.robot_id = 
 		    va_arg(args, int);
 		break;
+	    case MTP_CREATE_OBSTACLE:
+		mp->data.mtp_payload_u.create_obstacle.robot_id =
+		    va_arg(args, int);
+		break;
 	    default:
 		assert(0);
 		break;
@@ -645,6 +652,19 @@ mtp_error_t mtp_init_packetv(struct mtp_packet *mp,
 	
 	tag = va_arg(args, mtp_tag_t);
     }
+
+    if (mp->data.opcode == MTP_UPDATE_POSITION) {
+	struct mtp_update_position *mup;
+
+	mup = &mp->data.mtp_payload_u.update_position;
+	if (mup->position.timestamp == 0.0) {
+	    struct timeval tv;
+	    
+	    gettimeofday(&tv, NULL);
+	    mup->position.timestamp = (double)tv.tv_sec +
+		(((double)tv.tv_usec) / 100000);
+	}
+    }
     
     return retval;
 }
diff --git a/robots/mtp/mtp.x b/robots/mtp/mtp.x
index e2d37858c..3f6f2da17 100644
--- a/robots/mtp/mtp.x
+++ b/robots/mtp/mtp.x
@@ -110,6 +110,11 @@ enum mtp_opcode_t {
      */
     MTP_TELEMETRY		= 60,
 
+    /**
+     * Snapshot the state of a daemon.
+     */
+    MTP_SNAPSHOT		= 61,
+
     /**
      * vmcd tells emcd to wiggle a robot (for id purposes).
      */
@@ -194,6 +199,11 @@ struct obstacle_config {
     float zmax;
 };
 
+struct dyn_obstacle_config {
+    int robot_id;
+    obstacle_config config;
+};
+
 struct box {
     float x;
     float y;
@@ -352,9 +362,10 @@ union mtp_payload switch (mtp_opcode_t opcode) {
  case MTP_WIGGLE_STATUS:	mtp_wiggle_status	wiggle_status;
  case MTP_REQUEST_REPORT:	mtp_request_position	request_report;
  case MTP_CONTACT_REPORT:	mtp_contact_report	contact_report;
- case MTP_CREATE_OBSTACLE:	obstacle_config		create_obstacle;
+ case MTP_CREATE_OBSTACLE:	dyn_obstacle_config	create_obstacle;
  case MTP_UPDATE_OBSTACLE:	obstacle_config		update_obstacle;
  case MTP_REMOVE_OBSTACLE:	int			remove_obstacle;
+ case MTP_SNAPSHOT:		int			snapshot;
 };
 
 /**
-- 
GitLab