Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20636] pkg/trunk
|
From: <bla...@us...> - 2009-08-04 08:13:54
|
Revision: 20636
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20636&view=rev
Author: blaisegassend
Date: 2009-08-04 08:13:45 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Improved automatic documentation generation in dynamic_reconfigure.
Converted the forearm_cam node to URL-based camera naming.
Modified Paths:
--------------
pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg
pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch
pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h
pkg/trunk/drivers/cam/forearm_cam/manifest.xml
pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/stacks/driver_common/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py
Added Paths:
-----------
pkg/trunk/drivers/cam/forearm_cam/dox/
pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox
Modified: pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/cfg/ForearmCam.cfg 2009-08-04 08:13:45 UTC (rev 20636)
@@ -39,23 +39,20 @@
from dynamic_reconfigure.msg import SensorLevels
from dynamic_reconfigure.parameter_generator import *
-gen = ParameterGenerator(PACKAGE, "ForearmCam")
+gen = ParameterGenerator(PACKAGE, "forearm_node", "ForearmCam")
# Name Type Reconfiguration level Description Default Min Max
-gen.add("exit_on_fault", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Indicates if the driver should exit when an error occurs.", False)
-gen.add("if_name", str_t, SensorLevels.RECONFIGURE_CLOSE, "The network interface to which the camera is connected. Use \"auto\" for automatic.", "auto")
-gen.add("ip_address", str_t, SensorLevels.RECONFIGURE_CLOSE, "The ip address to which the camera will be configured. Use \"auto\" for automatic.", "auto")
+#gen.add("exit_on_fault", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Indicates if the driver should exit when an error occurs.", False)
gen.add("auto_gain", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the analog gain to automatic. Causes the @b gain setting to be ignored.", True)
gen.add("gain", int_t, SensorLevels.RECONFIGURE_CLOSE, "The camera analog gain.", 32, 16, 64)
-gen.add("port", int_t, SensorLevels.RECONFIGURE_CLOSE, "The local port the camera should connect to. Defaults to 9090.", 9090)
gen.add("ext_trig", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Set camera to trigger from the external trigger input.", False)
gen.add("video_mode", str_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the camera video mode.", "640x480x30")
gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the TF frame from which the camera is publishing.", "")
gen.add("trig_controller", str_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the trigger controller from which an externally trigged camera operates.", "")
gen.add("trig_rate", double_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the triggering rate in externally triggered mode.", 30)
gen.add("trig_phase", int_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the phase of the external trigger signal.", 0, 0, 1)
-gen.add("serial_number", int_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the serial number to use. Use -1 for automatic. The default of -2 will always fail.", -2)
gen.add("first_packet_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "Offset between the end of exposure and the minimal arrival time for the first frame packet.", 0.0025, 0)
gen.add("auto_exposure", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the camera exposure duration to automatic. Causes the @b exposure setting to be ignored.", True)
gen.add("exposure", double_t, SensorLevels.RECONFIGURE_CLOSE, "Sets the camera exposure The valid range depends on the video mode.", 0, 0.01)
+gen.add("camera_url", str_t, SensorLevels.RECONFIGURE_CLOSE, "URL defining which camera to connect to, on what interface and with which IP address.", 'any://')
exit(gen.generate())
Added: pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox (rev 0)
+++ pkg/trunk/drivers/cam/forearm_cam/dox/doc.dox 2009-08-04 08:13:45 UTC (rev 20636)
@@ -0,0 +1,118 @@
+/**
+
+
+@section topic Forearm Camera ROS Topics
+ - forearm_node
+
+ */
+
+
+/**
+\mainpage
+\htmlinclude manifest.html
+
+The forearm camera is an ethernet camera located in the forearm of the PR2.
+This page describes the API of the camera.
+
+A detailed specification of the camera can be found here:
+http://pr.willowgarage.com/wiki/Ethernet_Forearm_Camera
+
+The forearm_cam package documentation with tutorials and background
+information can be found here:
+http://pr.willowgarage.com/wiki/forearm_cam
+
+The foream camera can be used in triggered mode. Detailed documentation of
+the triggered mode can be found here:
+http://pr.willowgarage.com/wiki/camera_synchronization
+
+\subsection urls Forearm Camera URLs
+
+Forearm cameras and their network configuration are described by URLs in
+the following form:
+
+\verbatim
+name://camera_name[@camera_ip][#local_interface]
+serial://serial_number[@camera_ip][#local_interface]
+any://[@camera_ip][#local_interface]
+\endverbatim
+
+A \b name URL indicates the name of the camera to be found. A \b serial URL
+indicates the serial number of the camera to be found. An \b any URL will
+match any camera. If a URL matches more than one camera, the node will
+refuse to start the camera.
+
+Optionally, the ip address that the camera should be set to can be
+specified by prefixing it with a @ sign, and the interface through
+which to access the camera can be specified by prefixing it with a #
+sign.
+
+Some example URLs:
+
+\verbatim
+any:// // Matches any camera
+serial://15#eth2 // Matches the camera with s/n 15, and only looks for it on interface eth2
+name://left_forearm@10.68.0.210 // Matches the \b left_forearm camera, and sets its IP to 10.68.0.210
+\endverbatim
+
+\subsection forearm_node forearm_node
+
+The forearm camera driver. Connects to the camera and streams out images.
+
+\include ForearmCamConfig-usage.dox
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- none
+
+Publishes to:
+- \b "~image_raw": [sensor_msgs/Image] The raw image stream from the
+ camera.
+
+- \b "~cam_info": [sensor_msgs/CameraInfo] The camera's calibration
+ information.
+
+\include ForearmCamConfig.dox
+
+\section commandline Command-line tools
+
+- discover Finding cameras on the network
+
+\subsection discover discover
+
+A tool to \b discover cameras that are visible from the computer on which it
+is run.
+
+The \b discover tool broadcasts on one or all interfaces to discover
+cameras that are reachable. Its output indicates the camera's serial
+number, name (if it has one), MAC address, interface, currently configured
+IP address, and revision information.
+
+Notes:
+
+* If a camera is visible from multiple network interfaces, only the first one
+that responds will be shown.
+
+* The IP address returned by the discover tool is the camera's currently
+configured address. This may not be a legal address on the current
+network. This may not be the address stored in the camera's flash. This
+is in order of precedence: the last address that the camera was
+configured with, the address stored in the camera's flash memory the last
+time the camera was rebooted, the default address 167.254.0.1.
+
+* Cameras on a network interface that is not properly configured for
+IPv4 traffic will not be seen by the discover tool.
+
+\subsubsection Usage
+
+To discover all cameras that are visible:
+\verbatim
+rosrun forearm_cam discover
+\endverbatim
+
+To discover all cameras that are visible on interface \b eth1:
+\verbatim
+rosrun forearm_cam discover eth1
+\endverbatim
+
+*/
Modified: pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/forearm_cam.launch 2009-08-04 08:13:45 UTC (rev 20636)
@@ -1,10 +1,11 @@
<launch>
<group ns="forearm">
- <param name="if_name" type="str" value=""/>
- <param name="ip_address" type="str" value="10.0.1.123"/>
- <param name="serial_number" type="int" value="-1"/>
+ <!--param name="if_name" type="str" value=""/-->
+ <!--param name="ip_address" type="str" value="10.0.1.123"/-->
+ <!--param name="serial_number" type="int" value="-1"/-->
+ <!--param name="frame_id" type="str" value="???"/-->
+ <param name="camera_url" type="str" value="name://Camera13@10.0.1.123"/>
<param name="video_mode" type="str" value="640x480x30"/>
- <!--param name="frame_id" type="str" value="???"/-->
<param name="do_colorize" type="bool" value="True"/>
<param name="do_rectify" type="bool" value="True"/>
<param name="ext_trigger" type="bool" value="False"/>
Modified: pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/include/fcamlib.h 2009-08-04 08:13:45 UTC (rev 20636)
@@ -74,6 +74,7 @@
int fcamLibVersion( void );
+int fcamFindByUrl(const char *url, IpCamList *camera, unsigned wait_us, const char **errmsg);
int fcamDiscover(const char *ifName, IpCamList *ipCamList, const char *ipAddress, unsigned wait_us);
int fcamConfigure( IpCamList *camInfo, const char *ipAddress, unsigned wait_us);
int fcamStartVid( const IpCamList *camInfo, const uint8_t mac[6], const char *ipAddress, uint16_t port );
Modified: pkg/trunk/drivers/cam/forearm_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/manifest.xml 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/manifest.xml 2009-08-04 08:13:45 UTC (rev 20636)
@@ -1,6 +1,7 @@
<package>
<description>A library and ROS node to provide access to the forearm camera from MindTribe.</description>
- <author>Patrick Mihelich, Eric MacIntosh, David Palchak, Blaise Gassend</author>
+ <author>Blaise Gassend, Patrick Mihelich, Eric MacIntosh, David Palchak</author>
+ <url>http://pr.willowgarage.com/wiki/forearm_cam</url>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
Modified: pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/prf_forearm_cam_config.launch 2009-08-04 08:13:45 UTC (rev 20636)
@@ -1,8 +1,6 @@
<launch>
<group ns="forearm_cam_r">
- <param name="if_name" type="str" value="eth0"/>
- <param name="ip_address" type="str" value="10.68.0.68"/>
- <param name="serial_number" type="int" value="3"/>
+ <param name="camera_url" type="str" value="serial://3@10.68.0.68"/>
<param name="video_mode" type="str" value="640x480x30"/>
<param name="frame_id" type="str" value="r_forearm_cam_optical_frame"/>
<param name="do_colorize" type="bool" value="True"/>
Modified: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/fcamlib.c 2009-08-04 08:13:45 UTC (rev 20636)
@@ -42,6 +42,170 @@
/**
+ * Finds a camera matching the given name
+ *
+ * Names are of the form:
+\verbatim
+name://camera_name[@camera_ip][#local_interface]
+serial://serial_number[@camera_ip][#local_interface]
+any://[@camera_ip][#local_interface]
+\endverbatim
+ *
+ * A name URL indicates the name of the camera to be found. A serial URL
+ * indicates the serial number of the camera to be found. An any URL will
+ * match any camera, but if more than one camera is found, it will fail.
+ *
+ * Optionally, the ip address that the camera should be set to can be
+ * specified by prefixing it with a @ sign, and the interface through
+ * which to access the camera can be specified by prefixing it with a #
+ * sign.
+ *
+ * @param url The url of the camera to find
+ * @param camera The structure to fill the camera information into
+ * @param wait_us The number of microseconds to wait before timing out
+ * @param errmsg String containing a descriptive parse error message
+ *
+ * @return Returns 1 if a camera was found, 2 if multiple cameras were found,
+ * 0 if no cameras were found
+ * and -1 with errno set if a system errors occured.
+ */
+int fcamFindByUrl(const char *url, IpCamList *camera, unsigned wait_us, const char **errmsg)
+{
+ static const char *badprefix = "Bad URL prefix, expected name://, serial:// or any://.";
+ static const char *multiaddress = "Multiple @-prefixed camera addresses found.";
+ static const char *multiinterface = "Multiple #-prefixed host interfaces found.";
+ static const char *discoverfailed = "Discover failed. The specified address or interface may be bad.";
+ static const char *badserial = "serial:// may only be followed by an integer.";
+ static const char *badip = "@ must be followed by a valid IP address.";
+ const char *name = "name://"; int namelen = strlen(name);
+ const char *serial = "serial://"; int seriallen = strlen(serial);
+ const char *any = "any://"; int anylen = strlen(any);
+ char *idstart;
+ char *curpos;
+ char *address = NULL;
+ char *interface = NULL;
+ char *copy = malloc(strlen(url));
+ int retval = -1;
+ int mode = 0;
+ unsigned int serialnum = -1;
+ IpCamList camList;
+ IpCamList *curEntry;
+
+ fcamCamListInit(&camList); // Must happen before any potential failures.
+
+ errmsg = NULL;
+
+ if (!copy)
+ return -1;
+
+ strcpy(copy, url);
+
+ // Decypher the prefix.
+ if (!strncmp(copy, name, namelen))
+ {
+ idstart = copy + namelen;
+ mode = 1;
+ }
+ else if (!strncmp(copy, serial, seriallen))
+ {
+ idstart = copy + seriallen;
+ mode = 2;
+ }
+ else if (!strncmp(copy, any, anylen))
+ {
+ idstart = copy + anylen;
+ mode = 3;
+ }
+ else
+ {
+ *errmsg = badprefix;
+ goto bailout;
+ }
+
+ // Find any # or @ words.
+ for (curpos = idstart; *curpos; curpos++)
+ {
+ if (*curpos == '@')
+ {
+ if (address)
+ {
+ *errmsg = multiaddress;
+ goto bailout;
+ }
+ address = curpos + 1;
+ }
+ else if (*curpos == '#')
+ {
+ if (interface)
+ {
+ *errmsg = multiinterface;
+ goto bailout;
+ }
+ interface = curpos + 1;
+ }
+ else
+ continue; // Didn't find anything, don't terminate the string.
+ *curpos = 0;
+ }
+ // Now idstart, address and interface are set.
+
+ if (mode == 2) // serial:// convert the number to integer.
+ {
+ char *endpos;
+ serialnum = strtol(idstart, &endpos, 10);
+ if (*idstart == 0 || *endpos != 0)
+ {
+ *errmsg = badserial;
+ goto bailout;
+ }
+ }
+
+ // Build up a list of cameras. Only ones that are on the specified
+ // interface (if specified), and that can reply from the specified
+ // address (if specified) will end up in the list.
+ if (fcamDiscover(interface, &camList, address, wait_us) == -1)
+ {
+ *errmsg = discoverfailed;
+ goto bailout;
+ }
+
+ // Now search through the list for a match.
+ retval = 0; // From now on retval is the number of matches.
+ list_for_each_entry(curEntry, &(camList.list), list)
+ {
+ if ((mode == 1 && strcmp(idstart, curEntry->cam_name) == 0) ||
+ (mode == 2 && serialnum == curEntry->serial) ||
+ mode == 3)
+ {
+ retval++;
+ if (retval == 2)
+ {
+ goto bailout;
+ }
+ memcpy(camera, curEntry, sizeof(IpCamList));
+ if (address) // If an address has been specified, we fill it into the camera info.
+ {
+ struct in_addr ip;
+ if (inet_aton(address, &ip))
+ camera->ip = ip.s_addr;
+ else
+ {
+ fprintf(stderr, "Camera IP address is %s.", address);
+ *errmsg = badip;
+ retval = -1;
+ goto bailout;
+ }
+ }
+ }
+ }
+
+bailout:
+ fcamCamListDelAll(&camList);
+ free(copy);
+ return retval;
+}
+
+/**
* Waits for a FCAM 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'
@@ -1583,7 +1747,7 @@
return -1;
}
- fcamVidReceiveSocket( s, height, width, frameHandler, userData);
+ return fcamVidReceiveSocket( s, height, width, frameHandler, userData);
}
int fcamVidReceiveAuto( IpCamList *camera, size_t height, size_t width, FrameHandler frameHandler, void *userData ) {
@@ -1626,7 +1790,7 @@
}
port = ntohs(((struct sockaddr_in *)&localPort)->sin_port);
- fprintf(stderr, "Streaming to port %i.\n", port);
+// fprintf(stderr, "Streaming to port %i.\n", port);
if ( fcamStartVid( camera, (uint8_t *)&(localMac.sa_data[0]), inet_ntoa(localIp), port) != 0 )
{
Modified: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/host_netutil.c 2009-08-04 08:13:45 UTC (rev 20636)
@@ -50,7 +50,7 @@
strncpy(arp.arp_dev, camInfo->ifName, sizeof(arp.arp_dev));
if( ioctl(s, SIOCSARP, &arp) == -1 ) {
- perror("Warning, was unable to create ARP entry (are you root?)");
+ //perror("Warning, was unable to create ARP entry (are you root?)");
return -1;
} else {
debug("Camera %u successfully configured\n", camInfo->serial);
Modified: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/list.c 2009-08-04 08:13:45 UTC (rev 20636)
@@ -166,5 +166,4 @@
list_del(pos);
free(tmpListItem);
}
- return 0;
}
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-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-08-04 08:13:45 UTC (rev 20636)
@@ -221,9 +221,10 @@
// Camera information
std::string hwinfo_;
- int serial_number_;
- IpCamList* camera_;
- IpCamList camList;
+ IpCamList camera_;
+ std::string interface_;
+ std::string address_;
+// IpCamList camList;
// Trigger information
std::string trig_controller_cmd_;
@@ -240,13 +241,12 @@
public:
typedef forearm_cam::ForearmCamReconfigurator Reconfigurator;
- ForearmCamDriver() :
- camera_(NULL)
+ ForearmCamDriver()
{
misfire_blank_ = 0;
// Create a new IpCamList to hold the camera list
- fcamCamListInit(&camList);
+// fcamCamListInit(&camList);
// Clear statistics
last_image_time_ = 0;
@@ -272,12 +272,10 @@
height_ = MT9VModes[video_mode_].height;
imager_freq_ = MT9VModes[video_mode_].fps;
- // Distinguish between serial_number_ and config_.serial_number_ so
+ // Distinguish between camera_.serial and config_.camera_.serial so
// that when auto is selected, we will try to restart the previous
// camera before probing again.
- serial_number_ = config_.serial_number;
-
desired_freq_ = config_.ext_trig ? config_.trig_rate : imager_freq_;
if (!config_.auto_exposure && config_.exposure > 1 / desired_freq_)
@@ -295,7 +293,7 @@
~ForearmCamDriver()
{
- fcamCamListDelAll(&camList);
+// fcamCamListDelAll(&camList);
close();
}
@@ -318,10 +316,10 @@
}
// Forget results of previous discovery.
- camera_ = NULL;
- fcamCamListDelAll(&camList);
+// camera_ = NULL;
+// fcamCamListDelAll(&camList);
- // Discover any connected cameras, wait for 0.5 second for replies
+/* // Discover any connected cameras, wait for 0.5 second for replies
if( fcamDiscover(config_.if_name.c_str(), &camList, config_.ip_address.c_str(), SEC_TO_USEC(0.5)) == -1) {
ROS_FATAL("Discover error");
//node_handle_.shutdown();
@@ -335,7 +333,7 @@
// Open camera with requested serial number.
int index = -1;
- if (serial_number_ == -1) // Auto
+ if (camera_.serial == -1) // Auto
{
if (fcamCamListNumEntries(&camList) == 1)
{
@@ -346,15 +344,15 @@
ROS_FATAL("Camera autodetection only works when exactly one camera is discoverable. Unfortunately, we found %i cameras.", fcamCamListNumEntries(&camList));
}
}
- else if (serial_number_ == -2) // Nothing specified
+ else if (camera_.serial == -2) // Nothing specified
{
ROS_FATAL("No camera serial_number was specified. Specifying a serial number is now mandatory to avoid accidentally configuring a random camera elsewhere in the building. You can specify -1 for autodetection.");
}
else
{
- index = fcamCamListFind(&camList, serial_number_);
+ index = fcamCamListFind(&camList, camera_.serial);
if (index == -1) {
- ROS_FATAL("Couldn't find camera with S/N %i", serial_number_);
+ ROS_FATAL("Couldn't find camera with S/N %i", camera_.serial);
}
}
@@ -365,14 +363,40 @@
for (int i = 0; i < fcamCamListNumEntries(&camList); i++)
{
camera_ = fcamCamListGetEntry(&camList, i);
- ROS_FATAL("Found camera with S/N #%u", camera_->serial);
+ ROS_FATAL("Found camera with S/N #%u", camera_.serial);
}
return;
+ }*/
+
+ const char *errmsg;
+ int findResult = fcamFindByUrl(config_.camera_url.c_str(), &camera_, SEC_TO_USEC(0.1), &errmsg);
+ uint8_t *ip = (uint8_t *) &camera_.ip;
+ switch (findResult)
+ {
+ case 0:
+ ROS_ERROR("No camera matching %s was found.", config_.camera_url.c_str());
+ return;
+
+ case 1:
+ interface_ = camera_.ifName;
+ address_ = (boost::format("%i.%i.%i.%i")%(int)ip[0]%(int)ip[1]%(int)ip[2]%(int)ip[3]).str();
+ break;
+
+ case 2:
+ ROS_ERROR("More than one camera matched %s. Use the discover tool to determine a non-unique url for the camera.", config_.camera_url.c_str());
+ return;
+
+ case -1:
+ ROS_ERROR("Camera search failed: %s", errmsg);
+ return;
+
+ default:
+ ROS_ERROR("fcamFindByUrl returned illegal return value. This should never happen and is a bug.");
}
// Configure the camera with its IP address, wait up to 500ms for completion
- camera_ = fcamCamListGetEntry(&camList, index);
- retval = fcamConfigure(camera_, config_.ip_address.c_str(), SEC_TO_USEC(0.5));
+// camera_ = fcamCamListGetEntry(&camList, index);
+ retval = fcamConfigure(&camera_, address_.c_str(), SEC_TO_USEC(0.5));
if (retval != 0) {
if (retval == ERR_CONFIG_ARPFAIL) {
ROS_WARN("Unable to create ARP entry (are you root?), continuing anyway");
@@ -381,27 +405,27 @@
return;
}
}
- ROS_INFO("Configured camera, S/N #%u, IP address %s, name %s",
- camera_->serial, config_.ip_address.c_str(), camera_->cam_name);
+ camera_.serial = camera_.serial;
+ hwinfo_ = camera_.hwinfo;
+
+ ROS_INFO("Configured camera. Complete URLs: serial://%u@%s#%s name://%s@%s#%s",
+ camera_.serial, address_.c_str(), interface_.c_str(), camera_.cam_name, address_.c_str(), interface_.c_str());
- serial_number_ = camera_->serial;
- hwinfo_ = camera_->hwinfo;
-
// We are going to receive the video on this host, so we need our own MAC address
- if ( wgEthGetLocalMac(camera_->ifName, &localMac_) != 0 ) {
- ROS_FATAL("Unable to get local MAC address for interface %s", camera_->ifName);
+ if ( wgEthGetLocalMac(camera_.ifName, &localMac_) != 0 ) {
+ ROS_FATAL("Unable to get local MAC address for interface %s", camera_.ifName);
return;
}
// We also need our local IP address
- if ( wgIpGetLocalAddr(camera_->ifName, &localIp_) != 0) {
- ROS_FATAL("Unable to get local IP address for interface %s", camera_->ifName);
+ if ( wgIpGetLocalAddr(camera_.ifName, &localIp_) != 0) {
+ ROS_FATAL("Unable to get local IP address for interface %s", camera_.ifName);
return;
}
// Select trigger mode.
- if ( fcamTriggerControl( camera_, config_.ext_trig ? TRIG_STATE_EXTERNAL : TRIG_STATE_INTERNAL ) != 0) {
- ROS_FATAL("Trigger mode set error. Is %s accessible from interface %s? (Try running route to check.)", config_.ip_address.c_str(), config_.if_name.c_str());
+ if ( fcamTriggerControl( &camera_, config_.ext_trig ? TRIG_STATE_EXTERNAL : TRIG_STATE_INTERNAL ) != 0) {
+ ROS_FATAL("Trigger mode set error. Is %s accessible from interface %s? (Try running route to check.)", address_.c_str(), interface_.c_str());
return;
}
@@ -431,7 +455,7 @@
}
// Select a video mode
- if ( fcamImagerModeSelect( camera_, video_mode_ ) != 0) {
+ if ( fcamImagerModeSelect( &camera_, video_mode_ ) != 0) {
ROS_FATAL("Mode select error");
//node_handle_.shutdown();
return;
@@ -439,12 +463,12 @@
int bin_size = width_ > 320 ? 1 : 2;
// Set horizontal blanking
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_HORIZONTAL_BLANKING, MT9VModes[video_mode_].hblank * bin_size, NULL ) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_HORIZONTAL_BLANKING, MT9VModes[video_mode_].hblank * bin_size, NULL ) != 0)
{
ROS_WARN("Error setting horizontal blanking.");
}
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_VERTICAL_BLANKING, MT9VModes[video_mode_].vblank, NULL) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_VERTICAL_BLANKING, MT9VModes[video_mode_].vblank, NULL) != 0)
{
ROS_WARN("Error setting vertical blanking.");
}
@@ -458,14 +482,14 @@
}
*/
- if (fcamReliableSensorWrite(camera_, MT9V_REG_AGC_AEC_ENABLE, config_.auto_gain * 2 + config_.auto_exposure, NULL) != 0)
+ if (fcamReliableSensorWrite(&camera_, MT9V_REG_AGC_AEC_ENABLE, config_.auto_gain * 2 + config_.auto_exposure, NULL) != 0)
{
ROS_WARN("Error setting AGC/AEC mode. Exposure and gain may be incorrect.");
}
if (!config_.auto_gain)
{
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_ANALOG_GAIN, config_.gain, NULL) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_ANALOG_GAIN, config_.gain, NULL) != 0)
{
ROS_WARN("Error setting analog gain.");
}
@@ -483,7 +507,7 @@
if (explines > 32767) /// @TODO warning here?
explines = 32767;
ROS_DEBUG("Setting exposure lines to %i.", explines);
- if ( fcamReliableSensorWrite( camera_, MT9V_REG_TOTAL_SHUTTER_WIDTH, explines, NULL) != 0)
+ if ( fcamReliableSensorWrite( &camera_, MT9V_REG_TOTAL_SHUTTER_WIDTH, explines, NULL) != 0)
{
ROS_WARN("Error setting exposure.");
}
@@ -509,7 +533,7 @@
{
ROS_DEBUG("start()");
assert(state_ == OPENED);
- image_thread_.reset(new boost::thread(boost::bind(&ForearmCamDriver::imageThread, this, config_.port)));
+ image_thread_.reset(new boost::thread(boost::bind(&ForearmCamDriver::imageThread, this)));
state_ = RUNNING;
}
@@ -530,7 +554,7 @@
int setTestMode(uint16_t mode, diagnostic_updater::DiagnosticStatusWrapper &status)
{
- if ( fcamReliableSensorWrite( camera_, 0x7F, mode, NULL ) != 0) {
+ if ( fcamReliableSensorWrite( &camera_, 0x7F, mode, NULL ) != 0) {
status.summary(2, "Could not set imager into test mode.");
status.adds("Writing imager test mode", "Fail");
return 1;
@@ -542,7 +566,7 @@
usleep(100000);
uint16_t inmode;
- if ( fcamReliableSensorRead( camera_, 0x7F, &inmode, NULL ) != 0) {
+ if ( fcamReliableSensorRead( &camera_, 0x7F, &inmode, NULL ) != 0) {
status.summary(2, "Could not read imager mode back.");
status.adds("Reading imager test mode", "Fail");
return 1;
@@ -567,11 +591,11 @@
std::string getID()
{
- return (boost::format("FCAM%05u") % serial_number_).str();
+ return (boost::format("FCAM%05u") % camera_.serial).str();
}
private:
- void imageThread(int port)
+ void imageThread()
{
// Start video
if (!trig_controller_cmd_.empty())
@@ -600,9 +624,9 @@
return;
}
// Receive video
- fcamVidReceive(camera_->ifName, port, height_, width_, &ForearmCamDriver::frameHandler, this);*/
+ fcamVidReceive(camera_.ifName, port, height_, width_, &ForearmCamDriver::frameHandler, this);*/
- fcamVidReceiveAuto(camera_, height_, width_, &ForearmCamDriver::frameHandler, this);
+ fcamVidReceiveAuto( &camera_, height_, width_, &ForearmCamDriver::frameHandler, this);
// Stop Triggering
if (!trig_controller_cmd_.empty())
@@ -657,21 +681,24 @@
stat.addv("Missing EOF frames", missed_eof_count_);
stat.addv("Losses of image thread", lost_image_thread_count_);
stat.addv("First packet offset", config_.first_packet_offset);
- stat.adds("Interface", config_.if_name);
- stat.adds("Camera IP", config_.ip_address);
if (isClosed())
{
- stat.adds("Camera Serial #", "<not opened>");
- stat.adds("Camera Name", "<not opened>");
- stat.adds("Camera Hardware", "<not opened>");
- stat.adds("Camera MAC", "<not opened>");
+ static const std::string not_opened = "not_opened";
+ stat.adds("Camera Serial #", not_opened);
+ stat.adds("Camera Name", not_opened);
+ stat.adds("Camera Hardware", not_opened);
+ stat.adds("Camera MAC", not_opened);
+ stat.adds("Interface", not_opened);
+ stat.adds("Camera IP", not_opened);
}
else
{
- stat.adds("Camera Serial #", serial_number_);
- stat.adds("Camera Name", camera_->cam_name);
+ stat.adds("Camera Serial #", camera_.serial);
+ stat.adds("Camera Name", camera_.cam_name);
stat.adds("Camera Hardware", hwinfo_);
- stat.addsf("Camera MAC", "%02X:%02X:%02X:%02X:%02X:%02X", camera_->mac[0], camera_->mac[1], camera_->mac[2], camera_->mac[3], camera_->mac[4], camera_->mac[5]);
+ stat.addsf("Camera MAC", "%02X:%02X:%02X:%02X:%02X:%02X", camera_.mac[0], camera_.mac[1], camera_.mac[2], camera_.mac[3], camera_.mac[4], camera_.mac[5]);
+ stat.adds("Interface", interface_);
+ stat.adds("Camera IP", address_);
}
stat.adds("Image mode", config_.video_mode);
stat.addsf("Latest frame time", "%f", last_image_time_);
@@ -920,7 +947,7 @@
mac[i] = req.mac[i];
ROS_INFO("board_config called s/n #%i, and MAC %02x:%02x:%02x:%02x:%02x:%02x.", req.serial, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
stop();
- rsp.success = !fcamConfigureBoard(camera_, req.serial, &mac);
+ rsp.success = !fcamConfigureBoard( &camera_, req.serial, &mac);
if (rsp.success)
ROS_INFO("board_config succeeded.");
Modified: pkg/trunk/stacks/driver_common/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py
===================================================================
--- pkg/trunk/stacks/driver_common/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py 2009-08-04 07:23:59 UTC (rev 20635)
+++ pkg/trunk/stacks/driver_common/dynamic_reconfigure/src/dynamic_reconfigure/parameter_generator.py 2009-08-04 08:13:45 UTC (rev 20636)
@@ -77,12 +77,13 @@
'bool' : False,
}
- def __init__(self, pkgname, name):
+ def __init__(self, pkgname, nodename, name):
self.parameters = []
self.pkgname = pkgname
self.pkgpath = roslib.packages.get_pkg_dir(pkgname)
self.dynconfpath = roslib.packages.get_pkg_dir("dynamic_reconfigure")
self.name = name
+ self.nodename = nodename
self.msgname = name+"Config"
def add(self, name, type, level, description, default = None, min = None, max = None):
@@ -134,21 +135,38 @@
self.generatesetsrv()
self.generategetsrv()
self.generatedoc()
+ self.generateusage()
def generatedoc(self):
self.mkdir("dox")
f = open(os.path.join(self.pkgpath, "dox", self.msgname+".dox"), 'w')
- print >> f, "/**"
- print >> f, "\\subsection parameters ROS parameters"
+ #print >> f, "/**"
+ print >> f, "\\subsubsection parameters ROS parameters"
print >> f
print >> f, "Reads and maintains the following parameters on the ROS server"
print >> f
for param in self.parameters:
- print >> f, Template("- \b \"~$name\" : \b [$type] $description min: $min, default: $default, max: $max").substitute(param)
+ print >> f, Template("- \\b \"~$name\" : \\b [$type] $description min: $min, default: $default, max: $max").substitute(param)
print >> f
- print >> f, "*/"
+ #print >> f, "*/"
f.close()
+ def generateusage(self):
+ self.mkdir("dox")
+ f = open(os.path.join(self.pkgpath, "dox", self.msgname+"-usage.dox"), 'w')
+ #print >> f, "/**"
+ print >> f, "\\subsubsection usage Usage"
+ print >> f, '\\verbatim'
+ print >> f, Template('<node name="$nodename" pkg="$pkgname" type="$nodename">').\
+ substitute(pkgname = self.pkgname, nodename = self.nodename)
+ for param in self.parameters:
+ print >> f, Template(' <param name="$name" type="$type" value="$default" />').substitute(param)
+ print >> f, '</node>'
+ print >> f, '\\endverbatim'
+ print >> f
+ #print >> f, "*/"
+ f.close()
+
def crepr(self, param, val):
type = param["type"]
if type == 'str':
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
