Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20478] pkg/trunk
|
From: <bla...@us...> - 2009-08-03 08:13:38
|
Revision: 20478
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20478&view=rev
Author: blaisegassend
Date: 2009-08-03 08:13:29 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
Changed nomenclature in driver_base. Renamed method names to use camelCase.
Got forearm_node working with driver_base: now appears to be working well.
Modified Paths:
--------------
pkg/trunk/drivers/cam/forearm_cam/src/nodes/CMakeLists.txt
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp
pkg/trunk/drivers/generic/dynamic_reconfigure/include/dynamic_reconfigure/reconfigurator.h
pkg/trunk/drivers/generic/dynamic_reconfigure/templates/ConfigReconfigurator.h
pkg/trunk/stacks/hardware_test/self_test/include/self_test/self_test.h
Added Paths:
-----------
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver.h
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver_node.h
Removed Paths:
-------------
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/device.h
pkg/trunk/stacks/driver_common/driver_base/include/driver_base/driver.h
Deleted: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c 2009-08-03 08:13:29 UTC (rev 20478)
@@ -1,1289 +0,0 @@
-#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/drivers/cam/forearm_cam/src/nodes/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/CMakeLists.txt 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/CMakeLists.txt 2009-08-03 08:13:29 UTC (rev 20478)
@@ -1,5 +1,5 @@
rospack_add_executable(forearm_node forearm_node.cpp)
target_link_libraries(forearm_node forearmcam)
-rospack_add_executable(forearm_node_new forearm_node_new.cpp)
-target_link_libraries(forearm_node_new forearmcam)
+#rospack_add_executable(forearm_node_new forearm_node_new.cpp)
+#target_link_libraries(forearm_node_new forearmcam)
Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-08-03 08:13:29 UTC (rev 20478)
@@ -301,7 +301,7 @@
self_test_.addTest( &ForearmNode::resumeTest );
// Set up reconfiguration
- reconfigurator_.set_callback(boost::bind<void>(&ForearmNode::reconfigure, this, _1));
+ reconfigurator_.setCallback(boost::bind<void>(&ForearmNode::reconfigure, this, _1));
// Start up the
diagnostic_thread_ = new boost::thread(boost::bind(&ForearmNode::diagnosticsLoop, this));
@@ -318,9 +318,9 @@
if ((level | dynamic_reconfigure::SensorLevels::RECONFIGURE_CLOSE) == level)
close();
- reconfigurator_.get_config(config_);
+ reconfigurator_.getConfig(config_);
config_update();
- reconfigurator_.set_config(config_);
+ reconfigurator_.setConfig(config_);
if (was_open_)
open();
@@ -1080,7 +1080,7 @@
{
status.name = "Interruption Test";
- if (node_handle_.getNode()->numSubscribers("~image_raw") == 0)
+ if (cam_pub_.publisher().getNumSubscribers() == 0)
{
status.level = 0;
status.message = "No operation interrupted.";
Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node_new.cpp 2009-08-03 08:13:29 UTC (rev 20478)
@@ -61,7 +61,7 @@
#include <boost/tokenizer.hpp>
#include <boost/format.hpp>
#include <driver_base/driver.h>
-#include <driver_base/device.h>
+#include <driver_base/driver_node.h>
#include "fcamlib.h"
#include "host_netutil.h"
@@ -181,9 +181,9 @@
double frame_period_;
};
-class ForearmCamDevice : public driver_base::Device
+class ForearmCamDriver : public driver_base::Driver
{
- friend class ForearmCamDriver;
+ friend class ForearmCamNode;
public:
forearm_cam::ForearmCamConfig config_;
@@ -239,7 +239,7 @@
public:
typedef forearm_cam::ForearmCamReconfigurator Reconfigurator;
- ForearmCamDevice() :
+ ForearmCamDriver() :
camera_(NULL)
{
misfire_blank_ = 0;
@@ -289,13 +289,13 @@
}
}
- ~ForearmCamDevice()
+ ~ForearmCamDriver()
{
close();
}
- void open()
+ void doOpen()
{
assert(state_ == CLOSED);
ROS_DEBUG("open()");
@@ -487,12 +487,12 @@
// Initialize frame time filter.
frameTimeFilter_ = FrameTimeFilter(desired_freq_, 0.001, 0.5 / imager_freq_);
- config_bord_service_ = node_handle_.advertiseService("~board_config", &ForearmCamDevice::boardConfig, this);
+ config_bord_service_ = node_handle_.advertiseService("~board_config", &ForearmCamDriver::boardConfig, this);
state_ = OPENED;
}
- void close()
+ void doClose()
{
ROS_DEBUG("close()");
assert(state_ == OPENED);
@@ -500,18 +500,20 @@
state_ = CLOSED;
}
- void start()
+ void doStart()
{
ROS_DEBUG("start()");
assert(state_ == OPENED);
- image_thread_.reset(new boost::thread(boost::bind(&ForearmCamDevice::imageThread, this, config_.port)));
+ image_thread_.reset(new boost::thread(boost::bind(&ForearmCamDriver::imageThread, this, config_.port)));
state_ = RUNNING;
}
- void stop()
+ void doStop()
{
ROS_DEBUG("stop()");
assert(state_ == RUNNING);
+
+ state_ = OPENED;
if (image_thread_ && !image_thread_->timed_join((boost::posix_time::milliseconds) 2000))
{
@@ -519,8 +521,6 @@
lost_image_thread_count_++;
}
image_thread_.reset();
-
- state_ = OPENED;
}
int setTestMode(uint16_t mode, diagnostic_updater::DiagnosticStatusWrapper &status)
@@ -592,10 +592,10 @@
}
}
frameTimeFilter_.reset_filter();
- ROS_INFO("Camera running.");
+ ROS_INFO("Camera streaming.");
// Receive video
- fcamVidReceive(camera_->ifName, port, height_, width_, &ForearmCamDevice::frameHandler, this);
+ fcamVidReceive(camera_->ifName, port, height_, width_, &ForearmCamDriver::frameHandler, this);
// Stop Triggering
if (!trig_controller_cmd_.empty())
@@ -798,7 +798,7 @@
static int frameHandler(fcamFrameInfo *frameInfo, void *userData)
{
- ForearmCamDevice &fa_node = *(ForearmCamDevice*)userData;
+ ForearmCamDriver &fa_node = *(ForearmCamDriver*)userData;
return fa_node.frameHandler(frameInfo);
}
@@ -913,21 +913,22 @@
};
-class ForearmCamDriver : public driver_base::Driver<ForearmCamDevice>
+class ForearmCamNode : public driver_base::DriverNode<ForearmCamDriver>
{
public:
- ForearmCamDriver(ros::NodeHandle &nh) :
- driver_base::Driver<ForearmCamDevice>(nh),
+ ForearmCamNode(ros::NodeHandle &nh) :
+ driver_base::DriverNode<ForearmCamDriver>(nh),
cam_pub_(node_handle_.advertise<sensor_msgs::Image>("~image_raw", 1),
diagnostic_,
- diagnostic_updater::FrequencyStatusParam(&device_.desired_freq_, &device_.desired_freq_, 0.05),
+ diagnostic_updater::FrequencyStatusParam(&driver_.desired_freq_, &driver_.desired_freq_, 0.05),
diagnostic_updater::TimeStampStatusParam()),
cam_info_pub_(ros::Publisher(),
diagnostic_,
- diagnostic_updater::FrequencyStatusParam(&device_.desired_freq_, &device_.desired_freq_, 0.05),
- diagnostic_updater::TimeStampStatusParam())
+ diagnostic_updater::FrequencyStatusParam(&driver_.desired_freq_, &driver_.desired_freq_, 0.05),
+ diagnostic_updater::TimeStampStatusParam()),
+ calibrated_(false)
{
- device_.useFrame_ = boost::bind(&ForearmCamDriver::publishImage, this, _1, _2, _3, _4);
+ driver_.useFrame_ = boost::bind(&ForearmCamNode::publishImage, this, _1, _2, _3, _4);
}
private:
@@ -954,17 +955,18 @@
return 0;
}
- virtual void reconfigure_hook(int level)
+ virtual void reconfigureHook(int level)
{
+ ROS_DEBUG("ForearmCamNode::reconfigureHook called at level %x", level);
if ((level | dynamic_reconfigure::SensorLevels::RECONFIGURE_CLOSE) == level)
{
- cam_info_.width = device_.width_;
- cam_info_.height = device_.height_;
- image_.header.frame_id = device_.config_.frame_id;
- cam_info_.header.frame_id = device_.config_.frame_id;
+ cam_info_.width = driver_.width_;
+ cam_info_.height = driver_.height_;
+ image_.header.frame_id = driver_.config_.frame_id;
+ cam_info_.header.frame_id = driver_.config_.frame_id;
// Try to load camera intrinsics from flash memory
- calibrated_ = device_.loadIntrinsics(&cam_info_.D[0], &cam_info_.K[0],
+ calibrated_ = driver_.loadIntrinsics(&cam_info_.D[0], &cam_info_.K[0],
&cam_info_.R[0], &cam_info_.P[0]);
if (calibrated_)
ROS_INFO("Loaded intrinsics from camera");
@@ -979,26 +981,31 @@
}
}
- virtual void add_diagnostics()
+ virtual void addDiagnostics()
{
// Set up diagnostics
- diagnostic_.add("Link Status", &device_, &ForearmCamDevice::linkStatus );
+ diagnostic_.add("Link Status", &driver_, &ForearmCamDriver::linkStatus );
}
- virtual void add_running_tests()
+ virtual void addRunningTests()
{
- self_test_.add( "Streaming Test", this, &ForearmCamDriver::streamingTest);
+ self_test_.add( "Streaming Test", this, &ForearmCamNode::streamingTest);
}
- virtual void add_stopped_tests(SelfTest<ForearmCamDevice> st)
+ virtual void addOpenedTests()
{
+ }
+
+ virtual void addStoppedTests()
+ {
+ ROS_DEBUG("Adding forearm camera video mode tests.");
for (int i = 0; i < MT9V_NUM_MODES; i++)
{
- diagnostic_updater::TaskFunction f = boost::bind(&ForearmCamDriver::videoModeTest, this, MT9VModes[i].name, _1);
+ diagnostic_updater::TaskFunction f = boost::bind(&ForearmCamNode::videoModeTest, this, MT9VModes[i].name, _1);
self_test_.add( str(boost::format("Test Pattern in mode %s")%MT9VModes[i].name), f );
}
}
-
+
void streamingTest(diagnostic_updater::DiagnosticStatusWrapper& status)
{
cam_pub_.clear_window();
@@ -1052,46 +1059,46 @@
void videoModeTest(const std::string mode, diagnostic_updater::DiagnosticStatusWrapper& status)
{
- const std::string oldmode = device_.config_.video_mode;
- ForearmCamDevice::UseFrameFunction oldUseFrame = device_.useFrame_;
+ const std::string oldmode = driver_.config_.video_mode;
+ ForearmCamDriver::UseFrameFunction oldUseFrame = driver_.useFrame_;
- device_.config_.video_mode = mode;
- device_.config_update();
+ driver_.config_.video_mode = mode;
+ driver_.config_update();
VideoModeTestFrameHandler callback(status);
- device_.useFrame_ = boost::bind(&VideoModeTestFrameHandler::run, boost::ref(callback), _1, _2, _3, _4);
+ driver_.useFrame_ = boost::bind(&VideoModeTestFrameHandler::run, boost::ref(callback), _1, _2, _3, _4);
status.name = mode + " Pattern Test";
status.summary(0, "Passed"); // If nobody else fills this, then the test passed.
- open();
+ driver_.open();
- if (device_.setTestMode(0x3800, status))
+ if (driver_.setTestMode(0x3800, status))
goto reset_state;
- start();
+ driver_.start();
{
- int oldcount = device_.lost_image_thread_count_;
- stop();
- if (oldcount < device_.lost_image_thread_count_)
+ int oldcount = driver_.lost_image_thread_count_;
+ driver_.stop();
+ if (oldcount < driver_.lost_image_thread_count_)
{
ROS_ERROR("Lost the image_thread. This should never happen.");
status.summary(2, "Lost the image_thread. This should never happen.");
}
}
- close();
+ driver_.close();
- if (device_.setTestMode(0x0000, status))
+ if (driver_.setTestMode(0x0000, status))
goto reset_state;
reset_state:
- close();
- device_.useFrame_ = oldUseFrame;
- device_.config_.video_mode = oldmode;
- device_.config_update();
+ driver_.close();
+ driver_.useFrame_ = oldUseFrame;
+ driver_.config_.video_mode = oldmode;
+ driver_.config_update();
}
};
int main(int argc, char **argv)
{
- return driver_base::main<ForearmCamDriver>(argc, argv, "forearm_camera");
+ return driver_base::main<ForearmCamNode>(argc, argv, "forearm_camera");
}
Modified: pkg/trunk/drivers/generic/dynamic_reconfigure/include/dynamic_reconfigure/reconfigurator.h
===================================================================
--- pkg/trunk/drivers/generic/dynamic_reconfigure/include/dynamic_reconfigure/reconfigurator.h 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/generic/dynamic_reconfigure/include/dynamic_reconfigure/reconfigurator.h 2009-08-03 08:13:29 UTC (rev 20478)
@@ -48,6 +48,11 @@
#include <boost/function.hpp>
#include <ros/node_handle.h>
+/**
+ * @todo Add checks that settings are within range.
+ * @todo Add diagnostics.
+ */
+
namespace dynamic_reconfigure
{
/**
@@ -60,14 +65,16 @@
{
}
- void set_callback(const boost::function<void(int level)> &callback)
+ void setCallback(const boost::function<void(int level)> &callback)
{
callback_ = callback;
if (callback) // At startup we need to load the configuration with all level bits set. (Everything has changed.)
callback(~0);
+ else
+ ROS_INFO("setCallback did not call callback because it was zero."); /// @todo kill this line.
}
- void clear_callback()
+ void clearCallback()
{
callback_.clear();
}
@@ -82,44 +89,44 @@
public:
Reconfigurator(ros::NodeHandle &nh) : node_handle_(nh)
{
- config_ = ConfigManipulator::get_defaults();
- ConfigManipulator::read_from_param_server(node_handle_, config_);
+ config_ = ConfigManipulator::getDefaults();
+ ConfigManipulator::readFromParamServer(node_handle_, config_);
// Write to make sure everything is filled in.
- ConfigManipulator::write_to_param_server(node_handle_, config_);
+ ConfigManipulator::writeToParamServer(node_handle_, config_);
static const std::string get_config = "~get_configuration";
- get_service_ = node_handle_.advertiseService(get_config, &Reconfigurator<ConfigManipulator>::get_config_service, this);
+ get_service_ = node_handle_.advertiseService(get_config, &Reconfigurator<ConfigManipulator>::getConfigService, this);
static const std::string set_config = "~set_configuration";
- set_service_ = node_handle_.advertiseService(set_config, &Reconfigurator<ConfigManipulator>::set_config_service, this);
+ set_service_ = node_handle_.advertiseService(set_config, &Reconfigurator<ConfigManipulator>::setConfigService, this);
}
- void get_config(class ConfigManipulator::ConfigType &config)
+ void getConfig(class ConfigManipulator::ConfigType &config)
{
config = config_;
}
- void set_config(const class ConfigManipulator::ConfigType &config)
+ void setConfig(const class ConfigManipulator::ConfigType &config)
{
config_ = config;
- ConfigManipulator::write_to_param_server(node_handle_, config_);
+ ConfigManipulator::writeToParamServer(node_handle_, config_);
}
private:
- bool get_config_service(typename ConfigManipulator::GetService::Request &req,
+ bool getConfigService(typename ConfigManipulator::GetService::Request &req,
typename ConfigManipulator::GetService::Response &rsp)
{
- rsp.defaults = ConfigManipulator::get_defaults();
- rsp.min = ConfigManipulator::get_min();
- rsp.max = ConfigManipulator::get_max();
+ rsp.defaults = ConfigManipulator::getDefaults();
+ rsp.min = ConfigManipulator::getMin();
+ rsp.max = ConfigManipulator::getMax();
return true;
}
- bool set_config_service(typename ConfigManipulator::SetService::Request &req,
+ bool setConfigService(typename ConfigManipulator::SetService::Request &req,
typename ConfigManipulator::SetService::Response &rsp)
{
- int level = ConfigManipulator::get_change_level(req.config, config_);
+ int level = ConfigManipulator::getChangeLevel(req.config, config_);
- set_config(req.config);
+ setConfig(req.config);
// We expect config_ to be read, and possibly written during the
// callback.
Modified: pkg/trunk/drivers/generic/dynamic_reconfigure/templates/ConfigReconfigurator.h
===================================================================
--- pkg/trunk/drivers/generic/dynamic_reconfigure/templates/ConfigReconfigurator.h 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/drivers/generic/dynamic_reconfigure/templates/ConfigReconfigurator.h 2009-08-03 08:13:29 UTC (rev 20478)
@@ -64,19 +64,19 @@
typedef ${pkgname}::Set${name}Config SetService;
typedef ${pkgname}::${name}Config ConfigType;
- static const ConfigType &get_defaults()
+ static const ConfigType &getDefaults()
{
initialize();
return defaults;
}
- static const ConfigType &get_min()
+ static const ConfigType &getMin()
{
initialize();
return min;
}
- static const ConfigType &get_max()
+ static const ConfigType &getMax()
{
initialize();
return max;
@@ -89,19 +89,19 @@
* \param nh ros::NodeHandle through which to access the name server
* \param config Reference to the configuration to modify
*/
- static void read_from_param_server(ros::NodeHandle &nh, ConfigType &config)
+ static void readFromParamServer(ros::NodeHandle &nh, ConfigType &config)
{
$readparam
#line ${linenum} "${filename}"
}
- static void write_to_param_server(ros::NodeHandle &nh, const ConfigType &config)
+ static void writeToParamServer(ros::NodeHandle &nh, const ConfigType &config)
{
$writeparam
#line ${linenum} "${filename}"
}
- static int get_change_level(const ConfigType &config1, const ConfigType &config2)
+ static int getChangeLevel(const ConfigType &config1, const ConfigType &config2)
{
int changelvl = 0;
$changelvl
Deleted: pkg/trunk/stacks/driver_common/driver_base/include/driver_base/device.h
===================================================================
--- pkg/trunk/stacks/driver_common/driver_base/include/driver_base/device.h 2009-08-03 08:03:45 UTC (rev 20477)
+++ pkg/trunk/stacks/driver_common/driver_base/include/driver_base/device.h 2009-08-03 08:13:29 UTC (rev 20478)
@@ -1,116 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * 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, ...
[truncated message content] |
