Merge branch 'kinetic-devel' into pr-259

README.md

@@ -1,23 +1,8 @@
 # Motoman
 
-
-## ROS Distro Support
-
-|         | Indigo | Jade | Kinetic |
-|:-------:|:------:|:----:|:-------:|
-| Branch  | [`indigo-devel`](https://github.com/ros-industrial/motoman/tree/indigo-devel) | [`jade-devel`](https://github.com/ros-industrial/motoman/tree/jade-devel) | [`kinetic-devel`](https://github.com/ros-industrial/motoman/tree/kinetic-devel) |
-| Status  |  supported | supported |  supported |
-| Version | [version](http://repositories.ros.org/status_page/ros_indigo_default.html?q=motoman) | [version](http://repositories.ros.org/status_page/ros_jade_default.html?q=motoman) | [version](http://repositories.ros.org/status_page/ros_kinetic_default.html?q=motoman) |
-
-## Travis - Continuous Integration
-
-Status: [![Build Status](https://travis-ci.org/ros-industrial/motoman.svg?branch=kinetic-devel)](https://travis-ci.org/ros-industrial/motoman)
-
-## ROS Buildfarm
-
-|         | Indigo Source | Indigo Debian | Jade Source | Jade Debian |  Kinetic Source  |  Kinetic Debian |
-|:-------:|:-------------------:|:-------------------:|:-------------------:|:-------------------:|:-------------------:|:-------------------:|
-| motoman | [![not released](http://build.ros.org/buildStatus/icon?job=Isrc_uT__motoman__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__motoman__ubuntu_trusty__source/) | [![not released](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__motoman__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__motoman__ubuntu_trusty_amd64__binary/) | [![not released](http://build.ros.org/buildStatus/icon?job=Jsrc_uT__motoman__ubuntu_trusty__source)](http://build.ros.org/view/Jsrc_uT/job/Jsrc_uT__motoman__ubuntu_trusty__source/) | [![not released](http://build.ros.org/buildStatus/icon?job=Jbin_uT64__motoman__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Jbin_uT64/job/Jbin_uT64__motoman__ubuntu_trusty_amd64__binary/) | [![not released](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__motoman__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__motoman__ubuntu_xenial__source/) | [![not released](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__motoman__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__motoman__ubuntu_xenial_amd64__binary/) |
+[![Build Status](http://build.ros.org/job/Idev__motoman__ubuntu_trusty_amd64/badge/icon)](http://build.ros.org/job/Idev__motoman__ubuntu_trusty_amd64)
+[![License](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
 
 [![support level: consortium / vendor](https://img.shields.io/badge/support%20level-consortium%20/%20vendor-brightgreen.png)](http://rosindustrial.org/news/2016/10/7/better-supporting-a-growing-ros-industrial-software-platform)
 
@@ -30,11 +15,31 @@ The [motoman_experimental][] repository contains additional packages.
 
 Branch naming follows the ROS distribution they are compatible with. `-devel`
 branches may be unstable. Releases are made from the distribution branches
-(`hydro`, `indigo`).
+(`indigo`, `kinetic`).
 
-Older releases may be found in the old ROS-Industrial [subversion repository][]
+Older releases may be found in a mirror of the old ROS-Industrial [subversion repository][]
 archive.
 
+
+## ROS Distro Support
+
+|         | Indigo | Kinetic |
+|:-------:|:------:|:-------:|
+| Branch  | [`indigo-devel`](https://github.com/ros-industrial/motoman/tree/indigo-devel) | [`kinetic-devel`](https://github.com/ros-industrial/motoman/tree/kinetic-devel) |
+| Status  |  supported | supported |
+| Version | [version](http://repositories.ros.org/status_page/ros_indigo_default.html?q=motoman) | [version](http://repositories.ros.org/status_page/ros_kinetic_default.html?q=motoman) |
+
+## Travis - Continuous Integration
+
+Status: [![Build Status](https://travis-ci.org/ros-industrial/motoman.svg?branch=kinetic-devel)](https://travis-ci.org/ros-industrial/motoman)
+
+## ROS Buildfarm
+
+|         | Indigo Source | Indigo Debian | Kinetic Source  |  Kinetic Debian |
+|:-------:|:-------------------:|:-------------------:|:-------------------:|:-------------------:|
+| motoman | [![not released](http://build.ros.org/buildStatus/icon?job=Isrc_uT__motoman__ubuntu_trusty__source)](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__motoman__ubuntu_trusty__source/) | [![not released](http://build.ros.org/buildStatus/icon?job=Ibin_uT64__motoman__ubuntu_trusty_amd64__binary)](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__motoman__ubuntu_trusty_amd64__binary/) | [![not released](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__motoman__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__motoman__ubuntu_xenial__source/) | [![not released](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__motoman__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__motoman__ubuntu_xenial_amd64__binary/) |
+
+
 [ROS-Industrial]: http://wiki.ros.org/Industrial
 [ROS wiki]: http://wiki.ros.org/motoman
 [motoman_experimental]: https://github.com/ros-industrial/motoman_experimental

motoman_driver/MotoPlus/Controller.h

@@ -73,7 +73,7 @@
 
 #define ERROR_MSG_MAX_SIZE 64
 
-#define START_MAX_PULSE_DEVIATION 10
+#define START_MAX_PULSE_DEVIATION 30
 
 #define CONTROLLER_STATUS_UPDATE_PERIOD 10
 

motoman_driver/MotoPlus/MotionServer.c

@@ -53,6 +53,7 @@ int Ros_MotionServer_InitTrajPointFullEx(CtrlGroup* ctrlGroup, SmBodyJointTrajPt
 int Ros_MotionServer_AddTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFull* jointTrajData);
 int Ros_MotionServer_AddTrajPointFullEx(CtrlGroup* ctrlGroup, SmBodyJointTrajPtExData* jointTrajDataEx, int sequence);
 int Ros_MotionServer_JointTrajPtFullExProcess(Controller* controller, SimpleMsg* receiveMsg, SimpleMsg* replyMsg);
+int Ros_MotionServer_GetDhParameters(Controller* controller, SimpleMsg* replyMsg);
 
 // AddToIncQueue Task:
 void Ros_MotionServer_AddToIncQueueProcess(Controller* controller, int groupNo);
@@ -282,6 +283,9 @@ int Ros_MotionServer_GetExpectedByteSizeForMessageType(SimpleMsg* receiveMsg, in
 	case ROS_MSG_MOTO_WRITE_IO_GROUP:
 		expectedSize = minSize + sizeof(SmBodyMotoWriteIOGroup);
 		break;
+	case ROS_MSG_MOTO_GET_DH_PARAMETERS:
+		expectedSize = minSize; //no additional data on the request
+		break;
 	default: //invalid message type
 		return -1;
 	}
@@ -454,6 +458,11 @@ int Ros_MotionServer_SimpleMsgProcess(Controller* controller, SimpleMsg* receive
 		ret = Ros_IoServer_WriteIOGroup(receiveMsg, replyMsg);
 		break;
 
+	//-----------------------
+	case ROS_MSG_MOTO_GET_DH_PARAMETERS:
+		ret = Ros_MotionServer_GetDhParameters(controller, replyMsg);
+		break;
+
 	//-----------------------
 	default:
 		printf("Invalid message type: %d\n", receiveMsg->header.msgType);
@@ -1074,6 +1083,15 @@ int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* rec
 		return 0;
 	}
 
+	//Due to data loss when converting from double (ROS PC) to float (Simple Message serialization), we cannot accept
+	//a single trjectory that lasts more than 4 hours.
+	if (trajData->time >= MAX_TRAJECTORY_TIME_LENGTH)
+	{
+		printf("ERROR: Trajectory time (%.4f) > MAX_TRAJECTORY_TIME_LENGTH (%.4f)\r\n", trajData->time, MAX_TRAJECTORY_TIME_LENGTH);
+		Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_TIME, replyMsg, receiveMsg->body.jointTrajData.groupNo);
+		return 0;
+	}
+
 	// Check the trajectory sequence code
 	if(trajData->sequence == 0) // First trajectory point
 	{
@@ -1823,3 +1841,35 @@ STATUS Ros_MotionServer_DisableEcoMode(Controller* controller)
 	else
 		return NG;
 }
+
+int Ros_MotionServer_GetDhParameters(Controller* controller, SimpleMsg* replyMsg)
+{
+	int i; 
+	STATUS apiRet = OK;
+
+	//initialize memory
+	memset(replyMsg, 0x00, sizeof(SimpleMsg));
+
+	// set prefix: length of message excluding the prefix
+	replyMsg->prefix.length = sizeof(SmHeader) + sizeof(SmBodyMotoGetDhParameters);
+
+	// set header information of the reply
+	replyMsg->header.msgType = ROS_MSG_MOTO_GET_DH_PARAMETERS;
+	replyMsg->header.commType = ROS_COMM_SERVICE_REPLY;
+	replyMsg->header.replyType = ROS_REPLY_SUCCESS;
+
+	for (i = 0; i < controller->numGroup; i += 1)
+	{
+		if (controller->ctrlGroups[i] != NULL && replyMsg->header.replyType == ROS_REPLY_SUCCESS)
+		{
+			apiRet = GP_getDhParameters(i, &replyMsg->body.dhParameters.dhParameters[i]);
+
+			if (apiRet == OK)
+				replyMsg->header.replyType = ROS_REPLY_SUCCESS;
+			else
+				replyMsg->header.replyType = ROS_REPLY_FAILURE;
+		}
+	}
+
+	return apiRet;
+}

motoman_driver/MotoPlus/MotionServer.h

@@ -32,10 +32,12 @@
 #ifndef MOTIONSERVER_H
 #define MOTIONSERVER_H
 
-#define MOTION_STOP_TIMEOUT 20
-#define MOTION_START_TIMEOUT 5000  // in milliseconds
-#define MOTION_START_CHECK_PERIOD 50  // in millisecond
-#define MOTION_INIT_ROS_JOB "INIT_ROS"
+#define MOTION_STOP_TIMEOUT			20
+#define MOTION_START_TIMEOUT		5000  // in milliseconds
+#define MOTION_START_CHECK_PERIOD	50  // in millisecond
+#define MOTION_INIT_ROS_JOB			"INIT_ROS"
+
+#define MAX_TRAJECTORY_TIME_LENGTH	14400.0f //seconds (4 hours)
 
 extern void Ros_MotionServer_StartNewConnection(Controller* controller, int sd);
 extern BOOL Ros_MotionServer_HasDataInQueue(Controller* controller);

motoman_driver/MotoPlus/MotoROS.h

@@ -32,7 +32,7 @@
 #ifndef MOTOROS_H
 #define MOTOROS_H
 
-#define APPLICATION_VERSION					"1.8.1"
+#define APPLICATION_VERSION					"1.8.2"
 
 #include "MotoPlus.h"
 #include "ParameterExtraction.h"

motoman_driver/MotoPlus/MpRosAllControllers.vcxproj

@@ -113,7 +113,7 @@
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='YRC1000|Win32'">
     <NMakeBuildCommandLine>mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1_" -o build</NMakeBuildCommandLine>
-    <NMakeOutput>MotoRosYRC1_.out</NMakeOutput>
+    <NMakeOutput>$(MP_VS_Install)DevTools\OnlineDownload.exe</NMakeOutput>
     <NMakePreprocessorDefinitions>$(NMakePreprocessorDefinitions);$(Configuration)</NMakePreprocessorDefinitions>
     <NMakeIncludeSearchPath>$(MP_VS_Install)$(Configuration)\inc</NMakeIncludeSearchPath>
     <NMakeReBuildCommandLine>mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1_" -o rebuild</NMakeReBuildCommandLine>
@@ -124,7 +124,7 @@
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='YRC1000u|Win32'">
     <NMakeBuildCommandLine>mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1u_" -o build</NMakeBuildCommandLine>
-    <NMakeOutput>MotoRosYRC1u_.out</NMakeOutput>
+    <NMakeOutput>$(MP_VS_Install)DevTools\OnlineDownload.exe</NMakeOutput>
     <NMakePreprocessorDefinitions>$(NMakePreprocessorDefinitions);$(Configuration)</NMakePreprocessorDefinitions>
     <NMakeIncludeSearchPath>$(MP_VS_Install)$(Configuration)\inc</NMakeIncludeSearchPath>
     <NMakeReBuildCommandLine>mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1u_" -o rebuild</NMakeReBuildCommandLine>

motoman_driver/MotoPlus/ParameterExtraction.h

@@ -217,6 +217,17 @@ extern STATUS 	GP_isSdaRobot(BOOL* bIsSda);
 /******************************************************************************/
 extern STATUS 	GP_isSharedBaseAxis(BOOL* bIsSharedBaseAxis);
 
+/******************************************************************************/
+/* << 24 >>                                                              	  */
+/* Function name : STATUS GP_getDhParameters()								  */
+/* Functionality : Retrieves DH parameters for a given control group.		  */
+/* Parameter	 : int ctrlGrp - Robot control group (zero based index) [IN]  */
+/*					DH_PARAMETERS* dh - Value of the DH parameters [OUT]      */
+/* Return value	 : Success = OK 											  */
+/*				 : Failure = NG												  */
+/******************************************************************************/
+extern STATUS 	GP_getDhParameters(int ctrlGrp, DH_PARAMETERS* dh);
+
 #ifdef __cplusplus
 }
 #endif

motoman_driver/MotoPlus/ParameterTypes.h

@@ -112,6 +112,19 @@ typedef struct
 	ULONG cioAddressForAxis[MAX_PULSE_AXES][2];
 } JOINT_FEEDBACK_SPEED_ADDRESSES;
 
+typedef struct
+{
+	float theta;
+	float d;
+	float a;
+	float alpha;
+} DH_LINK;
+
+typedef struct
+{
+	DH_LINK link[MAX_PULSE_AXES];
+} DH_PARAMETERS;
+
 #ifdef __cplusplus
 }
 #endif

motoman_driver/MotoPlus/SimpleMessage.h

@@ -71,7 +71,9 @@ typedef enum
 	ROS_MSG_MOTO_IOCTRL_REPLY = 2011,
 
 	ROS_MSG_MOTO_JOINT_TRAJ_PT_FULL_EX = 2016,
-	ROS_MSG_MOTO_JOINT_FEEDBACK_EX = 2017
+	ROS_MSG_MOTO_JOINT_FEEDBACK_EX = 2017,
+
+	ROS_MSG_MOTO_GET_DH_PARAMETERS = 2020
 } SmMsgType;
 
 
@@ -134,7 +136,8 @@ typedef enum
 	ROS_RESULT_INVALID_DATA_POSITION,
 	ROS_RESULT_INVALID_DATA_SPEED,
 	ROS_RESULT_INVALID_DATA_ACCEL,
-	ROS_RESULT_INVALID_DATA_INSUFFICIENT
+	ROS_RESULT_INVALID_DATA_INSUFFICIENT,
+	ROS_RESULT_INVALID_DATA_TIME
 } SmInvalidSubCode;
 
 
@@ -321,6 +324,16 @@ struct _SmBodyMotoIoCtrlReply	// ROS_MSG_MOTO_IOCTRL_REPLY = 2011
 } __attribute__((__packed__));
 typedef struct _SmBodyMotoIoCtrlReply SmBodyMotoIoCtrlReply;
 
+//--------------
+// DH Parameters
+//--------------
+
+struct _SmBodyMotoGetDhParameters
+{
+	DH_PARAMETERS dhParameters[MOT_MAX_GR];
+} __attribute__((__packed__));
+typedef struct _SmBodyMotoGetDhParameters SmBodyMotoGetDhParameters;
+
 //--------------
 // Body Union
 //--------------
@@ -343,6 +356,7 @@ typedef union
 	SmBodyMotoWriteIOGroup writeIOGroup;
 	SmBodyMotoWriteIOGroupReply writeIOGroupReply;
 	SmBodyMotoIoCtrlReply ioCtrlReply;
+	SmBodyMotoGetDhParameters dhParameters;
 } SmBody;
 
 //-------------------