Menu
▾
▴
personalrobots-commit — Subversion commit messages
You can subscribe to this list here.
| 2008 |
Jan
|
Feb
|
Mar
(43) |
Apr
(196) |
May
(316) |
Jun
(518) |
Jul
(1293) |
Aug
(1446) |
Sep
(930) |
Oct
(1271) |
Nov
(1275) |
Dec
(1385) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2009 |
Jan
(1622) |
Feb
(1546) |
Mar
(1236) |
Apr
(1512) |
May
(1782) |
Jun
(1551) |
Jul
(2300) |
Aug
(3088) |
Sep
(452) |
Oct
|
Nov
|
Dec
|
|
From: <sf...@us...> - 2009-09-04 19:47:32
|
Revision: 23855
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23855&view=rev
Author: sfkwc
Date: 2009-09-04 19:47:26 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for visualization
Added Paths:
-----------
stacks/visualization/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:47:14
|
Revision: 23854
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23854&view=rev
Author: sfkwc
Date: 2009-09-04 19:47:08 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for visualization_common
Added Paths:
-----------
stacks/visualization_common/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:46:52
|
Revision: 23853
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23853&view=rev
Author: sfkwc
Date: 2009-09-04 19:46:46 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for sound_drivers
Added Paths:
-----------
stacks/sound_drivers/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:46:31
|
Revision: 23852
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23852&view=rev
Author: sfkwc
Date: 2009-09-04 19:46:25 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for slam_gmapping
Added Paths:
-----------
stacks/slam_gmapping/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:45:52
|
Revision: 23851
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23851&view=rev
Author: sfkwc
Date: 2009-09-04 19:45:46 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for simulator_stage
Added Paths:
-----------
stacks/simulator_stage/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:44:52
|
Revision: 23850
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23850&view=rev
Author: sfkwc
Date: 2009-09-04 19:44:46 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for robot_model
Added Paths:
-----------
stacks/robot_model/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:44:35
|
Revision: 23849
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23849&view=rev
Author: sfkwc
Date: 2009-09-04 19:44:29 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for opencv
Added Paths:
-----------
stacks/opencv/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:44:17
|
Revision: 23848
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23848&view=rev
Author: sfkwc
Date: 2009-09-04 19:44:10 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for navigation
Added Paths:
-----------
stacks/navigation/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:43:53
|
Revision: 23847
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23847&view=rev
Author: sfkwc
Date: 2009-09-04 19:43:47 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for laser_pipeline
Added Paths:
-----------
stacks/laser_pipeline/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:43:33
|
Revision: 23846
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23846&view=rev
Author: sfkwc
Date: 2009-09-04 19:43:23 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for joystick_drivers
Added Paths:
-----------
stacks/joystick_drivers/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:43:06
|
Revision: 23845
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23845&view=rev
Author: sfkwc
Date: 2009-09-04 19:43:00 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for image_common
Added Paths:
-----------
stacks/image_common/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:42:52
|
Revision: 23844
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23844&view=rev
Author: sfkwc
Date: 2009-09-04 19:42:46 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for geometry
Added Paths:
-----------
stacks/geometry/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:42:37
|
Revision: 23843
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23843&view=rev
Author: sfkwc
Date: 2009-09-04 19:42:31 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for common_msgs
Added Paths:
-----------
stacks/common_msgs/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-09-04 19:42:21
|
Revision: 23842
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23842&view=rev
Author: sfkwc
Date: 2009-09-04 19:42:16 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
copying trunk to latest for common
Added Paths:
-----------
stacks/common/tags/latest/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-09-04 19:09:51
|
Revision: 23841
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23841&view=rev
Author: vijaypradeep
Date: 2009-09-04 19:09:42 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Adding unittests for the LaserCbDetector
Modified Paths:
--------------
pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt
pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp
Added Paths:
-----------
pkg/trunk/stacks/calibration/laser_cb_detector/test/data/
pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png
pkg/trunk/stacks/calibration/laser_cb_detector/test/laser_cb_detector_unittest.cpp
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp 2009-09-04 18:35:50 UTC (rev 23840)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp 2009-09-04 19:09:42 UTC (rev 23841)
@@ -106,7 +106,7 @@
for (unsigned int j=0; j < config_.num_x; j++)
{
result.object_points[i*config_.num_x + j].x = j*config_.spacing_x;
- result.object_points[i*config_.num_y + j].y = j*config_.spacing_y;
+ result.object_points[i*config_.num_y + j].y = i*config_.spacing_y;
result.object_points[i*config_.num_y + j].z = 0.0;
}
}
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt 2009-09-04 18:35:50 UTC (rev 23840)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/test/CMakeLists.txt 2009-09-04 19:09:42 UTC (rev 23841)
@@ -4,3 +4,7 @@
rospack_add_gtest(test/cv_laser_bridge_unittest cv_laser_bridge_unittest.cpp)
target_link_libraries(test/cv_laser_bridge_unittest laser_cb_detector)
+
+
+rospack_add_gtest(test/laser_cb_detector_unittest laser_cb_detector_unittest.cpp)
+target_link_libraries(test/laser_cb_detector_unittest laser_cb_detector)
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp 2009-09-04 18:35:50 UTC (rev 23840)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/test/cv_laser_bridge_unittest.cpp 2009-09-04 19:09:42 UTC (rev 23841)
@@ -41,6 +41,8 @@
using namespace std;
using namespace laser_cb_detector;
+static const bool DEBUG=false;
+
calibration_msgs::DenseLaserSnapshot buildSnapshot(double start_range, double start_intensity, double increment,
const unsigned int H, const unsigned int W)
{
@@ -96,7 +98,8 @@
EXPECT_EQ(image->height, (int) NUM_SCANS);
EXPECT_EQ(image->width, (int) RAYS_PER_SCAN);
- displayImage(image);
+ if (DEBUG)
+ displayImage(image);
// Check the first and last pixel in the image
EXPECT_EQ( (int) *((unsigned char*)(image->imageData)+0), 0);
@@ -118,7 +121,8 @@
EXPECT_EQ(image->height, (int) NUM_SCANS);
EXPECT_EQ(image->width, (int) RAYS_PER_SCAN);
- displayImage(image);
+ if (DEBUG)
+ displayImage(image);
// Check to make sure some of the pixels saturated near the beginning and end of the range
EXPECT_EQ( (int) *((unsigned char*)(image->imageData)+5), 0);
Added: pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/test/data/cb_3x4.png 2009-09-04 19:09:42 UTC (rev 23841)
@@ -0,0 +1,4 @@
+\x89PNG
+
+ |
|
From: <wg...@us...> - 2009-09-04 18:36:05
|
Revision: 23840
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23840&view=rev
Author: wgdking
Date: 2009-09-04 18:35:50 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
EML posix driver now supports split txandrx calls, and uses seperate recv thread.
pr2_etherCAT now runs non-realtime thread to collect out-of-band diagnositcs data from slave devices.
pr2_etherCAT pubishes more diagnostics data for master and slave devices.
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/CMakeLists.txt
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_device.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/wg014.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ek1122.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg014.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/wg0x.cpp
Added Paths:
-----------
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_com.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_com.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_device.cpp
Modified: pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp 2009-09-04 18:27:37 UTC (rev 23839)
+++ pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp 2009-09-04 18:35:50 UTC (rev 23840)
@@ -41,6 +41,7 @@
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
+#include <pthread.h>
#include <diagnostic_updater/DiagnosticStatusWrapper.h>
#include <pr2_mechanism_control/mechanism_control.h>
@@ -144,6 +145,20 @@
return double(n.tv_nsec) / NSEC_PER_SEC + n.tv_sec;
}
+
+void *diagnosticLoop(void *args)
+{
+ EthercatHardware *ec((EthercatHardware *) args);
+ struct timespec tick;
+ clock_gettime(CLOCK_MONOTONIC, &tick);
+ while (!g_quit) {
+ ec->collectDiagnostics();
+ tick.tv_sec += 1;
+ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &tick, NULL);
+ }
+ return NULL;
+}
+
void *controlLoop(void *)
{
ros::NodeHandle node;
@@ -188,6 +203,15 @@
// Publish one-time before entering real-time to pre-allocate message vectors
publishDiagnostics(publisher);
+ //Start Non-realtime diagonostic thread
+ int rv;
+ static pthread_t diagnosticThread;
+ if ((rv = pthread_create(&diagnosticThread, NULL, diagnosticLoop, &ec)) != 0)
+ {
+ ROS_FATAL("Unable to create control thread: rv = %d\n", rv);
+ ROS_BREAK();
+ }
+
// Set to realtime scheduler for this thread
struct sched_param thread_param;
int policy = SCHED_FIFO;
@@ -242,6 +266,8 @@
}
ec.update(false, true);
+ //pthread_join(diagnosticThread, 0);
+
publisher.stop();
ros::shutdown();
Modified: pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch
===================================================================
--- pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch 2009-09-04 18:27:37 UTC (rev 23839)
+++ pkg/trunk/stacks/pr2_ethercat_drivers/eml/eml-r36.patch 2009-09-04 18:35:50 UTC (rev 23840)
@@ -52,15 +52,306 @@
===================================================================
--- include/ethercat/arch-RTnet/ethercat/ethercat_log.h (revision 36)
+++ include/ethercat/arch-RTnet/ethercat/ethercat_log.h (working copy)
-@@ -42,7 +42,6 @@
+@@ -42,7 +42,7 @@
#define EC_LOG_FATAL 0
// Redefine this function if not using eCos...
-#define log(level, args...) \
-printf(args)
-+#define ec_log(level, args...)
++#define ec_log(level, args...) \
++ if (level <= EC_LOG_FATAL) fprintf(stderr, args)
#endif // __ethercat_log_h__
+Index: include/ethercat/arch-RTnet/ethercat/netif.h
+===================================================================
+--- include/ethercat/arch-RTnet/ethercat/netif.h (revision 36)
++++ include/ethercat/arch-RTnet/ethercat/netif.h (working copy)
+@@ -39,6 +39,7 @@
+ struct EtherCAT_Frame;
+ struct netif;
+
++#include <stdint.h>
+ #include <stdlib.h>
+
+ #define ETHERCAT_DEVICE_NAME_MAX_LENGTH 256
+@@ -50,20 +51,90 @@
+ #define externC extern "C"
+ #else
+
+-typedef int bool;
+-#define false 0
+-#define true 1
+-
+ #define externC
+ #endif
+
++
++typedef char BOOL;
++#define FALSE 0
++#define TRUE 1
++
++
+ externC void if_attach(struct netif * netif);
+ externC int framedump(const struct EtherCAT_Frame * frame, unsigned char * buffer, size_t bufferlength);
+ externC int framebuild(struct EtherCAT_Frame * frame, const unsigned char * buffer);
+
+
++// Number of outstanding packets to keep track of
++#define PKT_LIST_SIZE 128
+
++// Number of buffers to hold recieved packets
++#define BUF_LIST_SIZE 16
+
++// Should be < number of RX buffers
++#define MAX_UNCLAIMED_PACKETS (BUF_LIST_SIZE-1)
++
++
++// buffer to hold recieved packets
++struct pkt_buf {
++ // True if the buffer is being used
++ BOOL is_free;
++ // buffer for to store ethernet message
++ // more than enough to hold any type of message
++ unsigned char data[2000];
++};
++
++// record of packet that has been sent but.
++// 1. has not received reply
++// .... OR ....
++// 2. has not been claimed by higher level software
++struct outstanding_pkt {
++ // whether this record is holding an outstanding packet or not
++ BOOL is_free;
++
++ // pointer to received packet buffer
++ // If this is NULL if the packet has not been received
++ struct pkt_buf *buf;
++
++ // The source MAC address is used to keep track of
++ // which packet belongs where
++ u_int8_t ether_shost[MAC_ADDRESS_SIZE];
++
++ // EtherCAT_Frame that was use to generate the sent packet...
++ // The exact same frame must be used to pickup the packet
++ struct EtherCAT_Frame * frame;
++
++ // To allow synchronization between input thread and rx function
++ //pthread_mutex_t rx_mut;
++ //pthread_mutexattr_t rx_attr;
++ pthread_cond_t rx_cond;
++
++ // Temember when packet was sent
++ struct timespec tx_time;
++};
++
++struct netif_counters
++{
++ uint64_t sent;
++ uint64_t received;
++ uint64_t collected;
++ uint64_t dropped;
++ uint64_t tx_error;
++ uint64_t tx_net_down;
++ uint64_t tx_full;
++ uint64_t rx_runt_pkt;
++ uint64_t rx_not_ecat;
++ uint64_t rx_other_eml;
++ uint64_t rx_bad_index;
++ uint64_t rx_bad_seqnum;
++ uint64_t rx_dup_seqnum;
++ uint64_t rx_dup_pkt;
++ uint64_t rx_bad_order;
++ uint64_t rx_late_pkt; // Count of all late packets
++ uint64_t rx_late_pkt_rtt_us; // Round trip time (in microseconds) of last late packet arrival
++ uint64_t rx_late_pkt_rtt_us_sum; // Sum of rtt's of all late packets (for calculating average)
++};
++
+ /// Generic ethercat interface towards lower level drivers.
+ /** It should be readily re-implemented for different OSes such as
+ RTAI, linux, ... etc. (For the ease of porting the interface
+@@ -75,12 +146,68 @@
+ packages/io/eth/current/src/ethercatmaster/eth_drv.c
+ and mapped in eth_drv_init()
+ */
+- bool (* txandrx)(struct EtherCAT_Frame * frame, struct netif * netif);
++ BOOL (* txandrx)(struct EtherCAT_Frame * frame, struct netif * netif);
+
++ /// Transmit and receive an EtherCAT frame - only attempt to send
++ /// frame once
++ BOOL (* txandrx_once)(struct EtherCAT_Frame * frame, struct netif * netif);
++
++
++ /// Transmit frame
++ /** Negative return value is an error code, positive and zero is a
++ handle
++ */
++ int (* tx)(struct EtherCAT_Frame * frame, struct netif * netif);
++
++
++ /// Recieve frame
++ /** May block, returns true if correct frame is recieved
++ */
++ BOOL (* rx)(struct EtherCAT_Frame * frame, struct netif * netif, int handle);
++
++ /// Recieve frame
++ /** Will not block, returns true if correct frame is recieved
++ */
++ BOOL (* rx_nowait)(struct EtherCAT_Frame * frame, struct netif * netif, int handle);
++
+ /// The MAC address
+ unsigned char hwaddr[MAC_ADDRESS_SIZE];
+ /// Filedescriptor of the socket
+ int socket_private;
++
++ /// Counters for certain types of events (packet drops, invalid packets, etc)
++ struct netif_counters counters;
++
++ /// Sequence number to put on next sent packet
++ unsigned tx_seqnum;
++
++ /// Secuence number of more recently recieved packet
++ unsigned rx_seqnum;
++
++ /// Outstanding pkt slot to use from next tx
++ unsigned next_pkt_index;
++
++ /// List of outstanding packets
++ /// (packets that have been tx'ed but not rx'ed)
++ struct outstanding_pkt pkt_list[PKT_LIST_SIZE];
++
++ // Number of tx'd packets that have not been rx'd yet
++ unsigned unclaimed_packets;
++
++ /// List of buffers used for packet reception
++ struct pkt_buf buf_list[BUF_LIST_SIZE];
++
++ // For thread safety: txandrx() can be called from multiple threads...
++ pthread_mutex_t txandrx_mut;
++ pthread_mutexattr_t txandrx_attr;
++
++ // For input thread -- if it is used
++ pthread_t input_thread;
++ volatile BOOL stop;
++ volatile BOOL is_stopped;
++
++ // Timeout for recieve in microseconds.
++ int timeout_us;
+ };
+
+ #endif // __netif_h__
+Index: include/dll/ethercat_dll.h
+===================================================================
+--- include/dll/ethercat_dll.h (revision 36)
++++ include/dll/ethercat_dll.h (working copy)
+@@ -53,11 +53,26 @@
+ */
+ void attach(struct netif * netif);
+
+- /// transmit and receive EtherCAT frame (blocking call!)
++ /// transmit and receive EtherCAT frame (blocking call!)
+ /** @param a_frame ethercat frame to be sent
++ * NOTE that txandrx will retry sending lost frames.
+ */
+ bool txandrx(EtherCAT_Frame * a_frame);
+
++ /// transmit an EtherCAT frame (non-blocking call)
++ /** @param a_frame ethercat frame to be sent
++ * @return positive or zero handle on success, negative value for error
++ * Successfull tx MUST be followed by rx call.
++ */
++ int tx(EtherCAT_Frame * a_frame);
++
++ /// receive an EtherCAT frame previously sent by tx() (blocking call!)
++ /** @param a_frame ethercat frame to be sent
++ * @param a_handle handle previously returned by tx()
++ * @return false for error/dropped packet
++ */
++ bool rx(EtherCAT_Frame * a_frame, int a_handle);
++
+ /// Destructor
+ /** @todo some kind of smart pointer concept necessary for all these
+ singletons. For EtherCAT master on ecos not really an issue...
+Index: include/dll/ethercat_device_addressed_telegram.h
+===================================================================
+--- include/dll/ethercat_device_addressed_telegram.h (revision 36)
++++ include/dll/ethercat_device_addressed_telegram.h (working copy)
+@@ -134,6 +134,29 @@
+ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
+ };
+
++/// Auto Increment Physical Read and Write Telegram (APRW)
++class APRW_Telegram : public Device_Addressing_Telegram {
++ public:
++ /// Constructor
++ /** @param a_idx index
++ @param a_adp auto increment address
++ @param a_ado physical memory address
++ @param a_wkc working counter
++ @param a_datalen data_length
++ @param a_data data to be read and written
++ */
++ APRW_Telegram(EC_USINT a_idx, EC_UINT a_adp,
++ EC_UINT a_ado,EC_UINT a_wkc,
++ EC_UINT a_datalen,
++ const unsigned char * a_data);
++
++ virtual ~APRW_Telegram();
++
++ protected:
++ virtual unsigned char * dump_command_field(unsigned char * a_buffer) const;
++ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
++};
++
+ /// Broadcast Write Telegram (BWR)
+ class BWR_Telegram : public Device_Addressing_Telegram {
+ public:
+@@ -203,7 +226,32 @@
+ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
+ };
+
++/// Node Addressed Physical Read Write Telegram (NPRW)
++/** @note older versions of the spec and the ethereal plugin use FPRW
++ for this type
++*/
++class NPRW_Telegram : public Device_Addressing_Telegram {
++ public:
++ /// Constructor
++ /** @param a_idx index
++ @param a_adp slave address
++ @param a_ado physical memory address
++ @param a_wkc working counter
++ @param a_datalen data_length
++ @param a_data data to be written
++ */
++ NPRW_Telegram(EC_USINT a_idx, EC_UINT a_adp,
++ EC_UINT a_ado,EC_UINT a_wkc,
++ EC_UINT a_datalen,
++ const unsigned char * a_data);
++
++ virtual ~NPRW_Telegram();
+
++ protected:
++ virtual unsigned char * dump_command_field(unsigned char * a_buffer) const;
++ virtual const unsigned char * build_command_field(const unsigned char * a_buffer);
++};
++
+ /// Node Addressed Physical Read Telegram (NPRD)
+ /** @note older versions of the spec and the ethereal plugin use FPWR
+ for this type
+Index: include/dll/ethercat_telegram.h
+===================================================================
+--- include/dll/ethercat_telegram.h (revision 36)
++++ include/dll/ethercat_telegram.h (working copy)
+@@ -78,6 +78,9 @@
+ ETHERCAT_TELEGRAM_HEADER_SIZE +
+ ETHERCAT_TELEGRAM_WKC_SIZE; }
+
++ /// attach telegram to this one - ordering is somewhat arbitrary
++ void attach(EC_Telegram *a_telegram);
++
+ /// Pointer to next telegram
+ EC_Telegram * next;
+ /// Pointer to previous telegram
Index: include/dll/ethercat_slave_memory.h
===================================================================
--- include/dll/ethercat_slave_memory.h (revision 36)
@@ -137,6 +428,30 @@
msg_priority = 0x03;
}
}
+Index: include/al/ethercat_process_data.h
+===================================================================
+--- include/al/ethercat_process_data.h (revision 36)
++++ include/al/ethercat_process_data.h (working copy)
+@@ -88,10 +88,15 @@
+ /// See note in class definition.
+ unsigned int m_is_running;
+
+- /// Telegram to be sent
+- LRW_Telegram m_lrw_telegram;
+- /// EtherCAT frame to be sent
+- EC_Ethernet_Frame m_lrw_frame;
++ /// Process data can be divided over MAX_CHUCKS packets
++ /// of upto CHUNK_SIZE bytes
++ static const unsigned MAX_CHUNKS=4;
++ static const unsigned CHUNK_SIZE=1486;
++
++ /// Telegram(s) to be sent
++ LRW_Telegram *m_lrw_telegram[MAX_CHUNKS];
++ /// EtherCAT frame(s) to be sent
++ EC_Ethernet_Frame *m_lrw_frame[MAX_CHUNKS];
+ };
+
+ #endif
Index: include/al/ethercat_slave_handler.h
===================================================================
--- include/al/ethercat_slave_handler.h (revision 36)
@@ -176,7 +491,15 @@
===================================================================
--- src/dll/ethercat_telegram.cxx (revision 36)
+++ src/dll/ethercat_telegram.cxx (working copy)
-@@ -144,7 +144,7 @@
+@@ -33,6 +33,7 @@
+
+ #include "dll/ethercat_telegram.h"
+ #include "ethercat/ethercat_log.h"
++#include <assert.h>
+
+ // Number of bits of the Reserved and NEXT field
+ static const EC_UINT ETHERCAT_LEN_NUM_BITS = 11;
+@@ -144,7 +145,7 @@
if (index == m_idx)
return true;
else{
@@ -185,7 +508,7 @@
return false;
}
}
-@@ -168,14 +168,14 @@
+@@ -168,14 +169,14 @@
nextbit = lennext & NEXT_BIT;
if ( ((nextbit == NEXT_BIT) && (next == NULL)) ||
((nextbit == 0x0000) && (next != NULL)) ){
@@ -202,6 +525,25 @@
return false;
}
return true;
+@@ -185,3 +186,18 @@
+ // irq field is currently unused
+ const EC_UINT EC_Telegram::m_irq = 0x0000;
+
++
++/** Attach a single a_telegram after this telegram in chain
++ */
++void EC_Telegram::attach(EC_Telegram *a_telegram)
++{
++ assert(this != a_telegram);
++ assert(a_telegram->next == NULL);
++ assert(a_telegram->previous == NULL);
++ a_telegram->next = this->next;
++ a_telegram->previous = this;
++ if (this->next != NULL) {
++ this->next->previous = a_telegram;
++ }
++ this->next = a_telegram;
++}
Index: src/dll/ethercat_slave_memory.cxx
===================================================================
--- src/dll/ethercat_slave_memory.cxx (revision 36)
@@ -266,7 +608,7 @@
===================================================================
--- src/dll/ethercat_dll.cxx (revision 36)
+++ src/dll/ethercat_dll.cxx (working copy)
-@@ -68,7 +68,7 @@
+@@ -68,10 +68,26 @@
{
bool succeed = m_if->txandrx(a_frame, m_if);
if (!succeed)
@@ -275,6 +617,105 @@
return succeed;
}
++int EtherCAT_DataLinkLayer::tx(EtherCAT_Frame * a_frame)
++{
++ int handle = m_if->tx(a_frame, m_if);
++ if (handle < 0)
++ ec_log(EC_LOG_INFO, "DLL::tx Error\n");
++ return handle;
++}
++
++bool EtherCAT_DataLinkLayer::rx(EtherCAT_Frame * a_frame, int a_handle)
++{
++ bool succeed = m_if->rx(a_frame, m_if, a_handle);
++ if (!succeed)
++ ec_log(EC_LOG_INFO, "DLL::rx Error\n");
++ return succeed;
++}
++
+ void EtherCAT_DataLinkLayer::attach(struct netif * netif)
+ {
+ m_if = netif;
+Index: src/dll/ethercat_device_addressed_telegram.cxx
+===================================================================
+--- src/dll/ethercat_device_addressed_telegram.cxx (revision 36)
++++ src/dll/ethercat_device_addressed_telegram.cxx (working copy)
+@@ -42,7 +42,9 @@
+ static const EC_USINT NPRD = 0x04; // Node addressed Physical read
+ static const EC_USINT APWR = 0x02; // Autoincrement Physical write
+ static const EC_USINT NPWR = 0x05; // Node addressed Physical write
+-static const EC_USINT ARMW = 0x0d; // Autoincrement Physical Read
++static const EC_USINT APRW = 0x03; // Autoincrement Physical read_write
++static const EC_USINT NPRW = 0x06; // Node addressed Physical read-write
++static const EC_USINT ARMW = 0x0d; // Autoincrement Physical write
+ // Multiple write
+ static const EC_USINT BRD = 0x07; // Broadcast Read
+ static const EC_USINT BWR = 0x08; // Broadcast Write
+@@ -137,6 +139,30 @@
+ }
+
+ // --------------------------------------------------
++// Auto Increment Physical Read Write Telegram
++// --------------------------------------------------
++APRW_Telegram::APRW_Telegram(EC_USINT a_idx, EC_UINT a_adp, EC_UINT a_ado, EC_UINT a_wkc,
++ EC_UINT a_datalen, const unsigned char * a_data)
++ : DA_TG(a_idx, a_adp, a_ado, a_wkc, a_datalen, a_data)
++{
++}
++
++APRW_Telegram::~APRW_Telegram(){}
++
++unsigned char *
++APRW_Telegram::dump_command_field(unsigned char * a_buffer) const
++{
++ a_buffer = host2nw(a_buffer, APRW);
++ return a_buffer;
++}
++
++const unsigned char * APRW_Telegram::build_command_field(const unsigned char * a_buffer)
++{
++ assert(a_buffer[0] == APRW);
++ return ++a_buffer;
++}
++
++// --------------------------------------------------
+ // Broadcast Write Telegram
+ // --------------------------------------------------
+ BWR_Telegram::BWR_Telegram(EC_USINT a_idx, EC_UINT a_ado,
+@@ -236,7 +262,33 @@
+ return ++a_buffer;
+ }
+
++
+ // --------------------------------------------------
++// Node Addressed Physical Read Write Telegram
++// --------------------------------------------------
++NPRW_Telegram::NPRW_Telegram(EC_USINT a_idx, EC_UINT a_adp, EC_UINT a_ado, EC_UINT a_wkc,
++ EC_UINT a_datalen, const unsigned char * a_data)
++ : DA_TG(a_idx, a_adp, a_ado, a_wkc, a_datalen, a_data)
++{
++}
++
++NPRW_Telegram::~NPRW_Telegram(){}
++
++unsigned char *
++NPRW_Telegram::dump_command_field(unsigned char * a_buffer) const
++{
++ a_buffer = host2nw(a_buffer, NPRW);
++ return a_buffer;
++}
++
++const unsigned char * NPRW_Telegram::build_command_field(const unsigned char * a_buffer)
++{
++ assert(a_buffer[0] == NPRW);
++ return ++a_buffer;
++}
++
++
++// --------------------------------------------------
+ // Auto Increment Physical Read Multiple Write Telegram
+ // --------------------------------------------------
+ ARMW_Telegram::ARMW_Telegram(EC_USINT a_idx, EC_UINT a_adp, EC_UINT a_ado, EC_UINT a_wkc,
Index: src/al/ethercat_slave_conf.cxx
===================================================================
--- src/al/ethercat_slave_conf.cxx (revision 36)
@@ -418,7 +859,39 @@
===================================================================
--- src/al/ethercat_process_data.cxx (revision 36)
+++ src/al/ethercat_process_data.cxx (working copy)
-@@ -74,7 +74,7 @@
+@@ -51,16 +51,27 @@
+ // al_instance cannot be initiated right now, since this results in a
+ // circular instantiation al_instance calls router_instance etc.
+ EtherCAT_PD_Buffer::EtherCAT_PD_Buffer()
+- : m_is_running(0),
+- m_lrw_telegram(0x00,0x00010000,0x00,0,NULL),
+- m_lrw_frame(&m_lrw_telegram)
++ : m_is_running(0)
+ {
++ for (unsigned i=0;i<MAX_CHUNKS;++i) {
++ m_lrw_telegram[i] = new LRW_Telegram(0x00,0x00010000,0x00,0,NULL);
++ m_lrw_frame[i] = new EC_Ethernet_Frame(m_lrw_telegram[i]);
++ }
++
+ // get pointer to DLL and logic
+ m_dll_instance = EtherCAT_DataLinkLayer::instance();
+ m_logic_instance = EC_Logic::instance();
+ }
+
+-EtherCAT_PD_Buffer::~EtherCAT_PD_Buffer(){}
++EtherCAT_PD_Buffer::~EtherCAT_PD_Buffer()
++{
++ for (unsigned i=0;i<MAX_CHUNKS;++i) {
++ delete m_lrw_telegram[i];
++ m_lrw_telegram[i] = NULL;
++ delete m_lrw_frame[i];
++ m_lrw_frame[i];
++ }
++}
+
+ void
+ EtherCAT_PD_Buffer::start()
+@@ -74,27 +85,70 @@
if (m_is_running > 0)
--m_is_running;
else
@@ -427,7 +900,24 @@
}
bool
-@@ -85,13 +85,25 @@
+ EtherCAT_PD_Buffer::txandrx(size_t datalen, unsigned char * data)
+ {
++ //define MAX_CHUNKS 4
++ //define CHUNK_SIZE 1486
++
++ if (datalen>(CHUNK_SIZE*MAX_CHUNKS)) {
++ ec_log(EC_LOG_ERROR, "PD_Buffer: Too much data (%zd) to send in %d chunks of %d bytes\n",
++ datalen, MAX_CHUNKS, CHUNK_SIZE);
++ return false;
++ }
++
++ int handles[MAX_CHUNKS];
++ for (unsigned i=0;i<MAX_CHUNKS;++i)
++ handles[i]=-1;
++
++ bool success = true;
+ if ( m_is_running != 0)
+ // In case only starting when all slaves in the appropriate state
// this becomes something like
// if ( EtherCAT_PD_Buffer::m_al_instance->get_num_slaves() == m_running)
{
@@ -439,27 +929,48 @@
- log(EC_LOG_ERROR, "PD_Buffer: Error sending PD\n");
- return false;
+ int dst = 0x00010000;
-+#define CHUNK_SIZE 1486
++ unsigned index=0;
+ while (datalen > 0)
+ {
++ assert(index<MAX_CHUNKS);
+ size_t chunk_size = datalen < CHUNK_SIZE ? datalen : CHUNK_SIZE;
-+ m_lrw_telegram.set_idx(m_logic_instance->get_idx());
-+ m_lrw_telegram.set_wkc(m_logic_instance->get_wkc());
-+ m_lrw_telegram.set_datalen(chunk_size);
-+ m_lrw_telegram.set_data(data);
-+ m_lrw_telegram.set_adr(dst);
++ LRW_Telegram* a_lrw_telegram(m_lrw_telegram[index]);
++ a_lrw_telegram->set_idx(m_logic_instance->get_idx());
++ a_lrw_telegram->set_wkc(m_logic_instance->get_wkc());
++ a_lrw_telegram->set_datalen(chunk_size);
++ a_lrw_telegram->set_data(data);
++ a_lrw_telegram->set_adr(dst);
+
-+ if (!(m_dll_instance->txandrx(&m_lrw_frame)))
++ int result = m_dll_instance->tx(m_lrw_frame[index]);
++ if (result < 0)
+ {
+ ec_log(EC_LOG_ERROR, "PD_Buffer: Error sending PD\n");
-+ return false;
++ success = false;
++ break;
+ }
++ handles[index] = result;
+ datalen -= chunk_size;
+ data += chunk_size;
+ dst += chunk_size;
++ ++index;
}
++
++ // Receive every packet that was sent
++ for(unsigned i=0; i<index; ++i) {
++ if (handles[i] != -1) {
++ if (!m_dll_instance->rx(m_lrw_frame[i],handles[i])) {
++ ec_log(EC_LOG_ERROR, "PD_Buffer: Error receiving PD\n");
++ success = false;
++ }
++ }
++ }
}
- return true;
+- return true;
++
++ return success;
+ }
+
+
Index: src/al/ethercat_slave_handler.cxx
===================================================================
--- src/al/ethercat_slave_handler.cxx (revision 36)
@@ -718,18 +1229,128 @@
===================================================================
--- src/arch/posix/ethercat_posix_drv.c (revision 36)
+++ src/arch/posix/ethercat_posix_drv.c (working copy)
-@@ -60,7 +60,7 @@
+@@ -30,7 +30,6 @@
+ // Automation GmbH, Eiserstrasse 5, D-33415 Verl, Germany.
+ //===========================================================================
+
+-
+ /*
+ Contributed by RvdM, October, 5th, 2007.
+ Purpose to use EML over PREEMPT_RT kernel
+@@ -39,7 +38,7 @@
+
+ #include <errno.h>
+ #include <signal.h>
+-#include <pthread.h>
++#include <pthread.h>
+ #include <sys/ioctl.h>
+ #include <sys/types.h>
+ #include <sys/socket.h>
+@@ -49,36 +48,94 @@
+ #include <net/if.h>
+ #include <arpa/inet.h>
+
++#include <assert.h>
+ #include <string.h>
+
+ #include <ethercat/netif.h>
+ #include <ethercat/ethercat_log.h>
+
++#include <time.h>
++
++
+ // Maximum tries to send and receive a message
+ #define MAX_TRIES_TX 10
+ // Timeout for receiving and transmitting messages is 2000 us
// specifying 1000us or lower did not work on plain Linux.
- // recv() returned immediately with EWOULDBLOCK.
- // May/must be different for PREEMPT_RT kernels.
+-// recv() returned immediately with EWOULDBLOCK.
+-// May/must be different for PREEMPT_RT kernels.
-#define TIMEOUT_USEC 2000
++// recv() returned immediately with EWOULDBLOCK.
++// May/must be different for xgPREEMPT_RT kernels.
+#define TIMEOUT_USEC 20000
++// Socket timeout - so input_thread can exit
++#define SOCKET_TIMEOUT_USEC 10000
++
// Maximum times the master may retry to create or close a socket
#define MAX_TRIES_SOCKET 10
-@@ -90,14 +90,24 @@
+ // Ethernet Header is 14 bytes: 2 * MAC ADDRESS + 2 bytes for the type
+ #define HEADER_SIZE 14
+ #define MAX_ETH_DATA 1500
+
++#define NSEC_PER_SEC 1000000000
++
++// Size of buffer that holds message from strerror
++#define ERRBUF_LEN 60
++
++static char* my_strerror(int errnum, char *buf, size_t buflen)
++{
++ assert(buflen>0);
++ assert(buf!=NULL);
++ buf[0] = '\0';
++ if (strerror_r(errnum, buf, buflen) != 0) {
++ snprintf(buf, buflen, "N%d", errnum);
++ }
++ return buf;
++}
++
++static void my_usleep(uint32_t usec)
++{
++ char errbuf[ERRBUF_LEN];
++ assert(usec<1000000);
++ struct timespec req, rem;
++ req.tv_sec = 0;
++ req.tv_nsec = usec*1000;
++ while (nanosleep(&req, &rem)!=0) {
++ int error = errno;
++ ec_log(EC_LOG_ERROR, "%s : Error : %s\n", __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ if (error != EINTR) {
++ break;
++ }
++ req = rem;
++ }
++ return;
++}
++
++// No longer set socket timeout, but insteads set timeout variable used by pthread_cond_timedwait.
++// Produces the simular semantics as setting socket timeout in non-threaded posix driver
++// Timeout value it on microseconds
+ int set_socket_timeout(struct netif* ni, int64_t timeout) {
++ if ((timeout*1000) >= NSEC_PER_SEC)
++ {
++ ec_log(EC_LOG_FATAL, "%s: timeout is too large : %ld\n", __func__, timeout);
++ assert(timeout*1000 < NSEC_PER_SEC);
++ return -1;
++ }
++ ni->timeout_us = timeout;
+ return 0;
+ }
+
+-int init_socket(const char* interface) {
++
++
++static int init_socket(const char* interface) {
+ int sock;
+ int tries = 0;
+ struct sockaddr_ll addr;
+ struct ifreq ifr;
+
++ char errbuf[ERRBUF_LEN];
++
+ while (((sock = socket(PF_PACKET, SOCK_RAW, htons(0x88A4))) < 0) && tries < MAX_TRIES_SOCKET) {
++ int error = errno;
++ fprintf(stderr, "Couldn't open raw socket for interface %s : %s\n",
++ interface, my_strerror(error,errbuf,sizeof(errbuf)));
+ sleep(1);
+ tries++;
+ }
+@@ -88,25 +145,45 @@
+ return -1;
+ }
- printf("Socket created: socket id: %d\n", sock);
+- printf("Socket created: socket id: %d\n", sock);
++ //printf("Socket created: socket id: %d\n", sock);
- int index_ioctl;
+ int rv, index_ioctl;
@@ -740,8 +1361,18 @@
+ close(sock);
+ return -1;
+ }
++ if (!(ifr.ifr_flags & IFF_UP)) {
++ fprintf(stderr,
++ "Interface %s is not UP\n"
++ " try : ifup %s\n"
++ ,interface, interface);
++ return -1;
++ }
+ if (!(ifr.ifr_flags & IFF_RUNNING)) {
-+ fprintf(stderr, "Interface %s is not UP and RUNNING\n", interface);
++ fprintf(stderr,
++ "Interface %s is not RUNNING\n"
++ " is cable plugged in and device powered?\n"
++ ,interface);
+ return -1;
+ }
+
@@ -751,78 +1382,910 @@
return -1;
}
- printf("Got interface: index: %d\n", index_ioctl);
-+ printf("Got interface: index: %d for %s\n", index_ioctl, ifr.ifr_name);
++ //printf("Got interface: index: %d for %s\n", index_ioctl, ifr.ifr_name);
struct timeval tv;
- tv.tv_sec = ( TIMEOUT_USEC ) / 1000000;
-@@ -167,12 +177,12 @@
- // The actual send
- int len_send = send(sock,(unsigned char *)&msg_to_send,msg_len ,0);
- if(len_send < 0)
+- tv.tv_sec = ( TIMEOUT_USEC ) / 1000000;
+- tv.tv_usec = ( TIMEOUT_USEC ) % 1000000;
++ tv.tv_sec = ( SOCKET_TIMEOUT_USEC ) / 1000000;
++ tv.tv_usec = ( SOCKET_TIMEOUT_USEC ) % 1000000;
+ if ( setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (void*)&tv, sizeof(tv) ) != 0) {
+ perror("Aborting: Cannot set timeout");
+ return -1;
+ } else {
+- printf("Packet round-trip timeout has been set to %u usecs.\n", TIMEOUT_USEC);
++ //printf("Input thread recv timeout has been set to %u usecs.\n", SOCKET_TIMEOUT_USEC);
+ }
+
+ addr.sll_family = AF_PACKET;
+@@ -123,8 +200,41 @@
+ }
+
+ int close_socket(struct netif *ni) {
++ assert(ni!=NULL);
++ char errbuf[ERRBUF_LEN];
++
++ // If sock is < 0, things have not been started
++ if (ni->socket_private < 0) {
++ ec_log(EC_LOG_ERROR, "%s: close called twice on netif\n", __func__);
++ return -1;
++ }
++
++ // First try stopping input thread
++ ec_log(EC_LOG_INFO, "%s : Stopping input thread...\n", __func__);
++ ni->stop = TRUE;
++ int tries = 0;
++ for (tries = 0; tries<10; ++tries) {
++ my_usleep(SOCKET_TIMEOUT_USEC);
++ if (ni->is_stopped) {
++ ec_log(EC_LOG_INFO, "%s : Input thread has stopped\n", __func__);
++ break;
++ }
++ }
++
++ // Force input thread to exit
++ if (!ni->is_stopped) {
++ ec_log(EC_LOG_ERROR, "%s: input thread d/n stop, cancelling thread\n", __func__);
++ int error = pthread_cancel(ni->input_thread);
++ if (error!=0) {
++ ec_log(EC_LOG_ERROR, "%s: error cancelling input thread : %s\n", __func__,
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ return -1;
++ }
++ my_usleep(SOCKET_TIMEOUT_USEC);
++ }
++
+ int ret = close(ni->socket_private);
+- int tries = 1;
++ tries = 1;
+ while(ret < 0 && tries < MAX_TRIES_SOCKET) {
+ ret = close(ni->socket_private);
+ tries++;
+@@ -132,6 +242,13 @@
+ }
+ if(ret < 0)
+ perror("Failed to close socket");
++
++ ni->socket_private = -1;
++
++ if (ni->is_stopped) {
++ free(ni);
++ }
++
+ return ret;
+ }
+
+@@ -142,112 +259,769 @@
+ unsigned char data [MAX_ETH_DATA];
+ };
+
+-static bool low_level_output(struct EtherCAT_Frame * frame, struct netif * netif)
++// Send packet, if successfull add handle to list of packet awaiting a response.
++// Returns postive handle to packet list for success.
++// Returns negative value for errors
++// Assumes txandrx_mut is held when this function is called
++static int low_level_output(struct EtherCAT_Frame * frame, struct netif * netif)
+ {
+- bool result = false;
++ struct netif *ni = netif;
++ assert(ni!=NULL);
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ char errbuf[ERRBUF_LEN];
++
++ // Is there a slot to send packet with?
++ if (ni->unclaimed_packets >= MAX_UNCLAIMED_PACKETS) {
++ // Maybe put a condition variable here is the future.
++ ec_log(EC_LOG_FATAL, "%s: too many outstanding packets : %d\n",
++ __func__, ni->unclaimed_packets);
++ ++ni->counters.tx_full;
++ return -1;
++ }
++
++ // Find empty spot in list to store information about the sent packet
++ struct outstanding_pkt* pkt=NULL;
++ unsigned pkt_index;
++ { unsigned xx;
++ for (xx=0; xx<PKT_LIST_SIZE; ++xx) {
++ pkt_index = (ni->next_pkt_index + xx) % PKT_LIST_SIZE;
++ struct outstanding_pkt* i_pkt = &ni->pkt_list[pkt_index];
++ if (i_pkt->is_free) {
++ pkt=i_pkt;
++ break;
++ }
++ }
++ }
++
++ // Couldn't find empty slot
++ if (pkt==NULL) {
++ ec_log(EC_LOG_FATAL, "%s: outstanding packet list is full\n", __func__);
++ ++ni->counters.tx_full;
++ return -1;
++ }
++
++ // higher level protocol error. Attempt to map too much data in one ethernet frame
+ struct eth_msg msg_to_send;
+- int tel;
+- for(tel = 0; tel<MAX_ETH_DATA; tel++)
+- msg_to_send.data[tel] = 0x00;
++ memset(msg_to_send.data, 0x00, MAX_ETH_DATA);
+ int len_dump = framedump(frame, msg_to_send.data, MAX_ETH_DATA);
++ if(len_dump==0) {
++ ec_log(EC_LOG_FATAL, "%s: message buffer overflow\n", __func__);
++ ++ni->counters.tx_error;
++ return -1;
++ }
++
++ // Send packet
+ int msg_len = len_dump + ETH_ALEN + ETH_ALEN + 2;
+- if(len_dump) {
+- int sock = netif->socket_private;
++ int sock = ni->socket_private;
+ // Destination address is broadcast MAC address
+ // FIXME Is this also valid for EtherCAT UDP ethernet frames?
+ memset(msg_to_send.ether_dhost, 0xFF, ETH_ALEN);
+- // Source address
+- for(tel = 0; tel<ETH_ALEN; tel++)
+- msg_to_send.ether_shost[tel] = (netif->hwaddr)[tel];
+- // Type is ethercat
++ // Source address, last 3 bytes are used as markers
++ memcpy(msg_to_send.ether_shost, ni->hwaddr, ETH_ALEN);
++ assert(pkt_index<=0xFF);
++ msg_to_send.ether_shost[3] = pkt_index;
++ ni->tx_seqnum = (ni->tx_seqnum+1) & 0xFFFF;
++ msg_to_send.ether_shost[4] = ((ni->tx_seqnum) >> 8) & 0xFF;
++ msg_to_send.ether_shost[5] = ((ni->tx_seqnum) >> 0) & 0xFF;
++
++ // EtherType is ethercat
+ msg_to_send.ether_type[0] = 0x88;
+ msg_to_send.ether_type[1] = 0xA4;
+
+
+- // The actual send
+- int len_send = send(sock,(unsigned char *)&msg_to_send,msg_len ,0);
+- if(len_send < 0)
- log(EC_LOG_FATAL, "low_level_output(): Cannot Send\n");
-+ ec_log(EC_LOG_FATAL, "low_level_output(): Cannot Send\n");
- else
- result = true;
+- else
+- result = true;
++ // Pad the packet so the data (src + dst + ethertype + payload) is at least 60 bytes
++ // This prevents wireshark from complianing about parsing errors
++ int pkt_len = (msg_len < 60) ? 60 : msg_len;
++
++ // Get send time
++ if (clock_gettime(CLOCK_REALTIME, &pkt->tx_time) != 0) {
++ int error = errno;
++ ec_log(EC_LOG_FATAL, "%s: Could not get send_time : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ ++ni->counters.tx_error;
++ return -1;
++ }
++
++ // Send packet - sending should not block.
++ int len_send = send(sock,(unsigned char *)&msg_to_send, pkt_len, MSG_DONTWAIT);
++ if(len_send < 0) {
++ int error = errno;
++ if (error == ENETDOWN) {
++ ++ni->counters.tx_net_down;
++ if ((ni->counters.tx_net_down & 0xFFF) == 1) {
++ ec_log(EC_LOG_FATAL, "%s: %lld times : %s\n",
++ __func__, (unsigned long long) ni->counters.tx_net_down, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++ } else {
++ ec_log(EC_LOG_FATAL, "%s: Cannot Send : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ ++ni->counters.tx_error;
++ }
++ return -1;
++ }
++ if (len_send != pkt_len) {
++ ec_log(EC_LOG_FATAL, "%s: Incomplete send, sent %d or %d bytes\n",
++ __func__, len_send, pkt_len);
++ ++ni->counters.tx_error;
++ return -1;
}
- else { // higher level protocol error. Attempt to map to much data in one ethernet frame
+- else { // higher level protocol error. Attempt to map to much data in one ethernet frame
- log(EC_LOG_FATAL, "EtherCAT fatal: message buffer overflow\n");
-+ ec_log(EC_LOG_FATAL, "EtherCAT fatal: message buffer overflow\n");
- // Release the message buffer again
- }
+- // Release the message buffer again
+- }
-@@ -189,12 +199,12 @@
- int len_recv = recv(sock,buffer_receive,sizeof(buffer_receive),0);
+- return result;
++ // Put packet in list of outstanding packets
++ ++ni->unclaimed_packets;
++ pkt->is_free = FALSE;
++ pkt->frame = frame;
++ memcpy(pkt->ether_shost, msg_to_send.ether_shost, ETH_ALEN);
++
++ // Generate handle for sent packet
++ int handle = 0xFFFFFF &
++ ((pkt->ether_shost[3]<<16) |
++ (pkt->ether_shost[4]<<8 ) |
++ (pkt->ether_shost[5]<<0 ));
++
++ // Start searching when picking next open packet slot
++ ni->next_pkt_index = (ni->next_pkt_index+1) % PKT_LIST_SIZE;
++
++ ++ni->counters.sent;
++ assert(handle >= 0);
++ return handle;
+ }
+
+
+-static bool low_level_input(struct EtherCAT_Frame * frame, struct netif * netif) {
++// Last two bytes of src MAC address lets us determine seqnum of packet
++static unsigned parse_seqnum(u_int8_t ether_shost[MAC_ADDRESS_SIZE]) {
++ unsigned seqnum =
++ ( ((unsigned) ether_shost[4]) << 8) |
++ ( ((unsigned) ether_shost[5]) << 0) ;
++ return seqnum;
++}
+
+- unsigned char buffer_receive[MAX_ETH_DATA + HEADER_SIZE];
+- struct eth_msg *msg_received = (struct eth_msg *)buffer_receive;
+- //Receive message from socket
+- int sock = netif->socket_private;
+- int len_recv = recv(sock,buffer_receive,sizeof(buffer_receive),0);
++
++enum input_retcode {RECOVERABLE_ERROR=0,UNRECOVERABLE_ERROR=-1,SUCCESS=1};
++
++// Receives packet and stores it in pkt_list
++// Returns 1 for success
++// Returns -1 for unrecoverable errors (socket problem, not enough input buffers)
++// Returns 0 for a timeout (no packet ready), or recoverable error (invalid packet received, bad packet format)
++// Assumes txandrx_mut is held when this function is called
++static enum input_retcode low_level_input(struct netif * ni)
++{
++ assert(ni!=NULL);
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ char errbuf[ERRBUF_LEN];
++
++ // Find empty packet buffer to hold recieved packet
++ struct pkt_buf *buf = NULL;
++ {unsigned buf_index;
++ for (buf_index=0; buf_index<BUF_LIST_SIZE; ++buf_index) {
++ struct pkt_buf *i_buf = &ni->buf_list[buf_index];
++ if (i_buf->is_free) {
++ buf = i_buf;
++ break;
++ }
++ }
++ }
++
++ if (buf==NULL) {
++ // No buffer to hold incoming packet
++ ec_log(EC_LOG_FATAL, "%s : EtherCAT fatal: packet buffer list if full\n", __func__);
++ return UNRECOVERABLE_ERROR;
++ }
++ assert(sizeof(struct eth_msg) < sizeof(buf->data)); //Const assert?
++ struct eth_msg *msg_received = (struct eth_msg *) buf->data;
++
++ // Try to receive message from socket
++ // Don't hold mutex while blocking on recv, also mark buffer as
++ // non-free, so it is not used by a different rx call.
++ int sock = ni->socket_private;
++ buf->is_free = FALSE;
++ pthread_mutex_unlock(&ni->txandrx_mut);
++ int len_recv = recv(sock,msg_received,sizeof(*msg_received),0);
++ pthread_mutex_lock(&ni->txandrx_mut);
++ buf->is_free = TRUE;
++
if(len_recv < 0) {
- //perror("low_level_input: Cannot receive msg: ");
+- //perror("low_level_input: Cannot receive msg: ");
- log(EC_LOG_ERROR, "low_level_input: Cannot receive msg: %d\n",len_recv);
-+ ec_log(EC_LOG_ERROR, "low_level_input: Cannot receive msg: %d\n",len_recv);
- return false;
+- return false;
++ int error = errno;
++ if (error != EAGAIN) { // log error, for anthing other than a timeout
++ ec_log(EC_LOG_ERROR, "%s: Cannot receive msg: %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ if (error == EINTR) {
++ return RECOVERABLE_ERROR;
++ } else {
++ return UNRECOVERABLE_ERROR;
++ }
++ }
++ return RECOVERABLE_ERROR;
}
++ if (len_recv <= sizeof(ETH_ALEN + ETH_ALEN + 2)) {
++ ec_log(EC_LOG_ERROR, "%s: recieved runt packet: %d\n",__func__,len_recv);
++ ++ni->counters.rx_runt_pkt;
++ return RECOVERABLE_ERROR; // we did receive a packet
++ }
++
if ( ((msg_received->ether_type[0]) != 0x88) || (msg_received->ether_type[1]) != 0xA4) {
- log(EC_LOG_ERROR, "low_level_input: No EtherCAT msg!\n");
-+ ec_log(EC_LOG_ERROR, "low_level_input: No EtherCAT msg!\n");
- return false;
+- return false;
++ ec_log(EC_LOG_ERROR, "%s: No EtherCAT msg!\n", __func__);
++ ++ni->counters.rx_not_ecat;
++ return RECOVERABLE_ERROR; // we did receive a packet
++ }
++
++ // Use 2nd and 3rd byte of source max address to identify instance of EML that sent this out
++ // Ignore packet with HW addresses from other EML libraries
++ if ((msg_received->ether_shost[1] != ni->hwaddr[1]) ||
++ (msg_received->ether_shost[2] != ni->hwaddr[2]) )
++ {
++ ++ni->counters.rx_other_eml;
++ if ((ni->counters.rx_other_eml & 0x3FF) == 1) {
++ ec_log(EC_LOG_WARNING,
++ "%s: received %lld packets sent out from another EML instance\n",
++ __func__, (unsigned long long) ni->counters.rx_other_eml);
++ }
++ return RECOVERABLE_ERROR;
++ }
++
++ // 3rd byte of source MAC address tells us where outstanding packet should be in queue
++ unsigned pkt_index = 0xFF & msg_received->ether_shost[3];
++ if (pkt_index >= PKT_LIST_SIZE) {
++ ec_log(EC_LOG_ERROR, "%s: packet doesn't belong in queue, bad index %d", __func__, pkt_index);
++ ++ni->counters.rx_bad_index;
++ return RECOVERABLE_ERROR;
++ }
++ struct outstanding_pkt *pkt= &ni->pkt_list[pkt_index];
++
++ unsigned rx_seqnum = parse_seqnum(msg_received->ether_shost);
++
++ // Print warning if this packet is a repeat or is recieved out-of-order
++ // This shouldn't happen with normal setup... It may be worthwhile
++ // know about this odd situation if it occurs.
++ int16_t diff = (rx_seqnum - ni->rx_seqnum) & 0xFFFF;
++ // diff == 1 : Good: this is the next expected packet..
++ // diff == 0 : Bad: seems like a duplicate packet has been recieved,
++ // diff < 1 : Bad: seems like an old got packets out-of-order, or old packet has been re-received
++ // diff > 1 : Ok: could mean previous <diff> packets have been
++ // lost, and this is first to make it back successfully
++ if (diff == 0) {
++ ec_log(EC_LOG_ERROR, "low_level_input: error, got packet with duplicate seqnum %d\n", rx_seqnum);
++ ++ni->counters.rx_dup_seqnum;
++ // allow following checks to drop packet
++ } else if (diff < 0) {
++ // Newer packet has already been recieved, somehow packets got
++ // reordered. This should not happen on a raw ethernet chain.
++ // Could occur if running ethercat over IP/UDP.
++ // Also, could be caused by buffering in OS.
++ ec_log(EC_LOG_ERROR,
++ "low_level_input: warning : got packet in incorrect order: got %d, expected %d\n",
++ rx_seqnum, (ni->rx_seqnum+1) & 0xFFFF);
++ ++ni->counters.rx_bad_order;
++ // Don't drop packet might still be uncollected --
++ // Allow following to filter it out.
++ }
++
++ // Determine if received seqnum matches seqnum used when sending packet
++ unsigned tx_seqnum = parse_seqnum(pkt->ether_shost);
++ if (rx_seqnum != tx_seqnum) {
++ ec_log(EC_LOG_ERROR,
++ "low_level_input: got packet with invalid seqnum: got %d, expected %d\n"
++ " next packet will be sent with seqnum %d\n"
++ " last received packet had seqnum %d\n"
++ , rx_seqnum, tx_seqnum, ni->tx_seqnum, ni->rx_seqnum);
++ ++ni->counters.rx_bad_seqnum;
++ return RECOVERABLE_ERROR;
++ }
++
++ // Make sure packet has not arrived already
++ if ( (pkt->buf!=NULL) ) {
++ ec_log(EC_LOG_ERROR, "low_level_input: got duplicate packet?\n");
++ ++ni->counters.rx_dup_pkt;
++ return RECOVERABLE_ERROR;
++ }
++
++ // Record most recently received seqnum
++ ni->rx_seqnum = rx_seqnum;
++
++ // Make sure packet has not been collected with rx
++ if ( (pkt->is_free==TRUE) ) {
++ ec_log(EC_LOG_ERROR,
++ "%s: got packet that has already been collected with rx(), increase socket timeout?\n", __func__);
++ ++ni->counters.rx_late_pkt;
++ // Keep track of how long it actually took for the late packet to get back.
++ struct timespec rx_time;
++ if (clock_gettime(CLOCK_REALTIME, &rx_time) != 0) {
++ int error = errno;
++ ec_log(EC_LOG_FATAL, "%s: Could not get recv time for late packet : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ } else {
++ unsigned round_trip_time_us =
++ (rx_time.tv_sec - pkt->tx_time.tv_sec) * 1000000 +
++ (rx_time.tv_nsec - pkt->tx_time.tv_nsec) / 1000;
++ ni->counters.rx_late_pkt_rtt_us = round_trip_time_us;
++ ni->counters.rx_late_pkt_rtt_us_sum += round_trip_time_us;
++ ec_log(EC_LOG_ERROR,
++ "%s: late packet arrived in %u us, timeout set to %u us\n",
++ __func__, round_trip_time_us, ni->timeout_us);
++ }
++ return RECOVERABLE_ERROR;
++ }
++
++ // Packet passes all checks... mark outstanding packet as having
++ // received reply - mark buffer as being used.
++ buf->is_free=FALSE;
++ pkt->buf=buf;
++
++ // Signal anybody waiting on sent packet
++ int error = pthread_cond_broadcast(&pkt->rx_cond);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL, "%s: cond broadcast : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ return UNRECOVERABLE_ERROR;
++ }
++
++ ++ni->counters.received;
++ return SUCCESS;
++}
++
++
++// Thread that continuously polls input and puts it into queue
++void* low_level_input_thread_func(void* data)
++{
++ char errbuf[ERRBUF_LEN];
++
++ // Increase priority of input thread
++ struct sched_param thread_param;
++ int policy = SCHED_FIFO;
++ thread_param.sched_priority = sched_get_priority_max(policy);
++ int error = pthread_setschedparam(pthread_self(), policy, &thread_param);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL, "%s : Setting thread sched : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++
++ struct netif * ni = (struct netif *) data;
++ ec_log(EC_LOG_INFO, "INFO: Starting input thread\n");
++ BOOL stop = FALSE;
++ while (stop==FALSE) {
++ error = pthread_mutex_lock(&ni->txandrx_mut);
++ assert(error==0);
++ stop = ni->stop;
++ if (low_level_input(ni)==UNRECOVERABLE_ERROR) {
++ ec_log(EC_LOG_FATAL, "%s : Unrecoverable error on input thread\n", __func__);
++ // Some unrecoverable error can be recovered with external conditions (ENETDOWN)
++ // However, don't spin on recv if it will immediately return an error code
++ //stop = TRUE;
++ my_usleep(10000);
++ }
++ error = pthread_mutex_unlock(&ni->txandrx_mut);
++ assert(error==0);
++ }
++ ec_log(EC_LOG_INFO, "INFO: Input thread is exiting\n");
++ ni->is_stopped=TRUE;
++ return NULL;
++}
++
++
++static void init_buf(struct pkt_buf *buf) {
++ buf->is_free = TRUE;
++}
++
++static BOOL init_pkt(struct outstanding_pkt *pkt) {
++ pkt->is_free = TRUE;
++ pkt->buf = NULL;
++ pkt->frame = NULL;
++ memset(pkt->ether_shost, 0, sizeof(pkt->ether_shost));
++
++ int error;
++ char errbuf[ERRBUF_LEN];
++
++ error = pthread_cond_init(&pkt->rx_cond,NULL);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"%s : Initializing rx condition var failed : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ return FALSE;
++ }
++
++ return TRUE;
++}
++
++
++// Tries lookup of packet from queue using handle
++// returns NULL if handle is stall/invalid, or pointer to outstanding pkt otherwise;
++// Assumes txandrx mutex is already held
++static struct outstanding_pkt *low_level_lookup(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ assert(frame!=NULL);
++ assert(ni!=NULL);
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ // Make sure handle makes sense, also make sure it is not a negative number
++ if ( ((handle>>24) & 0xFF) != 0) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: called with invalid handle 0x%08X\n", handle);
++ return NULL;
++ }
++
++ // Source MAC address packet should have
++ // First 3 bytes are sent by netif source MAC address
++ // Second 3 byte are encoded in handle.
++ u_int8_t ether_shost[ETH_ALEN];
++ memcpy(ether_shost, ni->hwaddr, ETH_ALEN);
++ ether_shost[3] = (handle>>16) & 0xFF;
++ ether_shost[4] = (handle>>8) & 0xFF;
++ ether_shost[5] = (handle>>0) & 0xFF;
++
++ // 3th byte of source MAC address tells us where outstanding packet should be in queue
++ unsigned pkt_index = 0xFF & ether_shost[3];
++ if (pkt_index >= PKT_LIST_SIZE) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: handle 0x%08X references bad pkt_index\n", handle, pkt_index);
++ return NULL; //error
++ }
++ struct outstanding_pkt *pkt = &ni->pkt_list[pkt_index];
++
++ // Check frame stored for outstanding packet against frame that was passed in
++ if (pkt->frame != frame) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: handle frame d/n match passed in frame \n");
++ return NULL; //error
++ }
++
++ // Check MAC address encoded in handle against mac address packet was sent out with
++ if (memcmp(pkt->ether_shost, ether_shost, ETH_ALEN) != 0) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: tried collecting with stale handle \n");
++ return NULL; //error
++ }
++
++ // Make sure packet was not already collected
++ if (pkt->is_free!=FALSE) {
++ ec_log(EC_LOG_ERROR, "low_level_lookup: tried collecting with same handle twice \n");
++ return NULL; //error
++ }
++
++ return pkt;
++}
++
++
++// Drop packet from queue of outstanding packets.
++// Returns FALSE if packet d/n seem to exist
++// Assumes txandrx mutex is already held
++BOOL low_level_release(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ // Lookup pointer to packet using handle
++ struct outstanding_pkt *pkt = low_level_lookup(frame, ni, handle);
++ if (pkt == NULL) {
++ return FALSE;
}
-@@ -202,7 +212,7 @@
- int succes = framebuild(frame,msg_received->data);
- if (succes != 0){
+- // build Ethercat Frame
+- int succes = framebuild(frame,msg_received->data);
+- if (succes != 0){
++ // Mark packet buffer as availible
++ if (pkt->buf!=NULL) {
++ init_buf(pkt->buf);
++ }
++
++ // Mark outstanding pkt as unused
++ pkt->is_free = TRUE;
++ pkt->buf = NULL;
++ pkt->frame = NULL;
++ // Don't clear source MAC address, to allow detection of packets
++ // that are received late, (rx was called before packet got back)
++
++ // One less outstanding packet
++ assert(ni->unclaimed_packets > 0);
++ --ni->unclaimed_packets;
++
++ // Packets are only released when they are not recieved
++ ++ni->counters.dropped;
++
++ return TRUE;
++}
++
++
++enum dequeue_retcode {DEQUEUE_SUCCESS=1,DEQUEUE_ERROR=-1,DEQUEUE_NOT_FOUND=0};
++
++// Tries retrieving packet from queue using handle
++// returns negative values for error, 0 if no packet is in queue, or postive value if packet is found
++// Assumes txandrx mutex is already held
++static int low_level_dequeue(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ assert(pthread_mutex_lock(&ni->txandrx_mut) == EDEADLK);
++
++ // Lookup pointer to packet using handle
++ struct outstanding_pkt *pkt = low_level_lookup(frame, ni, handle);
++ if (pkt == NULL) {
++ return DEQUEUE_ERROR;
++ }
++
++ // OK, handle is good...
++ // has response packet been received ?
++ if (pkt->buf==NULL) {
++ return DEQUEUE_NOT_FOUND; // packet has not been recieved
++ }
++ assert(pkt->buf->is_free == FALSE);
++
++ // Response packet has been received...
++
++ // Get pointer to ethernet packet payout
++ assert(sizeof(struct eth_msg) < sizeof(pkt->buf->data)); //Const assert?
++ struct eth_msg *msg_received = (struct eth_msg *) pkt->buf->data;
++
++ // Mark packet buffer as availible
++ init_buf(pkt->buf);
++
++ // Mare outstanding pkt as unused
++ init_pkt(pkt);
++
++ int success = framebuild(frame,msg_received->data);
++ if (success != 0){
// FIXME decent error handling here
- log(EC_LOG_ERROR, "low_level_input: framebuilding failed!\n");
+- return false;
+- }
+ ec_log(EC_LOG_ERROR, "low_level_input: framebuilding failed!\n");
- return false;
- }
++ return DEQUEUE_ERROR;
++ }
++
++ // One less unclaimed packet
++ assert(ni->unclaimed_packets > 0);
++ --ni->unclaimed_packets;
++
++ ++ni->counters.collected;
++ return DEQUEUE_SUCCESS; // Good
++}
++
++// Transmits a packet.
++// Returns a negative value for errors, or positive integer handle
++// handle and frame* should be used to get response with ec_posix_rx()
++static int ec_posix_tx(struct EtherCAT_Frame * frame, struct netif * ni)
++{
++ assert(ni != NULL);
++
++ int error = pthread_mutex_lock (&ni->txandrx_mut);
++ assert(error==0);
++ int handle = low_level_output(frame,ni);
++ error = pthread_mutex_unlock (&ni->txandrx_mut);
++ assert(error==0);
++ return handle;
++}
++
++
++// Receives a packet that has been sent with ec_posix_tx()
++// Returns TRUE for success, FALSE for errors or timeout.
++static BOOL ec_posix_rx_common(struct EtherCAT_Frame * frame, struct netif * ni, int handle, BOOL mayblock)
++{
++ assert(ni != NULL);
++ assert(frame != NULL);
++ char errbuf[ERRBUF_LEN];
++
++ // Find rx mutex for outstanding packet
++ struct outstanding_pkt *pkt;
++ int error = pthread_mutex_lock(&ni->txandrx_mut);
++ assert(error==0);
++ if (error!=0) {
++ ec_log(EC_LOG_FATAL, "%s: error locking mutex : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ return FALSE;
++ }
++ pkt = low_level_lookup(frame, ni, handle);
-@@ -222,24 +232,24 @@
- return true;
+- return true;
++ // Bad handle, double rx, etc...
++ if (pkt == NULL) {
++ pthread_mutex_unlock(&ni->txandrx_mut);
++ return FALSE;
++ }
++
++ // Try pulling result from queue
++ enum dequeue_retcode result = low_level_dequeue(frame, ni, handle);
++
++ // If this can block, try waiting for condition signal
++ if (mayblock) {
++ // Wait until <timeout_us> after packet was sent
++ struct timespec timeout = pkt->tx_time;
++ timeout.tv_nsec += (ni->timeout_us * 1000);
++ if (timeout.tv_nsec >= NSEC_PER_SEC) {
++ timeout.tv_nsec -= NSEC_PER_SEC;
++ ++timeout.tv_sec;
++ }
++ assert(timeout.tv_nsec < NSEC_PER_SEC);
++ assert(timeout.tv_nsec >= 0);
++
++ // Put loop around pthread_cond_timedwait to handle spurious wakeups
++ while (result == DEQUEUE_NOT_FOUND) {
++ // Wait on recv condition from input thread
++ error = pthread_cond_timedwait(&pkt->rx_cond, &ni->txandrx_mut, &timeout);
++ if (error != 0) {
++ if (error == ETIMEDOUT) {
++ // Timeout
++ } else {
++ ec_log(EC_LOG_FATAL, "%s: error waiting on timed condition : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++ break;
++ } else {
++ result = low_level_dequeue(frame, ni, handle);
++ // dequeue should always return 1 (since we know the packet is there)
++ // ... unless there was a spurious wakeup.
++ if (result == DEQUEUE_NOT_FOUND) {
++ ec_log(EC_LOG_FATAL, "%s: spurious wakeup : dequeue result=%d\n",
++ __func__, result);
++ }
++ }
++ }
++ } //end if mayblock
++
++ // If we didn't get the packet, release it's resources
++ if (result!=DEQUEUE_SUCCESS) {
++ low_level_release(frame, ni, handle);
++ }
++
++ error = pthread_mutex_unlock(&ni->txandrx_mut);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL, "%s: error unlocking mutex : %s\n",
++ __func__, my_strerror(error,errbuf,sizeof(errbuf)));
++ }
++ return (result==DEQUEUE_SUCCESS) ? TRUE : FALSE;
+ }
+
+-// For thread safety: txandrx() can be called from multiple threads...
+-static pthread_mutex_t txandrx_mut;
+
+-static bool ec_rtdm_txandrx(struct EtherCAT_Frame * frame, struct netif * netif) {
++// Receives a packet that has been sent with ec_posix_tx()
++// Returns TRUE for success, FALSE for errors or timeout.
++static BOOL ec_posix_rx(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ return ec_posix_rx_common(frame, ni, handle, TRUE);
++}
++
++// Receives a packet that has been sent with ec_posix_tx(), doesn't wait
++// Returns TRUE for success, FALSE for errors or timeout.
++static BOOL ec_posix_rx_nowait(struct EtherCAT_Frame * frame, struct netif * ni, int handle)
++{
++ return ec_posix_rx_common(frame, ni, handle, FALSE);
++}
++
++
++// Only attempt to send one packet
++static BOOL ec_posix_txandrx_once(struct EtherCAT_Frame * frame, struct netif * ni)
++{
++ assert(ni != NULL);
++
++ int handle = ec_posix_tx(frame, ni);
++ if (handle < 0) {
++ return FALSE;
++ }
++ BOOL result = ec_posix_rx(frame, ni, handle);
++ return result;
++}
++
++
++// Normal txandrx, try sending packet multiple times before giving up.
++static BOOL ec_rtdm_txandrx(struct EtherCAT_Frame * frame, struct netif * netif) {
++ struct netif *ni = netif;
++ assert(ni != NULL);
++ char errbuf[ERRBUF_LEN];
++
+ int tries = 0;
+ while (tries < MAX_TRIES_TX) {
+- pthread_mutex_lock (&txandrx_mut);
+- if (low_level_output(frame,netif)){
+- if (low_level_input(frame,netif)){
+- pthread_mutex_unlock(&txandrx_mut);
+- return true;
++ BOOL success = ec_posix_txandrx_once(frame, ni);
++ if (success) {
++ if (tries > 0) {
++ ec_log(EC_LOG_ERROR, "low_level_txandrx: sending/receiving failed %d times\n", tries);
}
- else{
+- else{
- log(EC_LOG_ERROR, "low_level_txandrx: receiving failed\n");
-+ ec_log(EC_LOG_ERROR, "low_level_txandrx: receiving failed\n");
- pthread_mutex_unlock(&txandrx_mut);
- }
+- pthread_mutex_unlock(&txandrx_mut);
+- }
++ return TRUE;
}
- else{
+- else{
- log(EC_LOG_ERROR, "low_level_txandrx: sending failed\n");
-+ ec_log(EC_LOG_ERROR, "low_level_txandrx: sending failed\n");
- pthread_mutex_unlock(&txandrx_mut);
- }
+- pthread_mutex_unlock(&txandrx_mut);
+- }
++ int error = pthread_mutex_lock(&ni->txandrx_mut);
++ assert(error==0);
++ //++ni->retries;
++ error = pthread_mutex_unlock(&ni->txandrx_mut);
++ assert(error==0);
tries++;
}
- log(EC_LOG_FATAL, "low_level_txandrx: failed: MAX_TRIES_TX: Giving up\n");
-+ ec_log(EC_LOG_FATAL, "low_level_txandrx: failed: MAX_TRIES_TX: Giving up\n");
- return false;
+- return false;
++ ec_log(EC_LOG_FATAL, "low_level_txandrx: failed %d times: Giving up\n", MAX_TRIES_TX);
++ return FALSE;
}
++
struct netif* init_ec(const char * interface) {
++ char errbuf[ERRBUF_LEN];
++
int sock = init_socket(interface);
if(sock < 0) {
- log(EC_LOG_FATAL,"Socket initialisation failed\n");
+ ec_log(EC_LOG_FATAL,"Socket initialisation failed\n");
return 0;
}
++
struct netif* ni = (struct netif*)malloc(sizeof(struct netif));
++ if (ni==NULL) {
++ ec_log(EC_LOG_FATAL,"Allocating netif struct failed\n");
++ return NULL;
++ }
++ int error = pthread_mutexattr_init(&ni->txandrx_attr);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"Initializing txandrx mutex attr failed : %s\n",
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ free(ni);
++ return NULL;
++ }
++
++ error = pthread_mutexattr_settype(&ni->txandrx_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"Setting type of mutex attr failed : %s\n",
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ free(ni);
++ return NULL;
++ }
++
++ error = pthread_mutex_init(&ni->txandrx_mut, &ni->txandrx_attr);
++ if (error != 0) {
++ ec_log(EC_LOG_FATAL,"Initializing txandrx mutex failed : %s\n",
++ my_strerror(error,errbuf,sizeof(errbuf)));
++ free(ni);
++ return NULL;
++ }
++
++ int index;
++ for (ind...
[truncated message content] |
|
From: <mee...@us...> - 2009-09-04 18:27:47
|
Revision: 23839
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23839&view=rev
Author: meeussen
Date: 2009-09-04 18:27:37 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
remove a bunch of undesired deps on pr2_mechanism_control
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/manifest.xml
pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/manifest.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-09-04 18:27:37 UTC (rev 23839)
@@ -33,7 +33,6 @@
<depend package="control_toolbox" />
<depend package="filters" />
<depend package="diagnostic_updater" />
- <depend package="pr2_mechanism_control" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-09-04 18:27:37 UTC (rev 23839)
@@ -8,7 +8,6 @@
<depend package="rospy"/>
<depend package="pr2_controller_interface" />
<depend package="pr2_mechanism_model" />
- <depend package="pr2_mechanism_control" />
<depend package="control_toolbox" />
<depend package="realtime_tools" />
<depend package="roscpp" />
Modified: pkg/trunk/sandbox/experimental_controllers/manifest.xml
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/experimental_controllers/manifest.xml 2009-09-04 18:27:37 UTC (rev 23839)
@@ -12,7 +12,6 @@
<depend package="pr2_controller_interface" />
<depend package="sensor_msgs" />
<depend package="pr2_mechanism_model" />
- <depend package="pr2_mechanism_control" />
<depend package="pluginlib" />
<depend package="control_toolbox" />
<depend package="tinyxml" />
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp 2009-09-04 18:27:37 UTC (rev 23839)
@@ -34,7 +34,6 @@
#include <algorithm>
-#include <pr2_mechanism_control/mechanism_control.h>
#include "kdl/chainfksolverpos_recursive.hpp"
#include "experimental_controllers/cartesian_pose_twist_controller.h"
#include "kdl/chainfksolvervel_recursive.hpp"
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp 2009-09-04 18:27:37 UTC (rev 23839)
@@ -33,7 +33,6 @@
#include "experimental_controllers/cartesian_tff_controller.h"
#include <algorithm>
-#include <pr2_mechanism_control/mechanism_control.h>
#include <kdl/chainfksolvervel_recursive.hpp>
#include "pluginlib/class_list_macros.h"
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp 2009-09-04 18:27:37 UTC (rev 23839)
@@ -34,7 +34,6 @@
#include <algorithm>
-#include <pr2_mechanism_control/mechanism_control.h>
#include "kdl/chainfksolverpos_recursive.hpp"
#include "experimental_controllers/cartesian_trajectory_controller.h"
#include "pluginlib/class_list_macros.h"
Modified: pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp 2009-09-04 18:27:37 UTC (rev 23839)
@@ -34,7 +34,6 @@
#include <experimental_controllers/cartesian_twist_controller_ik.h>
#include <algorithm>
-#include <pr2_mechanism_control/mechanism_control.h>
#include "pluginlib/class_list_macros.h"
#include <kdl/chainfksolvervel_recursive.hpp>
Modified: pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/experimental_controllers/src/head_servoing_controller.cpp 2009-09-04 18:27:37 UTC (rev 23839)
@@ -35,7 +35,6 @@
// Original version: Melonee Wise <mw...@wi...>
#include <experimental_controllers/head_servoing_controller.h>
-#include <pr2_mechanism_control/mechanism_control.h>
#include <cmath>
#include <angles/angles.h>
#include "pluginlib/class_list_macros.h"
Modified: pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/manifest.xml
===================================================================
--- pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-09-04 18:00:21 UTC (rev 23838)
+++ pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/manifest.xml 2009-09-04 18:27:37 UTC (rev 23839)
@@ -10,7 +10,6 @@
<depend package="control_toolbox" />
<depend package="roscpp" />
<depend package="robot_mechanism_controllers" />
- <depend package="pr2_mechanism_control" />
<depend package="pluginlib" />
<url>http://pr.willowgarage.com/joint_qualification_controllers</url>
<rosdep name="wxwidgets"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-09-04 18:00:34
|
Revision: 23838
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23838&view=rev
Author: eitanme
Date: 2009-09-04 18:00:21 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Putting some reasoning about unknown space back into the navigation stack so that now you just set parameters to decide whether or not to take unkown space into account when running
Modified Paths:
--------------
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -257,7 +257,7 @@
//we'll start our costmap up now that we're active
planner_cost_map_ros_->start();
- planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+ planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
planner_->setDoor(door,getPose2D(global_pose_),door_transformed);//set the goal into the planner
Modified: pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml 2009-09-04 18:00:21 UTC (rev 23838)
@@ -3,7 +3,7 @@
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
-unknown_threshold: 10
+unknown_threshold: 9
mark_threshold: 0
#END VOXEL STUFF
transform_tolerance: 0.3
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -83,8 +83,8 @@
tc_ = new TrajectoryPlannerROS("TrajectoryPlannerROS", &tf_, controller_costmap_ros_);
//initially clear any unknown space around the robot
- planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
- controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+ planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
+ controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
//TODO:spawn planning thread here?
}
Modified: pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/sandbox/doors_planner_core/src/move_base_door_action.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -256,7 +256,7 @@
//we'll start our costmap up now that we're active
planner_cost_map_ros_->start();
- planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+ planner_cost_map_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
planner_->setDoor(door,getPose2D(global_pose_),door_transformed);//set the goal into the planner
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -73,7 +73,7 @@
ros_node_.param("~min_in_place_vel_th", min_in_place_vel_th_, 0.4);
//initialize the copy of the costmap the controller will use
- costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+ costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
costmap_ros_->getCostmapCopy(costmap_);
string odom_topic, base_cmd_topic, joy_listen_topic, world_model_type;
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -108,8 +108,8 @@
//initially clear any unknown space around the robot
- planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
- controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+ planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
+ controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
//if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/costmap_model.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -57,8 +57,8 @@
//for circular robots the circumscribed radius will equal the inscribed radius and we can do a point check
if(circumscribed_radius == inscribed_radius){
unsigned char cost = costmap_.getCost(cell_x, cell_y);
- //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
- if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
+ //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
+ if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
return -1.0;
return 1.0;
}
@@ -189,8 +189,8 @@
double CostmapModel::pointCost(int x, int y){
unsigned char cost = costmap_.getCost(x, y);
//if the cell is in an obstacle the path is invalid
- //if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
- if(cost == LETHAL_OBSTACLE){
+ //if(cost == LETHAL_OBSTACLE){
+ if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
return -1;
}
Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_publisher.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -81,8 +81,8 @@
costmap.mapToWorld(i, j, wx, wy);
std::pair<double, double> p(wx, wy);
- //if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || costmap.getCost(i, j) == costmap_2d::NO_INFORMATION)
- if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+ //if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
+ if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || costmap.getCost(i, j) == costmap_2d::NO_INFORMATION)
raw_obstacles.push_back(p);
else if(costmap.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
inflated_obstacles.push_back(p);
Modified: pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/costmap_2d/src/costmap_2d_ros.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -260,7 +260,7 @@
ros_node_.param("origin_z", map_origin_z, 0.0);
int unknown_threshold, mark_threshold;
- ros_node_.param("unknown_threshold", unknown_threshold, 0);
+ ros_node_.param("unknown_threshold", unknown_threshold, z_voxels);
ros_node_.param("mark_threshold", mark_threshold, 0);
ROS_ASSERT(z_voxels >= 0 && unknown_threshold >= 0 && mark_threshold >= 0);
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-09-04 17:49:40 UTC (rev 23837)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-09-04 18:00:21 UTC (rev 23838)
@@ -110,8 +110,8 @@
make_plan_srv_ = ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
//initially clear any unknown space around the robot
- planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
- controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
+ planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
+ controller_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 4, circumscribed_radius_ * 4);
//if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pan...@us...> - 2009-09-04 17:49:53
|
Revision: 23837
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23837&view=rev
Author: pantofaru
Date: 2009-09-04 17:49:40 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
A package for classifying objects and people based on their height profiles.
Added Paths:
-----------
pkg/trunk/sandbox/height_classifier/
pkg/trunk/sandbox/height_classifier/.build_version
pkg/trunk/sandbox/height_classifier/CMakeLists.txt
pkg/trunk/sandbox/height_classifier/Makefile
pkg/trunk/sandbox/height_classifier/include/
pkg/trunk/sandbox/height_classifier/include/height_classifier/
pkg/trunk/sandbox/height_classifier/launch/
pkg/trunk/sandbox/height_classifier/launch/save_scans.launch
pkg/trunk/sandbox/height_classifier/launch/save_stereo_point_clouds.launch
pkg/trunk/sandbox/height_classifier/lib/
pkg/trunk/sandbox/height_classifier/mainpage.dox
pkg/trunk/sandbox/height_classifier/manifest.xml
pkg/trunk/sandbox/height_classifier/src/
pkg/trunk/sandbox/height_classifier/src/point_cloud_writer.cpp
Added: pkg/trunk/sandbox/height_classifier/.build_version
===================================================================
--- pkg/trunk/sandbox/height_classifier/.build_version (rev 0)
+++ pkg/trunk/sandbox/height_classifier/.build_version 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1 @@
+:
Added: pkg/trunk/sandbox/height_classifier/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/height_classifier/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/height_classifier/CMakeLists.txt 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(point_cloud_writer src/point_cloud_writer.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Added: pkg/trunk/sandbox/height_classifier/Makefile
===================================================================
--- pkg/trunk/sandbox/height_classifier/Makefile (rev 0)
+++ pkg/trunk/sandbox/height_classifier/Makefile 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/sandbox/height_classifier/launch/save_scans.launch
===================================================================
--- pkg/trunk/sandbox/height_classifier/launch/save_scans.launch (rev 0)
+++ pkg/trunk/sandbox/height_classifier/launch/save_scans.launch 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1,27 @@
+<launch>
+
+<remap from="/tf_message" to="/tf_message_muxed" />
+
+<node pkg="laser_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
+ <remap from="scan_in" to="tilt_scan_muxed"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0"/>
+ <param name="tf_tolerance_secs" type="double" value="0.03"/>
+ <param name="max_scans" type="int" value="400"/>
+ <param name="fixed_frame" type="string" value="base_link"/>
+ <param name="downsample_factor" type="int" value="2" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <rosparam command="load" file="$(find laser_filters)/examples/default_scan_shadows.yaml" />
+</node>
+
+<node pkg="pr2_laser_snapshotter" type="pr2_laser_snapshotter" output="screen" name="snapshotter" >
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal" />
+ <remap from="build_cloud" to="laser_scan_assembler/build_cloud" />
+ <remap from="full_cloud" to="full_cloud_filtered" />
+</node>
+
+<node pkg="height_classifier" type="point_cloud_writer" output="screen" name="point_cloud_writer">
+ <remap from="point_cloud" to="full_cloud_filtered" />
+ <param name="out_directory" type="string" value="$(find height_classifier)/scans/" />
+</node>
+
+</launch>
Added: pkg/trunk/sandbox/height_classifier/launch/save_stereo_point_clouds.launch
===================================================================
--- pkg/trunk/sandbox/height_classifier/launch/save_stereo_point_clouds.launch (rev 0)
+++ pkg/trunk/sandbox/height_classifier/launch/save_stereo_point_clouds.launch 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1,24 @@
+<launch>
+
+<remap from="/tf_message" to="/tf_message_muxed" />
+
+ <group ns="wide_stereo">
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false" output="screen" name="stereoproc">
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="True"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ <param name="do_keep_coords" type="bool" value="False"/>
+ <param name="num_disp" type="int" value="64"/>
+ </node>
+ </group>
+
+
+
+<node pkg="height_classifier" type="point_cloud_writer" output="screen" name="point_cloud_writer">
+ <remap from="point_cloud" to="full_cloud_filtered" />
+ <param name="out_directory" type="string" value="$(find height_classifier)/scans/" />
+</node>
+
+</launch>
Added: pkg/trunk/sandbox/height_classifier/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/height_classifier/mainpage.dox (rev 0)
+++ pkg/trunk/sandbox/height_classifier/mainpage.dox 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1,121 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b The height_classifier is a package for classifying objects (and people) by their height. Point cloud height values (z-values)
+are histogramed on a grid on the ground plane to create features for classification. For example, a person might be a 0.5m-wide
+blob of maximum height 1.5-2.5m, while a desk might be a 1m-wide blob of maximum height 1m.
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
Added: pkg/trunk/sandbox/height_classifier/manifest.xml
===================================================================
--- pkg/trunk/sandbox/height_classifier/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/height_classifier/manifest.xml 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1,20 @@
+<package>
+ <description brief="height_classifier">
+
+ height_classifier
+
+ </description>
+ <author>Caroline Pantofaru</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/height_classifier</url>
+ <!--depend package="laser_geometry"/-->
+ <depend package="sensor_msgs"/>
+ <depend package="std_msgs"/>
+ <depend package="roscpp"/>
+ <depend package="geometry_msgs"/>
+ <depend package="opencv_latest"/>
+ <depend package="tf"/>
+</package>
+
+
Added: pkg/trunk/sandbox/height_classifier/src/point_cloud_writer.cpp
===================================================================
--- pkg/trunk/sandbox/height_classifier/src/point_cloud_writer.cpp (rev 0)
+++ pkg/trunk/sandbox/height_classifier/src/point_cloud_writer.cpp 2009-09-04 17:49:40 UTC (rev 23837)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <stdio.h>
+#include <iostream>
+#include <sstream>
+#include <fstream>
+
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+#include <ros/ros.h>
+#include "sensor_msgs/PointCloud.h"
+#include "tf/transform_listener.h"
+#include "tf/message_filter.h"
+#include "message_filters/subscriber.h"
+
+class PointCloudWriter
+{
+
+public:
+
+ PointCloudWriter()
+ : pc_sub_(nh_, "point_cloud", 1)
+ {
+ ros::NodeHandle local_nh("~");
+ local_nh.param("out_directory",out_directory_,std::string("/tmp/"));
+ frame_ = "base_link";
+ tf_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud>(pc_sub_, tf_, frame_,1);
+ tf_filter_->registerCallback(boost::bind(&PointCloudWriter::pointCloudCB, this, _1));
+ tf_filter_->setTolerance(ros::Duration(0.03));
+ }
+
+ ~PointCloudWriter()
+ {
+ delete tf_filter_;
+ }
+
+ void pointCloudCB(const sensor_msgs::PointCloudConstPtr& pc)
+ {
+
+ // Convert the point cloud to the desired frame.
+ sensor_msgs::PointCloud pc_transformed;
+ tf_.transformPointCloud(frame_, *pc, pc_transformed);
+
+ std::stringstream ss;
+ ss << out_directory_ << "/" << pc->header.stamp << "." << frame_ << ".xyz";
+ std::fstream file(ss.str().c_str(), std::fstream::out);
+
+ ROS_INFO_STREAM("Writing file "<<ss.str());
+
+ // Write out the point cloud including all of the channels.
+ ROS_INFO_STREAM("Num points " << pc_transformed.points.size());
+ for (uint i=0; i<pc_transformed.points.size(); ++i)
+ {
+ file << pc_transformed.points[i].x << " " << pc_transformed.points[i].y << " " << pc_transformed.points[i].z;
+
+ for (uint j=0; j<pc_transformed.channels.size(); ++j)
+ file << " " << pc_transformed.channels[j].values[i];
+
+ file << std::endl;
+ }
+
+ file.close();
+
+ ROS_INFO_STREAM("File closed");
+ }
+
+protected:
+ ros::NodeHandle nh_;
+ tf::TransformListener tf_;
+ message_filters::Subscriber<sensor_msgs::PointCloud> pc_sub_;
+ std::string frame_;
+ tf::MessageFilter<sensor_msgs::PointCloud> *tf_filter_;
+ std::string out_directory_;
+};
+
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "point_cloud_to_file");
+ PointCloudWriter pc;
+
+ ros::spin();
+
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-09-04 17:35:47
|
Revision: 23836
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23836&view=rev
Author: tfoote
Date: 2009-09-04 17:35:40 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
trying to fix #2734
Modified Paths:
--------------
pkg/trunk/stacks/common/filters/include/filters/median.h
Modified: pkg/trunk/stacks/common/filters/include/filters/median.h
===================================================================
--- pkg/trunk/stacks/common/filters/include/filters/median.h 2009-09-04 17:27:20 UTC (rev 23835)
+++ pkg/trunk/stacks/common/filters/include/filters/median.h 2009-09-04 17:35:40 UTC (rev 23836)
@@ -156,7 +156,7 @@
template <typename T>
bool MedianFilter<T>::update(const T& data_in, T& data_out)
{
- if (!this->configured_)
+ if (!FilterBase<T>::configured_)
return false;
data_storage_->push_back(data_in);
@@ -244,7 +244,7 @@
// printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_)
return false;
- if (!this->configured_)
+ if (!FilterBase<T>::configured_)
return false;
data_storage_->push_back(data_in);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-09-04 17:27:29
|
Revision: 23835
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23835&view=rev
Author: vijaypradeep
Date: 2009-09-04 17:27:20 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
LaserCbDetector compiles. Probably buggy. Unit tests need to be written.
Modified Paths:
--------------
pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt
pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/calibration/laser_cb_detector/action/
pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action
pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h
pkg/trunk/stacks/calibration/laser_cb_detector/msg/
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg
pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigResult.msg
pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt 2009-09-04 17:05:59 UTC (rev 23834)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/CMakeLists.txt 2009-09-04 17:27:20 UTC (rev 23835)
@@ -16,9 +16,10 @@
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-rosbuild_add_library(${PROJECT_NAME} src/cv_laser_bridge.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/cv_laser_bridge.cpp
+ src/laser_cb_detector.cpp)
-# rosbuild_genmsg()
+rosbuild_genmsg()
# rosbuild_gensrv()
#common commands for building c++ executables and libraries
Added: pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/action/Config.action 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,20 @@
+uint32 num_x # Number of checkerboard corners in the X direction
+uint32 num_y # Number of corners in the Y direction
+float32 spacing_x # Spacing between corners in the X direction (meters)
+float32 spacing_y # Spacing between corners in the Y direction (meters)
+
+# Specify how many times we want to upsample the image.
+# This is often useful for detecting small checkerboards far away
+float32 width_scaling
+float32 height_scaling
+
+# Specifiy how intensity maps into a uint8. A specified window of
+# intensities is linearly scaled to 0-255
+float32 min_intensity
+float32 max_intensity
+
+uint32 subpixel_window
+int32 subpixel_zero_zone
+
+---
+---
Added: pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,67 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#ifndef LASER_CB_DETECTOR_LASER_CB_DETECTOR_H_
+#define LASER_CB_DETECTOR_LASER_CB_DETECTOR_H_
+
+#include <opencv/cv.h>
+#include <calibration_msgs/DenseLaserSnapshot.h>
+#include <laser_cb_detector/ConfigAction.h>
+#include <laser_cb_detector/cv_laser_bridge.h>
+#include <camera_calibration/CalibrationPattern.h>
+
+namespace laser_cb_detector
+{
+
+class LaserCbDetector
+{
+public:
+ LaserCbDetector();
+
+ bool configure(const ConfigGoal& config);
+
+ bool detect(const calibration_msgs::DenseLaserSnapshot& snapshot,
+ camera_calibration::CalibrationPattern& result);
+
+private:
+ bool configured_;
+ ConfigGoal config_;
+ CvLaserBridge bridge_;
+};
+
+}
+
+#endif
Modified: pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml 2009-09-04 17:05:59 UTC (rev 23834)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/manifest.xml 2009-09-04 17:27:20 UTC (rev 23835)
@@ -11,7 +11,8 @@
<depend package="calibration_msgs"/>
<depend package="roscpp"/>
<depend package="opencv_latest" />
-
+ <depend package="actionlib_msgs" />
+ <depend package="camera_calibration" />
</package>
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigAction.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+ConfigActionGoal action_goal
+ConfigActionResult action_result
+ConfigActionFeedback action_feedback
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionFeedback.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalStatus status
+ConfigFeedback feedback
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionGoal.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalID goal_id
+ConfigGoal goal
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigActionResult.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalStatus status
+ConfigResult result
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigFeedback.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1 @@
+
Added: pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/msg/ConfigGoal.msg 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,18 @@
+uint32 num_x # Number of checkerboard corners in the X direction
+uint32 num_y # Number of corners in the Y direction
+float32 spacing_x # Spacing between corners in the X direction (meters)
+float32 spacing_y # Spacing between corners in the Y direction (meters)
+
+# Specify how many times we want to upsample the image.
+# This is often useful for detecting small checkerboards far away
+float32 width_scaling
+float32 height_scaling
+
+# Specifiy how intensity maps into a uint8. A specified window of
+# intensities is linearly scaled to 0-255
+float32 min_intensity
+float32 max_intensity
+
+uint32 subpixel_window
+int32 subpixel_zero_zone
+
Added: pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp
===================================================================
--- pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/laser_cb_detector/src/laser_cb_detector.cpp 2009-09-04 17:27:20 UTC (rev 23835)
@@ -0,0 +1,124 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <laser_cb_detector/laser_cb_detector.h>
+
+using namespace std;
+using namespace laser_cb_detector;
+
+LaserCbDetector::LaserCbDetector() : configured_(false)
+{
+
+}
+
+
+
+bool LaserCbDetector::configure(const ConfigGoal& config)
+{
+ config_ = config;
+
+ return true;
+}
+
+
+bool LaserCbDetector::detect(const calibration_msgs::DenseLaserSnapshot& snapshot,
+ camera_calibration::CalibrationPattern& result)
+{
+
+ // ***** Convert the snapshot into an image, based on intensity window in config *****
+ if(!bridge_.fromIntensity(snapshot, config_.min_intensity, config_.max_intensity))
+ return false;
+ IplImage* image = bridge_.toIpl();
+
+ // ***** Resize the image based on scaling parameters in config *****
+ const int scaled_width = (int) (.5 + image->width * config_.width_scaling);
+ const int scaled_height = (int) (.5 + image->height * config_.height_scaling);
+ IplImage* image_scaled = cvCreateImage(cvSize( scaled_width, scaled_height),
+ image->depth,
+ image->nChannels);
+ cvResize(image, image_scaled, CV_INTER_LINEAR);
+
+ // ***** Allocate vector for found corners *****
+ vector<CvPoint2D32f> cv_corners;
+ cv_corners.resize(config_.num_x*config_.num_y);
+
+ // ***** Do the actual checkerboard extraction *****
+ CvSize board_size = cvSize(config_.num_x, config_.num_y);
+ int num_corners ;
+ int found = cvFindChessboardCorners( image_scaled, board_size, &cv_corners[0], &num_corners,
+ CV_CALIB_CB_ADAPTIVE_THRESH) ;
+
+ if(found)
+ {
+ CvSize subpixel_window = cvSize(config_.subpixel_window,
+ config_.subpixel_window);
+ CvSize subpixel_zero_zone = cvSize(config_.subpixel_zero_zone,
+ config_.subpixel_zero_zone);
+
+ // Subpixel fine-tuning stuff
+ cvFindCornerSubPix(image_scaled, &cv_corners[0], num_corners,
+ subpixel_window,
+ subpixel_zero_zone,
+ cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
+ }
+ cvReleaseImage(&image_scaled);
+
+ // ***** Downscale the detected corners and generate the CalibrationPattern message *****
+ result.header.stamp = snapshot.header.stamp;
+ result.header.frame_id = snapshot.header.frame_id;
+
+ result.object_points.resize(config_.num_x * config_.num_y);
+ for (unsigned int i=0; i < config_.num_y; i++)
+ {
+ for (unsigned int j=0; j < config_.num_x; j++)
+ {
+ result.object_points[i*config_.num_x + j].x = j*config_.spacing_x;
+ result.object_points[i*config_.num_y + j].y = j*config_.spacing_y;
+ result.object_points[i*config_.num_y + j].z = 0.0;
+ }
+ }
+
+ result.image_points.resize(num_corners);
+ for (int i=0; i < num_corners; i++)
+ {
+ result.image_points[i].x = cv_corners[i].x / config_.width_scaling;
+ result.image_points[i].y = cv_corners[i].y / config_.height_scaling;
+ }
+
+ result.success = found;
+
+ return true;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-09-04 17:06:05
|
Revision: 23834
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23834&view=rev
Author: meeussen
Date: 2009-09-04 17:05:59 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
add joint state test
Modified Paths:
--------------
pkg/trunk/stacks/pr2_simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch
Modified: pkg/trunk/stacks/pr2_simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch
===================================================================
--- pkg/trunk/stacks/pr2_simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch 2009-09-04 17:04:13 UTC (rev 23833)
+++ pkg/trunk/stacks/pr2_simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch 2009-09-04 17:05:59 UTC (rev 23834)
@@ -27,6 +27,16 @@
<!-- Note how we use a different parameter namespace and node name
for this test (mechanism_state_test vs. scan_test). -->
<param name="topic" value="/mechanism_state" />
+ <param name="hz" value="1.0" />
+ <param name="hzerror" value="0.5" />
+ <param name="test_duration" value="10.0" />
+ <param name="check_intervals" value="false" />
+ </test>
+ <!-- Test for publication of 'mechanism_state' topic. -->
+ <test test-name="hztest_test_joint_state" pkg="test_pr2_mechanism_controllers_gazebo" type="hztest" time-limit="50" name="joint_state_test">
+ <!-- Note how we use a different parameter namespace and node name
+ for this test (mechanism_state_test vs. scan_test). -->
+ <param name="topic" value="/joint_states" />
<param name="hz" value="100.0" />
<param name="hzerror" value="2.0" />
<param name="test_duration" value="10.0" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-09-04 17:04:26
|
Revision: 23833
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23833&view=rev
Author: eitanme
Date: 2009-09-04 17:04:13 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
No longer needs a dependency on 3dnav_pr2 because the robot_self_filter has been updated
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml 2009-09-04 16:16:14 UTC (rev 23832)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml 2009-09-04 17:04:13 UTC (rev 23833)
@@ -1,7 +1,5 @@
<launch>
-<include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
<node pkg="mux" type="throttle" args="3.0 base_scan base_scan_throttled" />
<node pkg="mux" type="throttle" args="3.0 tilt_scan tilt_scan_throttled" />
<node pkg="mux" type="throttle" args="3.0 tilt_scan_filtered tilt_scan_filtered_throttled" />
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml 2009-09-04 16:16:14 UTC (rev 23832)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml 2009-09-04 17:04:13 UTC (rev 23833)
@@ -20,7 +20,6 @@
<depend package="backup_safetysound"/>
<depend package="geometry_msgs"/>
<depend package="robot_self_filter"/>
- <depend package="3dnav_pr2"/>
<!-- packages used in the test scripts -->
<depend package="robot_actions"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-09-04 16:50:24
|
Revision: 23798
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23798&view=rev
Author: jfaustwg
Date: 2009-09-04 00:27:31 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
makefile
Modified Paths:
--------------
pkg/branches/visualization-0.9/Makefile
Modified: pkg/branches/visualization-0.9/Makefile
===================================================================
--- pkg/branches/visualization-0.9/Makefile 2009-09-04 00:25:58 UTC (rev 23797)
+++ pkg/branches/visualization-0.9/Makefile 2009-09-04 00:27:31 UTC (rev 23798)
@@ -1 +1,5 @@
-include $(shell rospack find mk)/cmake_stack.mk
+all:
+ cd rviz && make
+
+clean:
+ cd rviz && make clean
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 16:16:26
|
Revision: 23832
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23832&view=rev
Author: isucan
Date: 2009-09-04 16:16:14 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
keeping track of links to be updated
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-09-04 15:42:33 UTC (rev 23831)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-09-04 16:16:14 UTC (rev 23832)
@@ -3,7 +3,7 @@
rospack(planning_models)
rospack_add_boost_directories()
-set(ROS_BUILD_TYPE Debug)
+set(ROS_BUILD_TYPE Release)
rospack_add_library(planning_models src/kinematic_model.cpp
src/kinematic_state.cpp)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h 2009-09-04 15:42:33 UTC (rev 23831)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h 2009-09-04 16:16:14 UTC (rev 23832)
@@ -297,6 +297,9 @@
/** \brief The list of joints that are roots in this group */
std::vector<Joint*> jointRoots;
+ /** \brief The list of links that are updated when computeTransforms() is called, in the order they are updated */
+ std::vector<Link*> updatedLinks;
+
/** \brief Perform forward kinematics starting at the roots
within a group. Links that are not in the group are also
updated, but transforms for joints that are not in the
@@ -434,6 +437,9 @@
/** \brief The index at which a joint starts reading values in the state vector */
std::vector<unsigned int> jointIndex_;
+ /** \brief The list of links that are updated when computeTransforms() is called, in the order they are updated */
+ std::vector<Link*> updatedLinks_;
+
/** \brief The root joint */
Joint *root_;
@@ -453,7 +459,9 @@
btTransform rootTransform_;
boost::mutex lock_;
-
+
+
+ void buildConvenientDatastructures(void);
void buildGroups(const std::map< std::string, std::vector<std::string> > &groups);
Joint* buildRecursive(Link *parent, const urdf::Link *link);
Joint* constructJoint(const urdf::Joint *urdfJoint, std::vector<double> &bounds);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-09-04 15:42:33 UTC (rev 23831)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-09-04 16:16:14 UTC (rev 23832)
@@ -58,6 +58,7 @@
for (unsigned int i = 0 ; i < groups.size() ; ++i)
groupContent[groups[i]->name] = groups[i]->jointNames;
buildGroups(groupContent);
+ buildConvenientDatastructures();
}
else
root_ = NULL;
@@ -95,6 +96,7 @@
{
root_ = buildRecursive(NULL, root);
buildGroups(groups);
+ buildConvenientDatastructures();
}
}
else
@@ -173,6 +175,23 @@
computeTransforms(params);
}
+void planning_models::KinematicModel::buildConvenientDatastructures(void)
+{
+ if (root_)
+ {
+ std::queue<Link*> links;
+ links.push(root_->after);
+ while (!links.empty())
+ {
+ Link *link = links.front();
+ links.pop();
+ updatedLinks_.push_back(link);
+ for (unsigned int i = 0 ; i < link->after.size() ; ++i)
+ links.push(link->after[i]->after);
+ }
+ }
+}
+
void planning_models::KinematicModel::buildGroups(const std::map< std::string, std::vector<std::string> > &groups)
{
for (std::map< std::string, std::vector<std::string> >::const_iterator it = groups.begin() ; it != groups.end() ; ++it)
@@ -387,23 +406,13 @@
void planning_models::KinematicModel::computeTransforms(const double *params)
{
- for (unsigned int i = 0 ; i < jointList_.size() ; ++i)
+ const unsigned int js = jointList_.size();
+ for (unsigned int i = 0 ; i < js ; ++i)
jointList_[i]->updateVariableTransform(params + jointIndex_[i]);
- if (root_)
- {
- std::queue<Link*> links;
- links.push(root_->after);
- while (!links.empty())
- {
- Link *link = links.front();
- links.pop();
-
- link->computeTransform();
- for (unsigned int i = 0 ; i < link->after.size() ; ++i)
- links.push(link->after[i]->after);
- }
- }
+ const unsigned int ls = updatedLinks_.size();
+ for (unsigned int i = 0 ; i < ls ; ++i)
+ updatedLinks_[i]->computeTransform();
}
const planning_models::KinematicModel::Joint* planning_models::KinematicModel::getRoot(void) const
@@ -862,6 +871,21 @@
if (!found)
jointRoots.push_back(joints[i]);
}
+
+ for (unsigned int i = 0 ; i < jointRoots.size() ; ++i)
+ {
+ std::queue<Link*> links;
+ links.push(jointRoots[i]->after);
+
+ while (!links.empty())
+ {
+ Link *link = links.front();
+ links.pop();
+ updatedLinks.push_back(link);
+ for (unsigned int i = 0 ; i < link->after.size() ; ++i)
+ links.push(link->after[i]->after);
+ }
+ }
}
planning_models::KinematicModel::JointGroup::~JointGroup(void)
@@ -887,22 +911,11 @@
void planning_models::KinematicModel::JointGroup::computeTransforms(const double *params)
{
- for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ const unsigned int js = joints.size();
+ for (unsigned int i = 0 ; i < js ; ++i)
joints[i]->updateVariableTransform(params + jointIndex[i]);
- for (unsigned int i = 0 ; i < jointRoots.size() ; ++i)
- {
- std::queue<Link*> links;
- links.push(jointRoots[i]->after);
-
- while (!links.empty())
- {
- Link *link = links.front();
- links.pop();
-
- link->computeTransform();
- for (unsigned int i = 0 ; i < link->after.size() ; ++i)
- links.push(link->after[i]->after);
- }
- }
+ const unsigned int ls = updatedLinks.size();
+ for (unsigned int i = 0 ; i < ls ; ++i)
+ updatedLinks[i]->computeTransform();
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
