Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20118] pkg/trunk
|
From: <ei...@us...> - 2009-07-30 03:03:00
|
Revision: 20118
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20118&view=rev
Author: eitanme
Date: 2009-07-30 03:02:41 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Merging tha actionlib branch back into trunk
r29135@att (orig r19792): eitanme | 2009-07-27 18:30:30 -0700
Creating a branch for actionlib.... hopefully for the last time
r29137@att (orig r19794): eitanme | 2009-07-27 18:32:49 -0700
Changing ParticleCloud to PoseArray
r29139@att (orig r19796): eitanme | 2009-07-27 18:33:42 -0700
Adding action definition to the rep
r29148@att (orig r19805): eitanme | 2009-07-27 18:47:39 -0700
Some fixes... almost compiling
r29165@att (orig r19822): eitanme | 2009-07-27 20:41:07 -0700
Macro version of the typedefs that compiles
r29213@att (orig r19869): eitanme | 2009-07-28 11:49:10 -0700
Compling version of the ActionServer re-write complete with garbage collection, be default it will keep goals without goal handles for 5 seconds
r29220@att (orig r19876): eitanme | 2009-07-28 12:06:06 -0700
Fix to make sure that transitions into preempting and recalling states actually happen
r29254@att (orig r19888): eitanme | 2009-07-28 13:27:40 -0700
Forgot to actually call the cancel callback... addind a subscriber on the cancel topic
r29267@att (orig r19901): eitanme | 2009-07-28 14:41:09 -0700
Adding text field to GoalStatus to allow users to throw some debugging information into the GoalStatus messages
r29275@att (orig r19909): eitanme | 2009-07-28 15:43:49 -0700
Using tf remapping as I should've been doing for awhile
r29277@att (orig r19911): eitanme | 2009-07-28 15:48:48 -0700
The navigation stack can now handle goals that aren't in the global frame. However, these goals will be transformed to the global frame at the time of reception, so for achieving them accurately the global frame of move_base should really be set to match the goals.
r29299@att (orig r19933): stuglaser | 2009-07-28 17:08:10 -0700
Created genaction.py script to create the various messages that an action needs
r29376@att (orig r20003): vijaypradeep | 2009-07-29 02:45:24 -0700
ActionClient is running. MoveBase ActionServer seems to be crashing
r29409@att (orig r20033): vijaypradeep | 2009-07-29 11:57:54 -0700
Fixing bug with adding status trackers
r29410@att (orig r20034): vijaypradeep | 2009-07-29 11:58:18 -0700
Changing from Release to Debug
r29432@att (orig r20056): vijaypradeep | 2009-07-29 14:07:30 -0700
No longer building goal_manager_test.cpp
r29472@att (orig r20090): vijaypradeep | 2009-07-29 17:04:14 -0700
Lots of Client-Side doxygen
r29484@att (orig r20101): vijaypradeep | 2009-07-29 18:35:01 -0700
Adding to mainpage.dox
r29487@att (orig r20104): eitanme | 2009-07-29 18:55:06 -0700
Removing file to help resolve merge I hope
r29489@att (orig r20106): eitanme | 2009-07-29 19:00:07 -0700
Removing another file to try to resolve the branch
r29492@att (orig r20108): eitanme | 2009-07-29 19:14:25 -0700
Again removing a file to get the merge working
r29493@att (orig r20109): eitanme | 2009-07-29 19:34:45 -0700
Removing yet another file on which ssl negotiation fails
r29500@att (orig r20116): eitanme | 2009-07-29 19:54:18 -0700
Fixing bug in genaction
Modified Paths:
--------------
pkg/trunk/sandbox/actionlib/CMakeLists.txt
pkg/trunk/sandbox/actionlib/include/actionlib/action_server.h
pkg/trunk/sandbox/actionlib/include/actionlib/goal_id_generator.h
pkg/trunk/sandbox/actionlib/include/actionlib/single_goal_action_server.h
pkg/trunk/sandbox/actionlib/mainpage.dox
pkg/trunk/sandbox/actionlib/msg/GoalStatus.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseAction.msg
pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
pkg/trunk/sandbox/actionlib/src/move_base_server.cpp
pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp
pkg/trunk/stacks/nav/fake_localization/fake_localization.cpp
pkg/trunk/stacks/nav/move_base/CMakeLists.txt
pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h
pkg/trunk/stacks/nav/move_base/msg/MoveBaseAction.msg
pkg/trunk/stacks/nav/move_base/src/move_base.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/stacks/nav/navfn/src/navfn_ros.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/particle_cloud_2d_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/particle_cloud_2d_display.h
Added Paths:
-----------
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
pkg/trunk/sandbox/actionlib/genaction.py
pkg/trunk/sandbox/actionlib/include/actionlib/action_definition.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/
pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/comm_state.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/comm_state_machine.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/comm_state_machine.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/terminal_state.h
pkg/trunk/sandbox/actionlib/include/actionlib/managed_list.h
pkg/trunk/sandbox/actionlib/include/actionlib/old_action_client.h_old
pkg/trunk/sandbox/actionlib/include/actionlib/older_action_client.h_old
pkg/trunk/sandbox/descriptors_2d/descriptors.cpp
pkg/trunk/sandbox/descriptors_2d/descriptors.h
pkg/trunk/sandbox/descriptors_2d_gpl/image_descriptors_gpl.cpp
pkg/trunk/sandbox/descriptors_2d_gpl/include/image_descriptors_gpl/
pkg/trunk/sandbox/descriptors_2d_gpl/include/image_descriptors_gpl/image_descriptors_gpl.h
pkg/trunk/stacks/common_msgs/nav_msgs/msg/PoseArray.msg
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.0/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.0/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.1/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.1/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.1/run_stubs.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.0/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.0/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.1/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.1/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.1/run_stubs.launch
Removed Paths:
-------------
pkg/trunk/sandbox/actionlib/include/actionlib/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/old_action_client.h
pkg/trunk/stacks/common_msgs/nav_msgs/msg/ParticleCloud.msg
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/vision/place_recognition/test/directed.py
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:19631
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:20116
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
Added: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c (rev 0)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c 2009-07-30 03:02:41 UTC (rev 20118)
@@ -0,0 +1,1289 @@
+#include "pr2lib.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/time.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdbool.h>
+#include <net/if.h>
+
+#include "host_netutil.h"
+#include "ipcam_packet.h"
+
+/// Amount of time in microseconds that the host should wait for packet replies
+#define STD_REPLY_TIMEOUT SEC_TO_USEC(0.2)
+
+#define VMODEDEF(width, height, fps, hblank, vblank) { #width"x"#height"x"#fps, width, height, fps, hblank, vblank }
+const struct MT9VMode MT9VModes[MT9V_NUM_MODES] = {
+ VMODEDEF(752,480,15, 974, 138),
+ VMODEDEF(752,480,12.5, 848, 320),
+ VMODEDEF(640,480,30, 372, 47),
+ VMODEDEF(640,480,25, 543, 61),
+ VMODEDEF(640,480,15, 873, 225),
+ VMODEDEF(640,480,12.5, 960, 320),
+ VMODEDEF(320,240,60, 106, 73),
+ VMODEDEF(320,240,50, 180, 80),
+ VMODEDEF(320,240,30, 332, 169),
+ VMODEDEF(320,240,25, 180, 400)
+};
+
+/**
+ * Returns the version information for the library
+ *
+ *
+ * @return Returns the the library as an integer 0x0000MMNN, where MM = Major version and NN = Minor version
+ */
+int pr2LibVersion() {
+ return PR2LIB_VERSION;
+}
+
+
+/**
+ * Waits for a PR2 StatusPacket on the specified socket for a specified duration.
+ *
+ * The Status type and code will be reported back to the called via the 'type' & 'code'
+ * arguments. If the timeout expires before a valid status packet is received, then
+ * the function will still return zero but will indicate that a TIMEOUT error has occurred.
+ *
+ * @param s The open, bound & 'connected' socket to listen on
+ * @param wait_us The number of microseconds to wait before timing out
+ * @param type Points to the location to store the type of status packet
+ * @param code Points to the location to store the subtype/error code
+ *
+ * @return Returns 0 if no system errors occured. -1 with errno set otherwise.
+ */
+int pr2StatusWait( int s, uint32_t wait_us, uint32_t *type, uint32_t *code ) {
+ if( !wgWaitForPacket(s, PKTT_STATUS, sizeof(PacketStatus), &wait_us) && (wait_us != 0) ) {
+ PacketStatus sPkt;
+ if( recvfrom( s, &sPkt, sizeof(PacketStatus), 0, NULL, NULL ) == -1 ) {
+ perror("wgDiscover unable to receive from socket");
+ *type = PKT_STATUST_ERROR;
+ *code = PKT_ERROR_SYSERR;
+ return -1;
+ }
+
+ *type = ntohl(sPkt.status_type);
+ *code = ntohl(sPkt.status_code);
+ return 0;
+ }
+
+ *type = PKT_STATUST_ERROR;
+ *code = PKT_ERROR_TIMEOUT;
+ return 0;
+}
+
+
+/**
+ * Discovers all PR2 cameras that are connected to the 'ifName' ethernet interface and
+ * adds new ones to the 'ipCamList' list.
+ *
+ * @param ifName The ethernet interface name to use. Null terminated string (e.g., "eth0").
+ * @param ipCamList The list to which the new cameras should be added
+ * @param wait_us The number of microseconds to wait for replies
+ *
+ * @return Returns -1 with errno set for system call errors. Otherwise returns number of new
+ * cameras found.
+ */
+int pr2Discover(const char *ifName, IpCamList *ipCamList, const char *ipAddress, unsigned wait_us) {
+ // Create and initialize a new Discover packet
+ PacketDiscover dPkt;
+ dPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ dPkt.hdr.type = htonl(PKTT_DISCOVER);
+ strncpy(dPkt.hdr.hrt, "DISCOVER", sizeof(dPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(ifName, &dPkt.hdr.reply_to);
+ if(s == -1) {
+ perror("Unable to create socket\n");
+ return -1;
+ }
+
+ //// Determine the broadcast address for ifName. This is the address we
+ //// will tell the camera to send from.
+
+ struct in_addr newIP;
+ if (ipAddress) // An IP was specified
+ {
+ inet_aton(ipAddress, &newIP);
+ dPkt.ip_addr = newIP.s_addr;
+ }
+ else // We guess an IP by flipping the host bits of the local IP.
+ {
+ struct in_addr localip;
+ struct in_addr netmask;
+ wgIpGetLocalAddr(ifName, &localip);
+ wgIpGetLocalNetmask(ifName, &netmask);
+ dPkt.ip_addr = localip.s_addr ^ netmask.s_addr ^ ~0;
+ }
+
+ if( wgSendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)) == -1) {
+ perror("Unable to send broadcast\n");
+ }
+
+ // For the old API
+ if( wgSendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)-sizeof(dPkt.ip_addr)) == -1) {
+ perror("Unable to send broadcast\n");
+ }
+
+ // Count the number of new cameras found
+ int newCamCount = 0;
+ do {
+ // Wait in the loop for replies. wait_us is updated each time through the loop.
+ if( !wgWaitForPacket(s, PKTT_ANNOUNCE, sizeof(PacketAnnounce), &wait_us) && (wait_us != 0) ) {
+ // We've received an Announce packet, so pull it out of the receive queue
+ PacketAnnounce aPkt;
+ struct sockaddr_in fromaddr;
+ fromaddr.sin_family = AF_INET;
+ socklen_t fromlen = sizeof(fromaddr);
+
+ if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, (struct sockaddr *) &fromaddr, &fromlen) == -1 ) {
+ perror("wgDiscover unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ // Create a new list entry and initialize it
+ IpCamList *tmpListItem;
+ if( (tmpListItem = (IpCamList *)malloc(sizeof(IpCamList))) == NULL ) {
+ perror("Malloc failed");
+ close(s);
+ return -1;
+ }
+ pr2CamListInit( tmpListItem );
+
+ // Initialize the new list item's data fields (byte order corrected)
+ tmpListItem->hw_version = ntohl(aPkt.hw_version);
+ tmpListItem->fw_version = ntohl(aPkt.fw_version);
+ tmpListItem->ip = fromaddr.sin_addr.s_addr;
+ tmpListItem->serial = ntohl(aPkt.ser_no);
+ memcpy(&tmpListItem->mac, aPkt.mac, sizeof(aPkt.mac));
+ strncpy(tmpListItem->ifName, ifName, sizeof(tmpListItem->ifName));
+ tmpListItem->status = CamStatusDiscovered;
+ char pcb_rev = 0x0A + (0x0000000F & ntohl(aPkt.hw_version));
+ int hdl_rev = 0x00000FFF & (ntohl(aPkt.hw_version)>>4);
+ snprintf(tmpListItem->hwinfo, PR2_CAMINFO_LEN, "PCB rev %X : HDL rev %3X : FW rev %3X", pcb_rev, hdl_rev, ntohl(aPkt.fw_version));
+
+ // If this camera is already in the list, we don't want to add another copy
+ if( pr2CamListAdd( ipCamList, tmpListItem ) == CAMLIST_ADD_DUP) {
+ free(tmpListItem);
+ } else {
+ debug("MAC Address: %02X:%02X:%02X:%02X:%02X:%02X\n", aPkt.mac[0], aPkt.mac[1], aPkt.mac[2], aPkt.mac[3], aPkt.mac[4], aPkt.mac[5]);
+ debug("Product #%07u : Unit #%04u\n", ntohl(aPkt.product_id), ntohl(aPkt.ser_no));
+ debug("%s\n", tmpListItem->hwinfo);
+ newCamCount++;
+ }
+ }
+ } while(wait_us > 0);
+
+ close(s);
+ return newCamCount;
+}
+
+
+/**
+ * Configures the IP address of one specific camera.
+ *
+ * @param camInfo Structure describing the camera to configure
+ * @param ipAddress An ASCII string containing the new IP address to assign (e.g., "192.168.0.5")
+ * @param wait_us The number of microseconds to wait for a reply from the camera
+ *
+ * @return Returns -1 with errno set for system call failures.
+ * Returns 0 for success
+ * Returns ERR_TIMEOUT if no reply is received before the timeout expires
+ * Returns ERR_CONFIG_ARPFAIL if the camera was configured successfully but
+ * the host system could not update its arp table. This is normally because
+ * the library is not running as root.
+ */
+int pr2Configure( IpCamList *camInfo, const char *ipAddress, unsigned wait_us) {
+ // Create and initialize a new Configure packet
+ PacketConfigure cPkt;
+
+ cPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ cPkt.hdr.type = htonl(PKTT_CONFIGURE);
+ cPkt.product_id = htonl(CONFIG_PRODUCT_ID);
+ strncpy(cPkt.hdr.hrt, "CONFIGURE", sizeof(cPkt.hdr.hrt));
+
+ cPkt.ser_no = htonl(camInfo->serial);
+
+ struct in_addr newIP;
+ inet_aton(ipAddress, &newIP);
+ cPkt.ip_addr = newIP.s_addr;
+
+
+ // Create and send the Configure broadcast packet. It is sent broadcast
+ // because the camera does not yet have an IP address assigned.
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &cPkt.hdr.reply_to);
+ if(s == -1) {
+ perror("pr2Configure socket creation failed");
+ return -1;
+ }
+
+ if(wgSendUDPBcast(s, camInfo->ifName, &cPkt, sizeof(cPkt)) == -1) {
+ debug("Unable to send broadcast\n");
+ close(s);
+ return -1;
+ }
+
+
+ // 'Connect' insures we will only receive datagram replies from the camera's new IP
+ IPAddress camIP = newIP.s_addr;
+ if( wgSocketConnect(s, &camIP) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait up to wait_us for a valid packet to be received on s
+ do {
+ if( !wgWaitForPacket(s, PKTT_ANNOUNCE, sizeof(PacketAnnounce), &wait_us) && (wait_us != 0) ) {
+ PacketAnnounce aPkt;
+
+ if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, NULL, NULL ) == -1 ) {
+ perror("wgDiscover unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ // Need to have a valid packet from a camera with the expected serial number
+ if( ntohl(aPkt.ser_no) == camInfo->serial ) {
+ camInfo->status = CamStatusConfigured;
+ memcpy(&camInfo->ip, &cPkt.ip_addr, sizeof(IPAddress));
+ // Add the IP/MAC mapping to the system ARP table
+ if( wgArpAdd(camInfo) ) {
+ close(s);
+ return ERR_CONFIG_ARPFAIL;
+ }
+ break;
+ }
+ }
+ } while(wait_us > 0);
+ close(s);
+
+ if(wait_us != 0) {
+ return 0;
+ } else {
+ return ERR_TIMEOUT;
+ }
+}
+
+/**
+ * Instructs one camera to begin streaming video to the host/port specified.
+ *
+ * @param camInfo Structure that describes the camera to contact
+ * @param mac Contains the MAC address of the host that will receive the video
+ * @param ipAddress An ASCII string in dotted quad form containing the IP address of the host
+ * that will receive the video (e.g., "192.168.0.5")
+ * @param port The port number that the video should be sent to. Host byte order.
+ *
+ * @return Returns -1 with errno set for system call failures
+ * Returns 0 for success
+ * Returns 1 for protocol failures (timeout, etc)
+ */
+int pr2StartVid( const IpCamList *camInfo, const uint8_t mac[6], const char *ipAddress, uint16_t port ) {
+ PacketVidStart vPkt;
+
+ // Initialize the packet
+ vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ vPkt.hdr.type = htonl(PKTT_VIDSTART);
+ strncpy(vPkt.hdr.hrt, "Start Video", sizeof(vPkt.hdr.hrt));
+
+
+ // Convert the ipAddress string into a binary value in network byte order
+ inet_aton(ipAddress, (struct in_addr*)&vPkt.receiver.addr);
+
+ // Convert the receiver port into network byte order
+ vPkt.receiver.port = htons(port);
+
+ // Copy the MAC address into the vPkt
+ memcpy(&vPkt.receiver.mac, mac, 6);
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if(wgSendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1) {
+ goto err_out;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ goto err_out;
+ }
+
+ // Wait for a status reply
+ uint32_t type, code;
+ if( pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
+ goto err_out;
+ }
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+
+err_out:
+ close(s);
+ return -1;
+}
+
+/**
+ * Instructs one camera to stop sending video.
+ *
+ * @param camInfo Describes the camera to send the stop to.
+ * @return Returns 0 for success
+ * Returns -1 with errno set for system errors
+ * Returns 1 for protocol errors
+ */
+int pr2StopVid( const IpCamList *camInfo ) {
+ PacketVidStop vPkt;
+
+ // Initialize the packet
+ vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ vPkt.hdr.type = htonl(PKTT_VIDSTOP);
+ strncpy(vPkt.hdr.hrt, "Stop Video", sizeof(vPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1 ) {
+ goto err_out;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) == -1) {
+ goto err_out;
+ }
+
+ uint32_t type, code;
+ if(pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
+ goto err_out;
+ }
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: pr2StatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+
+err_out:
+ close(s);
+ return -1;
+}
+
+/**
+ * Instructs one camera to execute a soft reset.
+ *
+ * @param camInfo Describes the camera to reset.
+ * @return Returns 0 for success
+ * Returns -1 for system errors
+ */
+int pr2Reset( IpCamList *camInfo ) {
+ PacketReset gPkt;
+
+ // Initialize the packet
+ gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ gPkt.hdr.type = htonl(PKTT_RESET);
+ strncpy(gPkt.hdr.hrt, "Reset", sizeof(gPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+
+ close(s);
+
+ // Camera is no longer configured after a reset
+ camInfo->status = CamStatusDiscovered;
+
+ // There is no reply to a reset packet
+
+ return 0;
+}
+
+/**
+ * Gets the system time of a specified camera.
+ *
+ * In the camera, system time is tracked as a number of clock 'ticks' since
+ * the last hard reset. This function receives the number of 'ticks' as well as
+ * a 'ticks_per_sec' conversion factor to return a 64-bit time result in microseconds.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param time_us Points to the location to store the system time in us
+ *
+ * @return Returns 0 for success, -1 for system error, or 1 for protocol error.
+ */
+int pr2GetTimer( const IpCamList *camInfo, uint64_t *time_us ) {
+ PacketTimeRequest gPkt;
+
+ // Initialize the packet
+ gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ gPkt.hdr.type = htonl(PKTT_TIMEREQ);
+ strncpy(gPkt.hdr.hrt, "Time Req", sizeof(gPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ uint32_t wait_us = STD_REPLY_TIMEOUT;
+ do {
+ if( !wgWaitForPacket(s, PKTT_TIMEREPLY, sizeof(PacketTimer), &wait_us) && (wait_us != 0) ) {
+ PacketTimer tPkt;
+ if( recvfrom( s, &tPkt, sizeof(PacketTimer), 0, NULL, NULL ) == -1 ) {
+ perror("GetTime unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ *time_us = (uint64_t)ntohl(tPkt.ticks_hi) << 32;
+ *time_us += ntohl(tPkt.ticks_lo);
+
+ // Need to multiply final result by 1E6 to get us from sec
+ // Try to do this to limit loss of precision without overflow
+ // (We will overflow the 64-bit type with this approach
+ // after the camera has been up for about 11 continuous years)
+ //FIXME: Review this algorithm for possible improvements.
+ *time_us *= 1000;
+ *time_us /= (ntohl(tPkt.ticks_per_sec)/1000);
+ //debug("Got time of %llu us since last reset\n", *time_us);
+ close(s);
+ return 0;
+ }
+ } while(wait_us > 0);
+
+ debug("Timed out waiting for time value\n");
+ close(s);
+ return 1;
+}
+
+/**
+ * Reads one FLASH_PAGE_SIZE byte page of the camera's onboard Atmel dataflash.
+ *
+ * @param camInfo Describes the camera to connect to.
+ * @param address Specifies the 12-bit flash page address to read (0-4095)
+ * @param pageDataOut Points to at least FLASH_PAGE_SIZE bytes of storage in which to place the flash data.
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ *
+ */
+
+int pr2FlashRead( const IpCamList *camInfo, uint32_t address, uint8_t *pageDataOut ) {
+ PacketFlashRequest rPkt;
+
+ // Initialize the packet
+ rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ rPkt.hdr.type = htonl(PKTT_FLASHREAD);
+ if(address > FLASH_MAX_PAGENO) {
+ return 1;
+ }
+
+ // The page portion of the address is 12-bits wide, range (bit9..bit21)
+ rPkt.address = htonl(address<<9);
+
+ strncpy(rPkt.hdr.hrt, "Flash read", sizeof(rPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ uint32_t wait_us = STD_REPLY_TIMEOUT;
+ do {
+ if( !wgWaitForPacket(s, PKTT_FLASHDATA, sizeof(PacketFlashPayload), &wait_us) && (wait_us != 0) ) {
+ PacketFlashPayload fPkt;
+ if( recvfrom( s, &fPkt, sizeof(PacketFlashPayload), 0, NULL, NULL ) == -1 ) {
+ perror("GetTime unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ // Copy the received data into the space pointed to by pageDataOut
+ memcpy(pageDataOut, fPkt.data, FLASH_PAGE_SIZE);
+ close(s);
+ return 0;
+ }
+ } while(wait_us > 0);
+
+ debug("Timed out waiting for flash value\n");
+ close(s);
+ return 1;
+}
+
+/**
+ * Writes one FLASH_PAGE_SIZE byte page to the camera's onboard Atmel dataflash.
+ *
+ * @param camInfo Describes the camera to connect to.
+ * @param address Specifies the 12-bit flash page address to write (0-4095)
+ * @param pageDataOut Points to at least FLASH_PAGE_SIZE bytes of storage from which to get the flash data.
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ *
+ */
+int pr2FlashWrite( const IpCamList *camInfo, uint32_t address, const uint8_t *pageDataIn ) {
+ PacketFlashPayload rPkt;
+
+ // Initialize the packet
+ rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ rPkt.hdr.type = htonl(PKTT_FLASHWRITE);
+ if(address > FLASH_MAX_PAGENO) {
+ return 1;
+ }
+
+ // The page portion of the address is 12-bits wide, range (bit9..bit21)
+ rPkt.address = htonl(address<<9);
+ strncpy(rPkt.hdr.hrt, "Flash write", sizeof(rPkt.hdr.hrt));
+
+ memcpy(rPkt.data, pageDataIn, FLASH_PAGE_SIZE);
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response - once we get an OK, we're clear to send the next page.
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Sets the trigger type (internal or external) for one camera.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param triggerType Can be either TRIG_STATE_INTERNAL or TRIG_STATE_EXTERNAL
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2TriggerControl( const IpCamList *camInfo, uint32_t triggerType ) {
+ PacketTrigControl tPkt;
+
+ // Initialize the packet
+ tPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ tPkt.hdr.type = htonl(PKTT_TRIGCTRL);
+ tPkt.trig_state = htonl(triggerType);
+
+ if(triggerType == TRIG_STATE_INTERNAL ) {
+ strncpy(tPkt.hdr.hrt, "Int. Trigger", sizeof(tPkt.hdr.hrt));
+ } else {
+ strncpy(tPkt.hdr.hrt, "Ext. Trigger", sizeof(tPkt.hdr.hrt));
+ }
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &tPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &tPkt, sizeof(tPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Sets the permanent serial number and MAC configuration for one camera
+ *
+ * @warning: This command can only be sent once per camera. The changes are permanent.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param serial Is the 32-bit unique serial number to assign (the product ID portion is already fixed)
+ * @param mac Is the 48-bit unique MAC address to assign to the board
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2ConfigureBoard( const IpCamList *camInfo, uint32_t serial, MACAddress *mac ) {
+ PacketSysConfig sPkt;
+
+ // Initialize the packet
+ sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ sPkt.hdr.type = htonl(PKTT_SYSCONFIG);
+
+ strncpy(sPkt.hdr.hrt, "System Config", sizeof(sPkt.hdr.hrt));
+ memcpy(&sPkt.mac, mac, 6);
+ sPkt.serial = htonl(serial);
+
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+ // Wait for response
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Writes to one image sensor I2C register on one camera.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param reg The 8-bit register address to write into
+ * @parm data 16-bit value to write into the register
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2SensorWrite( const IpCamList *camInfo, uint8_t reg, uint16_t data ) {
+ PacketSensorData sPkt;
+
+ // Initialize the packet
+ sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ sPkt.hdr.type = htonl(PKTT_SENSORWR);
+
+ strncpy(sPkt.hdr.hrt, "Write I2C", sizeof(sPkt.hdr.hrt));
+ sPkt.address = reg;
+ sPkt.data = htons(data);
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Reads the value of one image sensor I2C register on one camera.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param reg The 8-bit register address to read from
+ * @parm data Pointer to 16 bits of storage to write the value to
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2SensorRead( const IpCamList *camInfo, uint8_t reg, uint16_t *data ) {
+ PacketSensorRequest rPkt;
+
+ // Initialize the packet
+ rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ rPkt.hdr.type = htonl(PKTT_SENSORRD);
+ rPkt.address = reg;
+ strncpy(rPkt.hdr.hrt, "Read I2C", sizeof(rPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ uint32_t wait_us = STD_REPLY_TIMEOUT;
+ do {
+ if( !wgWaitForPacket(s, PKTT_SENSORDATA, sizeof(PacketSensorData), &wait_us) && (wait_us != 0) ) {
+ PacketSensorData sPkt;
+ if( recvfrom( s, &sPkt, sizeof(PacketSensorData), 0, NULL, NULL ) == -1 ) {
+ perror("SensorRead unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ *data = ntohs(sPkt.data);
+ close(s);
+ return 0;
+ }
+ } while(wait_us > 0);
+
+ debug("Timed out waiting for sensor value\n");
+ close(s);
+ return 1;
+}
+
+/**
+ * Sets the address of one of the four automatically read I2C registers on one camera.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param index The index (0..3) of the register to set
+ * @param reg The 8-bit address of the register to read at position 'index', or I2C_AUTO_REG_UNUSED.
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2SensorSelect( const IpCamList *camInfo, uint8_t index, uint32_t reg ) {
+ PacketSensorSelect sPkt;
+
+ // Initialize the packet
+ sPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ sPkt.hdr.type = htonl(PKTT_SENSORSEL);
+
+ strncpy(sPkt.hdr.hrt, "Select I2C", sizeof(sPkt.hdr.hrt));
+ sPkt.index = index;
+ sPkt.address = htonl(reg);
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &sPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &sPkt, sizeof(sPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Selects one of the 10 pre-programmed imager modes in the specified camera.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param mode The mode number to select, range (0..9)
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2ImagerModeSelect( const IpCamList *camInfo, uint32_t mode ) {
+ PacketImagerMode mPkt;
+
+ // Initialize the packet
+ mPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ mPkt.hdr.type = htonl(PKTT_IMGRMODE);
+
+ mPkt.mode = htonl(mode);
+
+ strncpy(mPkt.hdr.hrt, "Set Mode", sizeof(mPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &mPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &mPkt, sizeof(mPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Provides for manually setting non-standard imager modes. Use only with extreme caution.
+ *
+ * @warning The imager frame size set by I2C must match the one used by the firmware, or system
+ * failure could result.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param horizontal The number of 8-bit image pixels per line of video. Maximum value 752.
+ * @param vertical The number of video lines per frame.
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2ImagerSetRes( const IpCamList *camInfo, uint16_t horizontal, uint16_t vertical ) {
+ PacketImagerSetRes rPkt;
+
+ // Initialize the packet
+ rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ rPkt.hdr.type = htonl(PKTT_IMGRSETRES);
+
+ rPkt.horizontal = htons(horizontal);
+ rPkt.vertical = htons(vertical);
+
+ strncpy(rPkt.hdr.hrt, "Set Res", sizeof(rPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+
+#define MAX_HORIZ_RESOLUTION 752
+#define LINE_NUMBER_MASK 0x3FF
+
+
+int pr2VidReceive( const char *ifName, uint16_t port, size_t height, size_t width, FrameHandler frameHandler, void *userData ) {
+ struct in_addr host_addr;
+ wgIpGetLocalAddr( ifName, &host_addr );
+
+ if( frameHandler == NULL ) {
+ debug("Invalid frame handler, aborting.\n");
+ return 1;
+ }
+
+ debug("pr2VidReceive ready to receive on %s (%s:%u)\n", ifName, inet_ntoa(host_addr), port);
+
+ int s = wgSocketCreate( &host_addr, port );
+ if( s == -1 ) {
+ return -1;
+ }
+
+ /*
+ * The default receive buffer size on a 32-bit Linux machine is only 128kB.
+ * At a burst data rate of ~ 82.6Mbit/s in the 640x480 @30fps, this buffer will fill in ~12.6ms.
+ * With the default Linux scheduler settings, this leaves virtually no time for any other processes
+ * to execute before the buffer overflows. Increasing the buffer reduces the chance of lost
+ * packets.
+ *
+ * The 8MB buffer here is far larger than necessary for most applications and may be reduced with
+ * some experimentation.
+ *
+ * Note that the Linux system command 'sysctl -w net.core.rmem_max=8388608' must be used to enable
+ * receive buffers larger than the kernel default.
+ */
+ size_t bufsize = 16*1024*1024; // 8MB receive buffer.
+
+ if( setsockopt(s, SOL_SOCKET,SO_RCVBUF, &bufsize, sizeof(bufsize)) == -1) {
+ perror("Can't set rcvbuf option");
+ close(s);
+ return -1;
+ }
+
+ socklen_t bufsizesize = sizeof(bufsize);
+ if( getsockopt(s, SOL_SOCKET,SO_RCVBUF, &bufsize, &bufsizesize) == 0) {
+ debug("Receive buffer size is: %i (%i)\n", bufsize, bufsizesize);
+ }
+ else
+ {
+ perror("Can't read receive buffer size");
+ }
+
+ // We receive data one line at a time; lines will be reassembled into this buffer as they arrive
+ uint8_t *frame_buf;
+ frame_buf = malloc(sizeof(uint8_t)*width*height);
+ if(frame_buf == NULL) {
+ perror("Can't malloc frame buffer");
+ close(s);
+ return -1;
+ }
+
+ // Allocate enough storage for the header as well as 'width' bytes of video data
+ PacketVideoLine *vPkt=malloc(sizeof(PacketVideoLine));
+ if(vPkt == NULL) {
+ perror("Can't malloc line packet buffer");
+ close(s);
+ return -1;
+ }
+
+ // Flag to indicate that 'frameInfo.frame_number' has not yet been set
+ bool firstPacket = true;
+
+ // Flag to indicate the current frame is complete (either EOF received or first line of next frame received)
+ bool frameComplete;
+
+ // Flag to indicate that we have set the 'frameInfo.startTime' timeval for this frame.
+ bool frameStartTimeSet;
+
+ // Holds return code from frameHandler
+ int handlerReturn;
+
+ // Counts number of lines received in current frame
+ uint32_t lineCount;
+
+ // Points to an EOF structure for passing to frameHandler;
+ PacketEOF *eof = NULL;
+
+ // Information structure to pass to the frame handler.
+ pr2FrameInfo frameInfo;
+
+ frameInfo.width = width;
+ frameInfo.height = height;
+
+ do {
+ lineCount = 0;
+ frameComplete = false;
+ frameStartTimeSet = false;
+ frameInfo.lastMissingLine = -1;
+ frameInfo.missingLines = 0;
+ frameInfo.shortFrame = false;
+ frameInfo.frame_number = vPkt->header.frame_number+1;
+ // Ensure the buffer is cleared out. Makes viewing short packets less confusing; missing lines will be black.
+ memset(frame_buf, 0, width*height);
+
+ // We detect a dropped EOF by unexpectedly receiving a line from the next frame before getting the EOF from this frame.
+ // After the last frame has been processed, start a fresh frame with the expected line.
+ if( (eof==NULL) && (firstPacket==false) ) {
+ if(vPkt->header.line_number < height ) {
+ memcpy(&(frame_buf[vPkt->header.line_number*width]), vPkt->data, width);
+ lineCount++;
+ }
+ frameInfo.frame_number &= ~0xFFFF;
+ frameInfo.frame_number |= vPkt->header.frame_number;
+ }
+
+ do {
+ // Wait forever for video packets to arrive; could use select() with a timeout here if a timeout is needed
+ struct sockaddr_in fromaddr;
+ struct timeval readtimeout;
+ fd_set set;
+ socklen_t fromaddrlen = sizeof(struct sockaddr_in);
+ fromaddr.sin_family = AF_INET;
+
+ // Wait for either a packet to be received or for timeout
+ handlerReturn = 0;
+ do {
+ readtimeout.tv_sec = 1;
+ readtimeout.tv_usec = 0;
+
+ FD_ZERO(&set);
+ FD_SET(s, &set);
+
+ if( select(s+1, &set, NULL, NULL, &readtimeout) == -1 ) {
+ perror("pr2VidReceive select failed");
+ close(s);
+ return -1;
+ }
+
+ // Call the frame handler with NULL to see if we should bail out.
+ if(! FD_ISSET(s, &set) && errno != EINTR) {
+ debug("Select timed out. Calling handler.");
+ handlerReturn = frameHandler(NULL, userData);
+ if (handlerReturn)
+ break;
+ }
+ } while (! FD_ISSET(s, &set));
+
+ // We timed out, and the handler returned nonzero.
+ if (handlerReturn)
+ break;
+
+ if( recvfrom( s, vPkt, sizeof(HeaderVideoLine)+width, 0, (struct sockaddr *) &fromaddr, &fromaddrlen ) == -1 ) {
+ perror("pr2VidReceive unable to receive from socket");
+ break;
+ }
+
+ // Convert fields to host byte order for easier processing
+ vPkt->header.frame_number = ntohl(vPkt->header.frame_number);
+ vPkt->header.line_number = ntohs(vPkt->header.line_number);
+ vPkt->header.horiz_resolution = ntohs(vPkt->header.horiz_resolution);
+ vPkt->header.vert_resolution = ntohs(vPkt->header.vert_resolution);
+
+ //debug("frame: #%i line: %i\n", vPkt->header.frame_number, 0x3FF & vPkt->header.line_number);
+
+ // Check to make sure the frame number information is consistent within the packet
+ uint16_t temp = (vPkt->header.line_number>>10) & 0x003F;
+ if (((vPkt->header.line_number & IMAGER_MAGICLINE_MASK) == 0) && (temp != (vPkt->header.frame_number & 0x003F))) {
+ debug("Mismatched line/frame numbers: %02X / %02X\n", temp, (vPkt->header.frame_number & 0x003F));
+ }
+
+ // Validate that the frame is the size we expected
+ if( (vPkt->header.horiz_resolution != width) || (vPkt->header.vert_resolution != height) ) {
+ debug("Invalid frame size received: %u x %u, expected %u x %u\n", vPkt->header.horiz_resolution, vPkt->header.vert_resolution, width, height);
+ close(s);
+ return 1;
+ }
+
+ // First time through we need to initialize the number of the first frame we received
+ if(firstPacket == true) {
+ firstPacket = false;
+ frameInfo.frame_number = vPkt->header.frame_number;
+ }
+
+ // Store the start time for the frame.
+ if (!frameStartTimeSet)
+ {
+ gettimeofday(&frameInfo.startTime, NULL);
+ frameStartTimeSet = true;
+ }
+
+ // Check for frames that ended with an error
+ if( (vPkt->header.line_number == IMAGER_LINENO_ERR) ||
+ (vPkt->header.line_number == IMAGER_LINENO_OVF) ) {
+ debug("Video error: %04X\n", vPkt->header.line_number);
+
+ // In the case of these special 'error' EOF packets, there has been a serious internal camera failure
+ // so we will abort rather than try to process the last frame.
+ break;
+ } else if (vPkt->header.line_number == IMAGER_LINENO_ABORT) {
+ debug("Video aborted\n");
+ break; //don't process last frame
+ } else if((vPkt->header.frame_number - frameInfo.frame_number) & 0xFFFF) { // The camera only sends 16 bits of frame number
+ // If we have received a line from the next frame, we must have missed the EOF somehow
+ debug ("Frame #%u missing EOF, got %i lines\n", frameInfo.frame_number, lineCount);
+ frameComplete = true;
+ // EOF was missing
+ eof = NULL;
+ } else if( vPkt->header.line_number == IMAGER_LINENO_EOF ) {
+ // This is a 'normal' (non-error) end of frame packet (line number 0x3FF)
+ frameComplete = true;
+
+ // Handle EOF data fields
+ eof = (PacketEOF *)vPkt;
+
+ // Correct to network byte order for frameHandler
+ eof->ticks_hi = ntohl(eof->ticks_hi);
+ eof->ticks_lo = ntohl(eof->ticks_lo);
+ eof->ticks_per_sec = ntohl(eof->ticks_per_sec);
+
+ // Correct to network byte order for frameHandler
+ eof->i2c_valid = ntohl(eof->i2c_valid);
+ for(int i=0; i<I2C_REGS_PER_FRAME; i++) {
+ eof->i2c[i] = ntohs(eof->i2c[i]);
+ }
+
+ if(lineCount != height) {
+ // Flag packet as being short for the frameHandler
+ eof->header.line_number = IMAGER_LINENO_SHORT;
+ frameInfo.shortFrame = true;
+ }
+ // Move to the next frame
+
+ } else {
+ // Remove extraneous frame information from the line number field
+ vPkt->header.line_number &= LINE_NUMBER_MASK;
+
+ if( vPkt->header.line_number >= vPkt->header.vert_resolution ) {
+ debug("Invalid line number received: %u (max %u)\n", vPkt->header.line_number, vPkt->header.vert_resolution);
+ break;
+ }
+ if (lineCount + frameInfo.missingLines < vPkt->header.line_number)
+ {
+ int missedLines = vPkt->header.line_number - lineCount - frameInfo.missingLines;
+ frameInfo.lastMissingLine = vPkt->header.line_number - 1;
+ debug("Frame #%i missed %i line(s) starting at %i src port %i\n", vPkt->header.frame_number,
+ missedLines, lineCount + frameInfo.missingLines, ntohs(fromaddr.sin_port));
+ frameInfo.missingLines += missedLines;
+ }
+ memcpy(&(frame_buf[vPkt->header.line_number*width]), vPkt->data, width);
+ lineCount++;
+ }
+ } while(frameComplete == false);
+
+ if( frameComplete == true ) {
+ frameInfo.frameData = frame_buf;
+ frameInfo.eofInfo = eof;
+ frameInfo.frame_number = frameInfo.frame_number;
+ handlerReturn = frameHandler(&frameInfo, userData);
+ } else {
+ // We wind up here if a serious error has resulted in us 'break'ing out of the loop.
+ // We won't call frameHandler, we'll just exit directly.
+ handlerReturn = -1;
+ }
+ } while( handlerReturn == 0 );
+
+ close(s);
+ return 0;
+}
Modified: pkg/trunk/sandbox/actionlib/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/actionlib/CMakeLists.txt 2009-07-30 02:55:36 UTC (rev 20117)
+++ pkg/trunk/sandbox/actionlib/CMakeLists.txt 2009-07-30 03:02:41 UTC (rev 20118)
@@ -19,7 +19,11 @@
genmsg()
-rospack_add_executable(move_base_server src/move_base_server.cpp)
+#rospack_add_executable(goal_manager_test src/goal_manager_test.cpp)
+
+#rospack_add_executable(move_base_server src/move_base_server.cpp)
rospack_add_executable(move_base_client src/move_base_client.cpp)
+
+
#add_subdirectory(test)
Added: pkg/trunk/sandbox/actionlib/genaction.py
===================================================================
--- pkg/trunk/sandbox/actionlib/genaction.py (rev 0)
+++ pkg/trunk/sandbox/actionlib/genaction.py 2009-07-30 03:02:41 UTC (rev 20118)
@@ -0,0 +1,125 @@
+#! /usr/bin/python
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+# * Neither the name of the Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Stuart Glaser
+
+import sys
+import cStringIO
+import re
+import roslib, roslib.msgs
+import os, os.path
+
+IODELIM = '---'
+COMMENTCHAR = roslib.msgs.COMMENTCHAR
+
+class ActionSpecException(Exception): pass
+
+def parse_action_spec(text, package_context = ''):
+ pieces = [cStringIO.StringIO()]
+ for l in text.split('\n'):
+ l = l.split(COMMENTCHAR)[0].strip() # Strips comments
+ if l.startswith(IODELIM):
+ pieces.append(cStringIO.StringIO())
+ else:
+ pieces[-1].write(l + '\n')
+ return [p.getvalue() for p in pieces]
+
+def write_file(filename, text):
+ f = open(filename, 'w')
+ f.write(text)
+ f.close()
+
+def main():
+
+ if len(sys.argv) < 2:
+ print "Need to give a package path"
+ sys.exit(1)
+ pkg = os.path.abspath(sys.argv[1])
+
+ if not os.path.exists(os.path.join(pkg, 'manifest.xml')):
+ print "Not a package %s" % pkg
+ sys.exit(1)
+
+ output_dir = os.path.join(pkg, 'msg')
+ if not os.path.exists(output_dir):
+ os.makedirs(output_dir)
+
+ action_dir = os.path.join(pkg, 'action')
+ for action_file in os.listdir(action_dir):
+ if action_file.endswith('.action'):
+ filename = os.path.join(action_dir, action_file)
+
+ f = open(filename)
+ action_spec = f.read()
+ f.close()
+
+ name = os.path.basename(filename)[:-7]
+ print "Generating for action %s" % name
+
+ pieces = parse_action_spec(action_spec)
+ if len(pieces) != 3:
+ raise ActionSpecException("%s: wrong number of pieces, %d"%(filename,len(pieces)))
+ goal, result, feedback = pieces
+
+ action_msg = """
+%sActionGoal action_goal
+%sActionResult action_result
+%sActionFeedback action_feedback
+""" % (name, name, name)
+
+ goal_msg = goal
+ action_goal_msg = """
+Header header
+actionlib/GoalID goal_id
+%sGoal goal
+""" % name
+
+ result_msg = result
+ action_result_msg = """
+Header header
+actionlib/GoalStatus status
+%sResult result
+""" % name
+
+ feedback_msg = feedback
+ action_feedback_msg = """
+Header header
+actionlib/GoalStatus status
+%sFeedback feedback
+""" % name
+
+ write_file(os.path.join(output_dir, "%sAction.msg"%name), action_msg)
+ write_file(os.path.join(output_dir, "%sGoal.msg"%name), goal_msg)
+ write_file(os.path.join(output_dir, "%sActionGoal.msg"%name), action_goal_msg)
+ write_file(os.path.join(output_dir, "%sResult.msg"%name), result_msg)
+ write_file(os.path.join(output_dir, "%sActionResult.msg"%name), action_result_msg)
+ write_file(os.path.join(output_dir, "%sFeedback.msg"%name), feedback_msg)
+ write_file(os.path.join(output_dir, "%sActionFeedback.msg"%name), action_feedback_msg)
+
+
+if __name__ == '__main__': main()
Property changes on: pkg/trunk/sandbox/actionlib/genaction.py
___________________________________________________________________
Added: svn:executable
+ *
Deleted: pkg/trunk/sandbox/actionlib/include/actionlib/action_client.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/action_client.h 2009-07-30 02:55:36 UTC (rev 20117)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/action_client.h 2009-07-30 03:02:41 UTC (rev 20118)
@@ -1,857 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef ACTIONLIB_ACTION_CLIENT_H_
-#define ACTIONLIB_ACTION_CLIENT_H_
-
-#include <list>
-
-#include <boost/thread/recursive_mutex.hpp>
-#include <boost/interprocess/sync/scoped_lock.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include "actionlib/one_shot_timer.h"
-#include "actionlib/goal_id_generator.h"
-#include "actionlib/client_goal_status.h"
-#include "actionlib/one_shot_timer.h"
-#include "actionlib/enclosure_deleter.h"
-
-// Messages
-//#include "actionlib/ActionHeader.h"
-#include "actionlib/GoalStatusArray.h"
-#include "actionlib/RequestType.h"
-
-namespace actionlib
-{
-
-//! \todo figure out why I get compile errors trying to use boost::mutex::scoped_lock()
-class ScopedLock
-{
-public:
- ScopedLock(boost::recursive_mutex& mutex) : mutex_(mutex)
- {
- mutex_.lock();
- }
- ~ScopedLock()
- {
- mutex_.unlock();
- }
-private:
- boost::recursive_mutex& mutex_;
-
-};
-
-
-template<class ActionGoal, class Goal, class ActionResult, class Result, class ActionFeedback, class Feedback>
-class ActionClient
-{
-public:
- // Need forward declaration for typedefs
- class GoalHandle;
-private:
- typedef ActionClient<ActionGoal, Goal, ActionResult, Result, ActionFeedback, Feedback> ActionClientT;
- typedef boost::shared_ptr<const ActionGoal> ActionGoalConstPtr;
- typedef boost::shared_ptr<const Goal> GoalConstPtr;
-
- typedef boost::shared_ptr<const ActionResult> ActionResultConstPtr;
- typedef boost::shared_ptr<const Result> ResultConstPtr;
-
- typedef boost::shared_ptr<const ActionFeedback> ActionFeedbackConstPtr;
- typedef boost::shared_ptr<const Feedback> FeedbackConstPtr;
-
- typedef boost::function<void (const GoalHandle&) > CompletionCallback;
- typedef boost::function<void (const GoalHandle&, const FeedbackConstPtr&) > FeedbackCallback;
-
- class GoalManager
- {
- public:
- //! \brief Builds the goal manager and then sends the goal over the wire
- GoalManager(const GoalID& goal_id, const Goal& goal, CompletionCallback ccb, FeedbackCallback fcb, ActionClientT* ac, const ros::Duration& runtime_timeout);
- void preemptGoal(const ros::Time& preempt_time);
- ClientGoalStatus getStatus();
- ResultConstPtr getResult();
-
- private:
- enum Co...
[truncated message content] |
