Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21006] pkg/trunk
|
From: <bla...@us...> - 2009-08-07 03:48:17
|
Revision: 21006
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21006&view=rev
Author: blaisegassend
Date: 2009-08-07 03:48:05 +0000 (Fri, 07 Aug 2009)
Log Message:
-----------
Finished move of forearm_cam
Modified Paths:
--------------
pkg/trunk/stacks/camera_drivers/forearm_cam/include/forearm_cam/ipcam_packet.h
pkg/trunk/stacks/camera_drivers/forearm_cam/src/fcamlib/fcamlib.c
pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/CMakeLists.txt
pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/set_name.cpp
pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/upload_mcs.cpp
Removed Paths:
-------------
pkg/trunk/drivers/cam/forearm_cam/
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/include/forearm_cam/ipcam_packet.h
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/include/forearm_cam/ipcam_packet.h 2009-08-07 03:37:38 UTC (rev 21005)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/include/forearm_cam/ipcam_packet.h 2009-08-07 03:48:05 UTC (rev 21006)
@@ -26,6 +26,7 @@
#define FLASH_PAGE_SIZE 264
#define FLASH_NAME_PAGENO FLASH_MAX_PAGENO
+#define FLASH_CALIBRATION_PAGENO (FLASH_MAX_PAGENO - 2)
#define CAMERA_NAME_LEN 40
#ifdef LIBRARY_BUILD
@@ -583,8 +584,6 @@
/**
* The PacketAnnounce is generated by the camera in response to a PacketDiscover or PacketConfigure.
* It provides complete information about the camera and the versions of its subcomponents.
- *
- * The system's IP address is not explicitly encoded in the PacketAnnounce since it is present in the IP headers.
*/
typedef struct PACKED_ATTRIBUTE {
/// Generic Command Packet Headers
@@ -596,7 +595,7 @@
uint32_t ser_no; ///< The unique four-byte serial number assigned to this camera
char product_name[40]; ///< The fixed product name assigned to the FCAM camera by Willow Garage. Null terminated string.
char camera_name[40]; ///< The name assigned to this particular camera. Null terminated string.
- IPAddress camera_ip; ///< The currently configured camera name.
+ IPAddress camera_ip; ///< The default power-up IP address for the camera.
/**
* FPGA and system board revision, formatted as follows:
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/fcamlib/fcamlib.c
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/fcamlib/fcamlib.c 2009-08-07 03:37:38 UTC (rev 21005)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/fcamlib/fcamlib.c 2009-08-07 03:48:05 UTC (rev 21006)
@@ -13,7 +13,7 @@
#include "forearm_cam/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 STD_REPLY_TIMEOUT SEC_TO_USEC(0.5)
#define STOP_REPLY_TIMEOUT SEC_TO_USEC(0.001)
#define VMODEDEF(width, height, fps, hblank, vblank) { #width"x"#height"x"#fps, width, height, fps, hblank, vblank }
@@ -515,9 +515,6 @@
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
@@ -531,16 +528,33 @@
perror("fcamConfigure socket creation failed");
return -1;
}
+
+ if (!ipAddress)
+ {
+ cPkt.ip_addr = camInfo->ip;
+
+ if(wgSendUDP(s, &camInfo->ip, &cPkt, sizeof(cPkt)) == -1) {
+ debug("Unable to send packet\n");
+ close(s);
+ return -1;
+ }
+ }
+ else
+ {
+ struct in_addr newIP;
+ inet_aton(ipAddress, &newIP);
+ cPkt.ip_addr = newIP.s_addr;
- if(wgSendUDPBcast(s, camInfo->ifName, &cPkt, sizeof(cPkt)) == -1) {
- debug("Unable to send broadcast\n");
- close(s);
- 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;
+ IPAddress camIP = cPkt.ip_addr;
if( wgSocketConnect(s, &camIP) ) {
close(s);
return -1;
@@ -873,6 +887,8 @@
if (!retval)
return 0;
+
+ debug("Flash read failed.");
}
return retval;
@@ -979,6 +995,7 @@
retval = fcamFlashWrite( camInfo, address, pageDataIn );
if (retval)
{
+ debug("Failed write, retries left: %i.", *retries);
//printf("Failed compare write.\n");
continue;
}
@@ -986,12 +1003,14 @@
retval = fcamReliableFlashRead( camInfo, address, buffer, retries );
if (retval)
{
+ debug("Failed compare read.");
//printf("Failed compare read.\n");
continue;
}
if (!memcmp(buffer, pageDataIn, FLASH_PAGE_SIZE))
return 0;
+ debug("Failed compare.");
//printf("Failed compare.\n");
if (*retries == 0) // In case retries ran out during the read.
@@ -1822,7 +1841,7 @@
return 1;
}
- debug("fcamVidReceive ready to receive on %s (%s:%u)\n", ifName, inet_ntoa(localIp), port);
+ debug("fcamVidReceive ready to receive on %s (%s:%u)\n", camera->ifName, inet_ntoa(localIp), port);
s = wgSocketCreate( &localIp, 0 );
if( s == -1 ) {
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp 2009-08-07 03:37:38 UTC (rev 21005)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp 2009-08-07 03:48:05 UTC (rev 21006)
@@ -199,7 +199,6 @@
ros::ServiceServer config_bord_service_;
// Video mode
-
int video_mode_;
int width_;
int height_;
@@ -225,6 +224,7 @@
// Camera information
std::string hwinfo_;
IpCamList camera_;
+ double last_camera_ok_time_;
// Trigger information
std::string trig_controller_cmd_;
@@ -249,6 +249,7 @@
// fcamCamListInit(&camList);
// Clear statistics
+ last_camera_ok_time_ = 0;
last_image_time_ = 0;
missed_line_count_ = 0;
missed_eof_count_ = 0;
@@ -321,14 +322,24 @@
ROS_WARN("rmem_max is %i. Buffer overflows and packet loss may occur. Minimum recommended value is 20000000. Updates may not take effect until the driver is restarted. See http://pr.willowgarage.com/wiki/errors/Dropped_Frames_and_rmem_max for details.", rmem_max_);
}
- const char *errmsg;
- int findResult = fcamFindByUrl(config_.camera_url.c_str(), &camera_, SEC_TO_USEC(0.2), &errmsg);
+ double thresh_time = ros::Time::now().toSec() - 10;
+ double last_time = last_image_time_ < last_camera_ok_time_ ?
+ last_camera_ok_time_ : last_image_time_;
+ if (last_time < thresh_time)
+ { // Haven't heard from the camera in a while, redo discovery.
+ ROS_DEBUG("Redoing discovery.");
+ const char *errmsg;
+ int findResult = fcamFindByUrl(config_.camera_url.c_str(), &camera_, SEC_TO_USEC(0.2), &errmsg);
- if (findResult)
- {
- ROS_ERROR("Matching URL %s : %s", config_.camera_url.c_str(), errmsg);
- return;
+ if (findResult)
+ {
+ ROS_ERROR("Matching URL %s : %s", config_.camera_url.c_str(), errmsg);
+ return;
+ }
+ retval = fcamConfigure(&camera_, camera_.ip_str, SEC_TO_USEC(0.5));
}
+ else
+ retval = fcamConfigure(&camera_, NULL, SEC_TO_USEC(0.5));
retval = fcamConfigure(&camera_, camera_.ip_str, SEC_TO_USEC(0.5));
if (retval != 0) {
@@ -453,6 +464,7 @@
config_bord_service_ = node_handle_.advertiseService("~board_config", &ForearmCamDriver::boardConfig, this);
state_ = OPENED;
+ last_camera_ok_time_ = ros::Time::now().toSec(); // If we get here we are communicating with the camera well.
}
void doClose()
@@ -562,7 +574,7 @@
fcamVidReceiveAuto( &camera_, height_, width_, &ForearmCamDriver::frameHandler, this);
- // Stop Triggering
+ /*// Stop Triggering
if (!trig_controller_cmd_.empty())
{
ROS_DEBUG("Stopping triggering.");
@@ -575,7 +587,7 @@
// so we don't really care.
ROS_DEBUG("Was not able to stop triggering.");
}
- }
+ } */
/*stop_video:
// Stop video
boost::mutex::scoped_lock lock(mutex_);
@@ -786,12 +798,18 @@
return fa_node.frameHandler(frameInfo);
}
+ uint16_t intrinsicsChecksum(uint16_t *data, int words)
+ {
+ uint16_t sum = 0;
+ for (int i = 0; i < words; i++)
+ sum += htons(data[i]);
+ return htons(0xFFFF - sum);
+ }
+
// TODO: parsing is basically duplicated in prosilica_node
bool loadIntrinsics(double* D, double* K, double* R, double* P)
{
- // FIXME: Hardcoding these until we get a response on flash read/write bug.
- // These values are good for PRF and possibly OK for the other cameras.
-#define FOREARM_FLASH_IS_BUGGY
+//#define FOREARM_FLASH_IS_BUGGY
#ifdef FOREARM_FLASH_IS_BUGGY
static const double D_[] = {-0.34949, 0.13668, 0.00039, -0.00110, 0.00000};
static const double K_[] = {427.31441, 0.00000, 275.80804,
@@ -810,17 +828,25 @@
return true;
#else
// Retrieve contents of user memory
- static const int CALIBRATION_PAGE = 0;
- std::string buffer(FLASH_PAGE_SIZE, '\0');
- if (fcamReliableFlashRead(camera_, CALIBRATION_PAGE, (uint8_t*)&buffer[0], NULL) != 0) {
- ROS_WARN("Flash read error");
+ std::string calbuff(2 * FLASH_PAGE_SIZE, 0);
+
+ if(fcamReliableFlashRead(&camera_, FLASH_CALIBRATION_PAGENO, (uint8_t *) &calbuff[0], NULL) != 0 ||
+ fcamReliableFlashRead(&camera_, FLASH_CALIBRATION_PAGENO+1, (uint8_t *) &calbuff[FLASH_PAGE_SIZE], NULL) != 0)
+ {
+ ROS_ERROR("Error reading camera intrinsics.\n");
return false;
}
+ uint16_t chk = intrinsicsChecksum((uint16_t *) &calbuff[0], calbuff.length() / 2);
+ if (chk)
+ {
+ ROS_WARN("Camera intrinsics have a bad checksum. They have probably never been set.\n");
+ }
+
// Separate into lines
typedef boost::tokenizer<boost::char_separator<char> > Tok;
boost::char_separator<char> sep("\n");
- Tok tok(buffer, sep);
+ Tok tok(calbuff, sep);
// Check "header"
Tok::iterator iter = tok.begin();
@@ -913,6 +939,7 @@
calibrated_(false)
{
driver_.useFrame_ = boost::bind(&ForearmCamNode::publishImage, this, _1, _2, _3, _4);
+ driver_.setPostOpenHook(boost::bind(&ForearmCamNode::postOpenHook, this));
}
private:
@@ -948,23 +975,26 @@
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_ = 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");
- else
- ROS_WARN("Failed to load intrinsics from camera");
-
- // Receive frames through callback
- if (calibrated_)
- cam_info_pub_.set_publisher(node_handle_.advertise<sensor_msgs::CameraInfo>("~cam_info", 1));
- else
- cam_info_pub_.set_publisher(ros::Publisher());
}
}
+ void postOpenHook()
+ {
+ // Try to load camera intrinsics from flash memory
+ 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");
+ else
+ ROS_WARN("Failed to load intrinsics from camera");
+
+ // Receive frames through callback
+ if (calibrated_)
+ cam_info_pub_.set_publisher(node_handle_.advertise<sensor_msgs::CameraInfo>("~cam_info", 1));
+ else
+ cam_info_pub_.set_publisher(ros::Publisher());
+ }
+
virtual void addDiagnostics()
{
// Set up diagnostics
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/CMakeLists.txt 2009-08-07 03:37:38 UTC (rev 21005)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/CMakeLists.txt 2009-08-07 03:48:05 UTC (rev 21006)
@@ -1,6 +1,9 @@
rospack_add_executable(access_register access_register.cpp)
target_link_libraries(access_register forearmcam)
+rospack_add_executable(set_calibration set_calibration.cpp)
+target_link_libraries(set_calibration forearmcam)
+
rospack_add_executable(set_name set_name.cpp)
target_link_libraries(set_name forearmcam)
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/set_name.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/set_name.cpp 2009-08-07 03:37:38 UTC (rev 21005)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/set_name.cpp 2009-08-07 03:48:05 UTC (rev 21006)
@@ -58,7 +58,7 @@
int read_name(IpCamList *camera)
{
- unsigned char namebuff[FLASH_PAGE_SIZE];
+ uint8_t namebuff[FLASH_PAGE_SIZE];
IdentityFlashPage *id = (IdentityFlashPage *) &namebuff;
if(fcamReliableFlashRead(camera, FLASH_NAME_PAGENO, (uint8_t *) namebuff, NULL) != 0)
@@ -70,7 +70,7 @@
uint16_t chk = checksum((uint16_t *) namebuff);
if (chk)
{
- fprintf(stderr, "Previous camera name had bad checksum. Error: %04x\n", chk);
+ fprintf(stderr, "Previous camera name had bad checksum.\n");
}
id->cam_name[sizeof(id->cam_name) - 1] = 0;
@@ -83,7 +83,7 @@
int write_name(IpCamList *camera, char *name, char *new_ip)
{
- unsigned char namebuff[FLASH_PAGE_SIZE];
+ uint8_t namebuff[FLASH_PAGE_SIZE];
IdentityFlashPage *id = (IdentityFlashPage *) &namebuff;
if (strlen(name) > sizeof(id->cam_name) - 1)
@@ -111,7 +111,7 @@
fprintf(stderr, "Success! Restarting camera, should take about 10 seconds to come back up after this.\n");
- fcamReconfigureFPGA(camera);
+ fcamReset(camera);
return 0;
}
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/upload_mcs.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/upload_mcs.cpp 2009-08-07 03:37:38 UTC (rev 21005)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/utilities/upload_mcs.cpp 2009-08-07 03:48:05 UTC (rev 21006)
@@ -267,7 +267,7 @@
}
int addr = page * FLASH_PAGE_SIZE;
- int startretries = 1;
+ int startretries = 10;
int retries = startretries;
if (fcamReliableFlashWrite(&camera, page, (uint8_t *) firmware + addr, &retries) != 0)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
