目录
点击此处下载项目文件：力矩传感器通信传输数据，直接下载运行可用
1. 创建工作空间
mkdir -p ./robotiq_ft/src
cd ./robotiq_ft/
catkin_make
code .2.设置tasks.json文件
Ctrl+Shift+B，No build to run found -> Create tasks.jason file from template -> Others example to run an arbitrary externalcommand,把下面的代码复制进tasks.json文件中
{
// 有关 tasks.json 格式的文档，请参见
// https://go.microsoft.com/fwlink/?LinkId=733558
"version": "2.0.0",
"tasks": [
{
"label": "catkin_make:debug", //代表提示的描述性信息
"type": "shell", //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
"command": "catkin_make",//这个是我们需要运行的命令
"args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
"group": {"kind":"build","isDefault":true},
"presentation": {
"reveal": "always"//可选always或者silence，代表是否输出信息
},
"problemMatcher": "$msCompile"
}
]
}再次按 Ctrl+Shift+B编译，编译成功即可
3.创建功能包
在src处右键，创建功能包robotiq_ft，添加roscpp rospy std_msgs
4. 修改c_cpp_properties.json文件
注意，要在includePath中，将"/home/yjw/Desktop/my_robotiq_ft/src/robotiq_ft/include/**"改成自己的功能包include文件夹所在的路径，方法是在include处打开终端，输入pwd命令就会出现路径
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/opt/ros/melodic/include/**",
"/usr/include/**",
"/home/yjw/Desktop/my_robotiq_ft/src/robotiq_ft/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}5. 修改内部的CMakeLists.txt文件
cmake_minimum_required(VERSION 3.0.2)
project(robotiq_ft)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES robotiq_ft
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
include_directories("/usr/include/eigen3")
add_library(robotiq_lib # 自己定义库的名字robotiq_lib
include/${PROJECT_NAME}/mutex.h # 调用的自定义头文件，${PROJECT_NAME}表示功能包名
include/${PROJECT_NAME}/rq_int.h
include/${PROJECT_NAME}/rq_sensor_com.h
include/${PROJECT_NAME}/rq_sensor_socket.h
include/${PROJECT_NAME}/rq_sensor_state.h
include/${PROJECT_NAME}/rq_thread.h
src/mutex.c # 调用的自定义源文件
src/my_robotiq_ft.cpp
src/rq_sensor_com.c
src/rq_sensor_socket.c
src/rq_sensor_state.c
src/rq_thread.c
)
add_executable(my_robotiq_ft src/my_robotiq_ft.cpp)
add_executable(robotiq_sub
src/robotiq_sub.cpp
)
# find_package(Franka 0.7.0 REQUIRED)
add_dependencies(robotiq_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(my_robotiq_ft ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(robotiq_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(robotiq_lib
# ${Franka_LIBRARIES}
${catkin_LIBRARIES}
)
target_link_libraries(my_robotiq_ft
robotiq_lib
# ${Franka_LIBRARIES}
${catkin_LIBRARIES}
)
target_link_libraries(robotiq_sub
robotiq_lib
# ${Franka_LIBRARIES}
${catkin_LIBRARIES}
)
6. 创建.launch .h .c .cpp文件
所有文件如下所示
6.1 创建launch文件
在生成的robotiq_ft功能包目录中右键，添加launch文件夹，然后在launch文件夹中添加my_robotiq_ft.launch文件
<launch>
<node name="my_robotiq_ft" pkg="robotiq_ft" type="my_robotiq_ft" />
</launch>6.2 创建.h源文件
在include目录下创建mutex.h，rq_int.h，rq_sensor_com.h，rq_sensor_socket.h，rq_sensor_state.h，rq_thread.h 具体代码如下(这里要检查一下include文件夹名是否显示为include/robotiq_ft，如果robotiq_ft没有跟在inlcude后面，则会报错)
mutex.h
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file mutex.h
* \date July 2, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifndef MUTEX_H_
#define MUTEX_H_
#ifdef __unix__ //For Unix
#include <pthread.h>
#define MUTEX pthread_mutex_t
#elif defined(_WIN32)||defined(WIN32) //For Windows
#include <windows.h>
#include <process.h>
#define MUTEX HANDLE
#endif
//Functions
int MUTEX_INIT(MUTEX *mutex);
int MUTEX_LOCK(MUTEX *mutex);
int MUTEX_UNLOCK(MUTEX *mutex);
#endif /* MUTEX_H_ */
rq_int.h
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_int.h
* \date June 30, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifndef RQ_INT_H_
#define RQ_INT_H_
typedef char INT_8;
typedef short INT_16;
typedef int INT_32;
typedef long INT_64;
typedef unsigned char UINT_8;
typedef unsigned short UINT_16;
typedef unsigned int UINT_32;
typedef unsigned long UINT_64;
#endif
rq_sensor_com.h
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_sensor_com.h
* \date June 19, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifndef RQ_SENSOR_COM_H
#define RQ_SENSOR_COM_H
#include "rq_int.h"
#include <stdio.h>
#include <stdbool.h>
#define MP_BUFF_SIZE 1024
INT_8 rq_sensor_com(void);
void rq_sensor_com_read_info_high_lvl(void);
INT_8 rq_com_start_stream(void);
void rq_com_listen_stream(void);
bool rq_com_get_stream_detected(void);
bool rq_com_get_valid_stream(void);
//Accesseur
void rq_com_get_str_serial_number(INT_8 * serial_number);
void rq_com_get_str_firmware_version(INT_8 * firmware_version);
void rq_com_get_str_production_year(INT_8 * production_year);
float rq_com_get_received_data(UINT_8 i);
bool rq_com_got_new_message(void);
void rq_com_do_zero_force_flag(void);
void stop_connection(void);
#endif //RQ_SENSOR_COM_H
rq_sensor_socket.h
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file socket.h
* \date May 20, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifndef RQ_SENSOR_SOCKET_H_
#define RQ_SENSOR_SOCKET_H_
#include <stdint.h>
#ifdef WIN32
#include <winsock2.h>
#elif defined (linux)
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h> /* close */
#include <netdb.h> /* gethostbyname */
#define INVALID_SOCKET -1
#define SOCKET_ERROR -1
#define closesocket(s) close(s)
typedef int SOCKET;
typedef struct sockaddr_in SOCKADDR_IN;
typedef struct sockaddr SOCKADDR;
typedef struct in_addr IN_ADDR;
#else
#error not defined for this platform
#endif
int init_connection();
void end_connection(int sock);
void write_client(SOCKET sock, uint8_t *buffer);
#ifdef __unix__ //For Unix
void *Read_socket(void *t);
#elif defined(_WIN32)||defined(WIN32) //For Windows
void __cdecl Read_socket( void *t );
#endif
void set_socket_message_to_null(void);
void socket_message(uint8_t * chr_return);
int getNbClient(void);
SOCKET get_client_sock_accessor(void);
SOCKET get_client_sock_stream(void);
SOCKET get_socket_stream();
#endif /* RQ_SENSOR_SOCKET_H_l */
rq_sensor_state.h
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_sensor_state.h
* \date June 19, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifndef RQ_SENSOR_STATE_H
#define RQ_SENSOR_STATE_H
#include "rq_int.h"
#include <stdbool.h>
enum rq_sensor_state_values
{
RQ_STATE_INIT, ///< State that initialize the com. with the sensor
RQ_STATE_READ_INFO, ///< State that reads the firmware version,
///< serial number and production year
RQ_STATE_START_STREAM, ///< State that start the sensor in streaming
///< mode
RQ_STATE_RUN ///< State that reads the streaming data from
///< the sensor
};
INT_8 rq_sensor_state(void);
void rq_state_get_command(INT_8 const * const name, INT_8 * const value);
void rq_set_zero(void);
enum rq_sensor_state_values rq_sensor_get_current_state(void);
bool rq_state_got_new_message(void);
float rq_state_get_received_data(UINT_8 i);
#endif //RQ_SENSOR_STATE_H
rq_thread.h
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_thread.h
* \date July 3, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifndef RQ_THREAD_H_
#define RQ_THREAD_H_
#ifdef __unix__ //For Unix
int create_Thread(void *(*start_rtn)(void *));
#elif defined(_WIN32)||defined(WIN32) //For Windows
int create_Thread(void( __cdecl *start_address )( void * ));
#endif
int close_Thread(int iId);
void wait_thread(int iId);
#endif /* RQ_THREAD_H_ */
6.3.创建src源文件
在生成的第二个src目录下创建mutex.c，my_robotiq_ft.cpp，robotiq_sub.cpp，rq_sensor_com.c，rq_sensor_socket.c，rq_sensor_state.c，rq_thread.c 具体代码如下
mutex.c
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file mutex.c
* \date July 2, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#ifdef __unix__ //For Unix
#include <pthread.h>
#define MUTEX pthread_mutex_t
#elif defined(_WIN32)||defined(WIN32) //For Windows
#include <windows.h>
#include <process.h>
#define MUTEX HANDLE
#endif
int MUTEX_INIT(MUTEX *mutex)
{
#ifdef __unix__ //For Unix
return pthread_mutex_init (mutex, NULL);;
#elif defined(_WIN32)||defined(WIN32) //For Windows
*mutex = CreateMutex(0, FALSE, 0);;
return (*mutex==0);
#endif
return -1;
}
int MUTEX_LOCK(MUTEX *mutex)
{
#ifdef __unix__ //For Unix
return pthread_mutex_lock( mutex );
#elif defined(_WIN32)||defined(WIN32) //For Windows
return (WaitForSingleObject(*mutex, INFINITE)==WAIT_FAILED?1:0);
#endif
return -1;
}
int MUTEX_UNLOCK(MUTEX *mutex)
{
#ifdef __unix__ //For Unix
return pthread_mutex_unlock( mutex );
#elif defined(_WIN32)||defined(WIN32) //For Windows
return (ReleaseMutex(*mutex)==0);
#endif
return -1;
}
my_robotiq_ft.cpp
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file main.c
* \date June 18, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
// 包含了 ROS、STL 和 Robotiq 的相关头文件。
// ROS 头文件 主要用于与 ROS 系统交互，处理消息传递和坐标变换。
#include "ros/ros.h"
// 这个头文件定义了 ROS 消息类型 geometry_msgs::WrenchStamped，它表示带时间戳和框架ID的力矩传感器数据。Wrench 是一个三维力和三维力矩的组合，通常用于表示力传感器输出的数据。
// geometry_msgs::WrenchStamped 消息类型包含以下字段：
// header: 消息头，包含时间戳和坐标系（frame_id）。
// wrench: 力矩数据，包含力（force）和力矩（torque）。
#include "geometry_msgs/WrenchStamped.h"
// 这个头文件定义了与 tf 库相关的数据类型和函数，tf 库用于 ROS 中不同坐标系之间的变换。transform_datatypes.h 包含了坐标变换所需的矩阵、四元数等数据结构及操作函数。
#include "tf/transform_datatypes.h"
// 该头文件提供了 tf::TransformBroadcaster 类，用于在 ROS 中发布坐标系变换。它允许你广播坐标系之间的变换（例如，发布从传感器坐标系到机器人基坐标系的变换）。
#include "tf/transform_broadcaster.h"
// STL头文件 提供了标准库的功能，主要用于线程管理和输入输出
#include <thread> // 这个头文件来自 C++ 标准库，用于线程管理。在这里，通常用于在 ROS 循环中进行阻塞式等待或者实现多线程操作。
#include <iostream> //这是 C++ 标准输入输出流库，提供了 std::cout、std::cin 等功能，用于输出调试信息或读取输入。
// Robotiq's code
// rq 命名空间下包含了 Robotiq 的 C 语言接口，提供与传感器通信的功能。
namespace rq {
extern "C" {
//提供了与 Robotiq 传感器通信的功能，通常用于建立与传感器的连接、发送和接收数据。
#include "robotiq_ft/rq_sensor_com.h"
// 定义了获取传感器状态的相关函数和数据结构，可以获取传感器的当前状态（如传感器是否工作正常、传感器的读取值等）。
#include "robotiq_ft/rq_sensor_state.h"
// 提供了与 Robotiq 接口的整数相关功能，通常用于处理与传感器接口通信时所需的整数数据类型和转换。
#include "robotiq_ft/rq_int.h"
// 提供了多线程相关功能，用于在进行传感器通信时确保线程的正确管理和同步。
#include "robotiq_ft/rq_thread.h"
// #include "robotiq_ft/mutex.h"
}
} // namespace rq
// 该函数不断检查与传感器的连接状态，直到成功连接为止。
// rq::rq_sensor_state() 返回 0 表示连接成功。
namespace rq {
void wait_for_other_connection() {
using namespace std::chrono_literals; //using namespace std::chrono_literals; 允许使用字面量（literal）来简化时间表示。在这里，它使得 1s 这个字面量表示 1 秒的时间，而不需要使用 std::chrono::seconds(1) 来表示。
rq::INT_8 ret; //定义了一个类型为 INT_8 的变量 ret。这个类型通常表示一个 8 位整数，用来存储与传感器状态相关的返回值。rq::rq_sensor_state() 函数将返回这个状态值。
ROS_DEBUG_STREAM("waiting for connection\n");
while (true) {
std::this_thread::sleep_for(1s); //std::this_thread::sleep_for(1s); 使得当前线程暂停（即“睡眠”）1秒
ret = rq::rq_sensor_state();
if (ret == 0) {
break;
}
}
}
}
// 该函数用于初始化传感器的连接，调用 rq::rq_sensor_state() 来检查传感器的连接状态。
// 如果传感器状态返回 -1，表示连接失败，会调用 wait_for_other_connection 等待重新连接。
void setup_robotiq() {
//IF can't connect with the sensor wait for another connection
rq::INT_8 ret = rq::rq_sensor_state();
ROS_DEBUG_STREAM("first\n");
if (ret == -1) {
rq::wait_for_other_connection();
// fprintf(stderr, "NO!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
}
//Read high-level informations
ret = rq::rq_sensor_state();
ROS_DEBUG_STREAM("second\n");
if (ret == -1) {
rq::wait_for_other_connection();
}
//Initialize connection with the client
ret = rq::rq_sensor_state();
ROS_DEBUG_STREAM("third\n");
if (ret == -1) {
rq::wait_for_other_connection();
}
}
int main(int argc, char** argv) {
const std::string pub_topic_param = "/ftsensor_node_name";
const std::string sensor_frame_id_param = "/ftsensor_frame";
setup_robotiq();
// 初始化 ROS 节点
ros::init(argc, argv, "my_robotiq_ft");
ros::NodeHandle ros_node;
// 这段代码的作用是从 ROS 参数服务器获取两个参数：
// pub_topic_param 对应的 node_name，这是发布话题的名称。
// sensor_frame_id_param 对应的 frame_id，这是传感器数据使用的坐标系框架 ID。
std::string node_name;
std::string frame_id;
const int queue_size = 10; //消息队列长度，多余的抛弃
// ???????????????????????????????????????????????????????
// if (!ros_node.getParam(pub_topic_param, node_name)) {
// ROS_ERROR_STREAM(pub_topic_param << " not found in parameter server!\n");
// return -1;
// }
// if (!ros_node.getParam(sensor_frame_id_param, frame_id)) {
// ROS_ERROR_STREAM(sensor_frame_id_param << " not found in parameter server!\n");
// return -1;S
// }
// 创建一个发布者 sensor_pub，它将消息类型设为 geometry_msgs::WrenchStamped，并将数据发布到 node_name 主题，队列大小为 10
// ros::Publisher sensor_pub = ros_node.advertise<geometry_msgs::WrenchStamped>(node_name, queue_size);
ros::Publisher sensor_pub = ros_node.advertise<geometry_msgs::WrenchStamped>("robotiq_ft", queue_size);
ROS_DEBUG_STREAM("ready!\n");
while (ros::ok()) {
// 调用 rq_sensor_state() 获取传感器状态
rq::INT_8 ret = rq::rq_sensor_state();
// 如果获取传感器状态失败，等待其他连接
if (ret == -1) {
rq::wait_for_other_connection();
}
// 如果传感器当前状态是 RUN 状态，继续执行
if (rq::rq_sensor_get_current_state() == rq::RQ_STATE_RUN) {
geometry_msgs::WrenchStamped msg;
// 设置消息头部信息
msg.header.frame_id = frame_id; // 坐标系 ID
msg.header.stamp = ros::Time(0); // 时间戳，设置为 0 表示无特定时间
// 填充力和力矩数据
msg.wrench.force.x = rq::rq_state_get_received_data(0); // 力的 X 分量
msg.wrench.force.y = rq::rq_state_get_received_data(1); // 力的 Y 分量
msg.wrench.force.z = rq::rq_state_get_received_data(2); // 力的 Z 分量
msg.wrench.torque.x = rq::rq_state_get_received_data(3); // 力矩的 X 分量
msg.wrench.torque.y = rq::rq_state_get_received_data(4); // 力矩的 Y 分量
msg.wrench.torque.z = rq::rq_state_get_received_data(5); // 力矩的 Z 分量
fprintf(stderr, "YES!\n");
// 发布消息到话题
sensor_pub.publish(msg);
// 调用 ros::spinOnce() 进行一次事件处理，处理订阅的消息
ros::spinOnce();
}
// ros::Duration(0.1).sleep();
}
return 0;
}
robotiq_sub.cpp
#include "ros/ros.h"
#include "geometry_msgs/WrenchStamped.h"// 引入 WrenchStamped 消息类型
// 定义一个回调函数来处理接收到的 WrenchStamped 消息
void wrenchCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg)
{
// 打印消息中的力和力矩信息
ROS_INFO("Received Wrench data:");
ROS_INFO("Force: [%.3f, %.3f, %.3f]", msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z);
ROS_INFO("Torque: [%.3f, %.3f, %.3f]", msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z);
}
int main(int argc, char** argv)
{
// 初始化 ROS 节点
ros::init(argc, argv, "wrench_listener");
ros::NodeHandle nh;
// 订阅传感器数据话题 (假设话题名称为 /sensor_wrench)
ros::Subscriber sub = nh.subscribe("robotiq_ft", 10, wrenchCallback);
// 进入循环，等待回调函数
ros::spin();
return 0;
}
rq_sensor_com.c
// /* Software License Agreement (BSD License)
// *
// * Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
// *
// * Copyright (c) 2014, Robotiq, Inc
// */
// /*
// * \file rq_sensor_com.c
// * \date June 18, 2014
// * \author Jonathan Savoie <jonathan.savoie@robotic.com>
// * \maintener Nicolas Lauzier <nicolas@robotiq.com>
// */
// //
// //Includes
// //Platform specific
// #ifdef __unix__ /*For Unix*/
// #define _BSD_SOURCE
// #include <termios.h>
// #include <unistd.h>
// #include <dirent.h>
// #include <fcntl.h>
// #include <unistd.h>
// #include <time.h>
// #define Sleep(x) (usleep (x*1000))
// #elif defined(_WIN32)||defined(WIN32) /*For Windows*/
// #include <windows.h>
// #endif
// #include <string.h>
// #include <errno.h>
// //application specific
// #include "robotiq_ft/rq_sensor_com.h"
// // Definitions
// #define REGISTER_SELECT_OUTPUT 410
// #define REGISTER_SERIAL_NUMBER 510
// #define REGISTER_PRODUCTION_YEAR 514
// #define REGISTER_FIRMWARE_VERSION 500
// #define RQ_COM_MAX_STR_LENGTH 20
// #define RQ_COM_JAM_SIGNAL_CHAR 0xff
// #define RQ_COM_JAM_SIGNAL_LENGTH 50
// #define RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE 20 //20 * 4ms = 80ms sans bytes avant de dire qu'on n'a pas le stream
// #define RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE 40 //40 * 4ms = 160ms sans messages valide afin de dire que la communication fonctionne mal (16 mauvais messages)
//
// // Private variables
// static float rq_com_received_data[6] = {0.0};
// static float rq_com_received_data_offset[6] = {0.0};
// static UINT_16 rq_com_computed_crc = 0;
// static UINT_16 rq_com_crc = 0;
// static INT_8 rq_com_str_sensor_serial_number[RQ_COM_MAX_STR_LENGTH];
// static INT_8 rq_com_str_sensor_production_year[RQ_COM_MAX_STR_LENGTH];
// static INT_8 rq_com_str_sensor_firmware_version[RQ_COM_MAX_STR_LENGTH];
// static UINT_32 rq_com_msg_received = 0;
// static UINT_8 rq_com_rcv_buff[MP_BUFF_SIZE];
// static UINT_8 rq_com_snd_buff[MP_BUFF_SIZE];
// static UINT_8 rq_com_rcv_buff2[MP_BUFF_SIZE];
// static INT_32 rq_com_rcv_len;
// static INT_32 rq_com_rcv_len2 = 0;
// static INT_32 rq_com_zero_force_flag = 0;
// static INT_32 rq_state_sensor_watchdog = 0;
// //variables related to the communication status
// static bool rq_com_stream_detected = false;
// static bool rq_com_valid_stream = false;
// static bool rq_com_new_message = false;
// static INT_32 rq_com_timer_for_stream_detection = 0;
// static INT_32 rq_com_timer_for_valid_stream = 0;
// ///
// //Private functions declaration
// static INT_8 rq_com_tentative_connexion(void);
// static void rq_com_send_jam_signal(void);
// static void rq_com_stop_stream_after_boot(void);
// static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len);
// static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len);
// //Modbus functions
// static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length );
// static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n);
// static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data);
// static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data);
// static INT_32 rq_com_wait_for_fc_16_echo(void);
// static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data);
// static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data);
// #ifdef __unix__ //For Unix
// static UINT_8 rq_com_identify_device(INT_8 const * const d_name);
// #endif //For Unix
// #ifdef __unix__ //For Unix
// static INT_32 fd_connexion = -1;
// static INT_8 set_com_attribs (INT_32 fd, speed_t speed);
// #elif defined(_WIN32)||defined(WIN32) //For Windows
// HANDLE hSerial;
// #endif
// #include <stdio.h>
// //
// //Function definitions
// /**
// * \fn void rq_sensor_com(void)
// * \brief Discovers and initialize the communication with the sensor
// * \details The functions loops through all the serial com ports
// * and polls them to discover the sensor
// */
// INT_8 rq_sensor_com()
// {
// #ifdef __unix__ //For Unix
// UINT_8 device_found = 0;
// DIR *dir = NULL;
// struct dirent *entrydirectory = NULL;
// //Close a previously opened connection to a device
// close(fd_connexion);
// if ((dir = opendir("/sys/class/tty/")) == NULL)
// {
// fprintf(stderr, "/sys/class/tty can't be opened");
// return -1;
// }
// //Loops through the files in the /sys/class/tty/ directory
// while ((entrydirectory = readdir(dir)) != NULL && device_found == 0)
// {
// //Look for a serial device
// if (strstr(entrydirectory->d_name, "ttyS") ||
// strstr(entrydirectory->d_name, "ttyUSB"))
// {
// fprintf(stderr, "found: %s\n", entrydirectory->d_name);
// device_found = rq_com_identify_device(entrydirectory->d_name);
// }
// }
// closedir(dir);
// if (device_found == 0)
// {
// fprintf(stderr, "no device found in /sys/class/tty\n");
// return -1;
// }
// #elif defined(_WIN32)||defined(WIN32) //For Windows
// DCB dcb;
// INT_32 i;
// INT_8 port[13];
// for(i = 0;i < 256; i++){
// sprintf(port,"\\\\.\\COM%d",i);
// hSerial = CreateFileA(port, GENERIC_READ | GENERIC_WRITE,
// 0,NULL,OPEN_EXISTING,0,NULL);
// if(hSerial != INVALID_HANDLE_VALUE){
// dcb.DCBlength = sizeof(DCB);
// if (!GetCommState(hSerial, &dcb)){
// CloseHandle(hSerial);
// hSerial = INVALID_HANDLE_VALUE;
// continue;//Permet de recommencer la boucle
// }
// dcb.BaudRate = CBR_19200;
// dcb.ByteSize = 8;
// dcb.StopBits = ONESTOPBIT;
// dcb.Parity = NOPARITY;
// dcb.fParity = FALSE;
// /* No software handshaking */
// dcb.fTXContinueOnXoff = TRUE;
// dcb.fOutX = FALSE;
// dcb.fInX = FALSE;
// //dcb.fNull = FALSE;
// /* Binary mode (it's the only supported on Windows anyway) */
// dcb.fBinary = TRUE;
// /* Don't want errors to be blocking */
// dcb.fAbortOnError = FALSE;
// /* Setup port */
// if(!SetCommState(hSerial, &dcb)){
// CloseHandle(hSerial);
// continue;//Permet de recommencer la boucle
// }
// COMMTIMEOUTS timeouts={0};
// timeouts.ReadIntervalTimeout=0;
// timeouts.ReadTotalTimeoutConstant=1;
// timeouts.ReadTotalTimeoutMultiplier=0;
// timeouts.WriteTotalTimeoutConstant=1;
// timeouts.WriteTotalTimeoutMultiplier=0;
// if(!SetCommTimeouts(hSerial, &timeouts)){
// CloseHandle(hSerial);
// hSerial = INVALID_HANDLE_VALUE;
// continue;//Permet de recommencer la boucle
// }
// if (rq_com_tentative_connexion() == 1){
// return 0;
// }
// CloseHandle(hSerial);
// }
// }
// return -1;
// #else
// #endif
// return 0;
// }
// /**
// * \fn static INT_8 rq_com_tentative_connexion()
// * \brief Tries connecting to the sensor
// * \returns 1 if the connection attempt succeeds, -1 otherwise
// */
// static INT_8 rq_com_tentative_connexion()
// {
// INT_32 rc = 0;
// UINT_16 firmware_version [1];
// UINT_8 retries = 0;
// while(retries < 5 && rq_com_stream_detected == false)
// {
// rq_com_listen_stream();
// retries++;
// }
// rq_com_listen_stream();
// if(rq_com_stream_detected)
// {
// rq_com_stop_stream_after_boot();
// }
// //Give some time to the sensor to switch to modbus
// Sleep(100);
// //If the device returns an F as the first character of the fw version,
// //we consider its a sensor
// rc = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 2, firmware_version);
// if (rc != -1)
// {
// if (firmware_version[0] >> 8 == 'F')
// {
// return 1;
// }
// }
// return -1;
// }
// /**
// * \fn static INT_8 set_com_attribs()
// * \brief Sets the com port parameters to match those of the sensor
// * \param fd, file descriptor that points to the serial port
// * \param speed, baudrate
// * \return 0 in case of a success, -1 otherwise
// */
// #ifdef __unix__ //For Unix
// static INT_8 set_com_attribs (INT_32 fd, speed_t speed)
// {
// struct termios tty;
// memset (&tty, 0, sizeof (struct termios));
// if (tcgetattr (fd, &tty) != 0)
// {
// return -1;
// }
// cfsetospeed (&tty, speed);
// cfsetispeed (&tty, speed);
// tty.c_cflag |= (CLOCAL | CREAD);
// tty.c_cflag &= ~CSIZE;
// tty.c_cflag |= CS8;
// tty.c_cflag &= ~CSTOPB;
// tty.c_cflag &= ~PARENB;
// tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// tty.c_iflag &= ~INPCK;
// tty.c_iflag &= ~(IXON | IXOFF | IXANY);
// tty.c_cc[VMIN] = 0; // read doesn't block
// tty.c_cc[VTIME] = 1;
// if (tcsetattr (fd, TCSANOW, &tty) != 0)
// {
// printf("error %d from tcsetattr", errno);
// return -1;
// }
// return 0;
// }
// #endif
// /**
// * \fn void rq_com_listen_stream()
// * \brief Listens and decode a valid stream input
// */
// void rq_com_listen_stream(void)
// {
// static INT_32 last_byte = 0;
// static INT_32 new_message = 0;
// INT_32 i = 0;
// INT_32 j = 0;
// //Capture and store the current value of the sensor and use it to
// //zero subsequent sensor values
// if(rq_com_zero_force_flag == 1)
// {
// rq_com_zero_force_flag = 0;
// for(i = 0; i < 6; i++)
// {
// rq_com_received_data_offset[i] = rq_com_received_data[i];
// }
// }
// Sleep(10);
// //Increment communication state counters
// if(rq_com_timer_for_stream_detection++ > RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE)
// {
// rq_com_timer_for_stream_detection = RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE;
// rq_com_stream_detected = false;
// }
// if(rq_com_timer_for_valid_stream++ > RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE)
// {
// rq_com_timer_for_valid_stream = RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE;
// rq_com_valid_stream = false;
// }
// last_byte = 0;
// new_message = 0;
// //Data reception
// rq_com_rcv_len = rq_com_read_port(rq_com_rcv_buff, MP_BUFF_SIZE);
// //If at least a byte is received, we consider the sensor to stream
// if(rq_com_rcv_len > 0)
// {
// rq_com_stream_detected = true;
// rq_com_timer_for_stream_detection = 0;
// }
// //Copie les données au bout du buffer 2
// for(i = 0; i < rq_com_rcv_len; i++)
// {
// //If the buffer overflows, set the index to the beginning
// if(rq_com_rcv_len2 == MP_BUFF_SIZE)
// {
// //Next bytes will overwrite the begining of the buffer
// rq_com_rcv_len2 = 0;
// break;
// }
// rq_com_rcv_buff2[rq_com_rcv_len2++] = rq_com_rcv_buff[i];
// }
// //empty the buffers
// memset( rq_com_rcv_buff, 0, sizeof(rq_com_rcv_buff));
// memset( rq_com_snd_buff, 0, sizeof(rq_com_snd_buff));
// //If there is enough characters,...
// if(rq_com_rcv_len2 >= 16 && rq_com_rcv_len >= 1)
// {
// //Search for a valid stream message in the buffer
// for(i = rq_com_rcv_len2 - 16; i >= 0; i--)
// {
// //Header
// if(rq_com_rcv_buff2[i] == 0x20 && rq_com_rcv_buff2[i+1] == 0x4E && new_message == 0)
// {
// last_byte = i - 1;
// rq_com_computed_crc = rq_com_compute_crc(rq_com_rcv_buff2 + i * sizeof(*rq_com_rcv_buff2), 14);
// rq_com_crc = (rq_com_rcv_buff2[i+14] + rq_com_rcv_buff2[i+15] * 256);
// rq_com_msg_received++;
// //The crc is valid.. the message can be used
// if(rq_com_computed_crc == rq_com_crc)
// {
// last_byte = i + 15; //will erase the message
// //convert the efforts to floating point numbers
// for(j = 0; j < 3; j++)
// {
// rq_com_received_data[j] = ((float)((INT_16)((UINT_8)rq_com_rcv_buff2[i+2+2*j] + ((INT_16)(UINT_8)rq_com_rcv_buff2[i+3+2*j]) * 256))) / 100 ;
// }
// for(j = 3; j < 6; j++)
// {
// rq_com_received_data[j] = ((float)((INT_16)((UINT_8)rq_com_rcv_buff2[i+2+2*j] + ((INT_16)(UINT_8)rq_com_rcv_buff2[i+3+2*j]) * 256))) / 1000;
// }
// //Signal the stream as valid
// rq_com_valid_stream = true;
// rq_com_new_message = true;
// rq_com_timer_for_valid_stream = 0;
// new_message = 1;
// rq_state_sensor_watchdog = 1;
// }
// else
// {
// last_byte = i + 15;
// new_message = 1;
// }
// }
// }
// if(last_byte > 0)
// {
// //On shift le buffer 2 afin de ne garder que ce qui dépasse le dernier caractère du dernier message complet
// for(i = 0; i < (rq_com_rcv_len2 - last_byte - 1); i++)
// {
// rq_com_rcv_buff2[i] = rq_com_rcv_buff2[last_byte + 1 + i];
// }
// rq_com_rcv_len2 = rq_com_rcv_len2 - last_byte - 1;
// }
// }
// }
// /**
// * \fn static void rq_com_send_jam_signal(void)
// * \brief Send a signal that interrupts the streaming
// */
// static void rq_com_send_jam_signal(void)
// {
// //Build the jam signal
// memset(rq_com_snd_buff, RQ_COM_JAM_SIGNAL_CHAR, RQ_COM_JAM_SIGNAL_LENGTH);
// //Send the jam signal
// rq_com_write_port(rq_com_snd_buff,RQ_COM_JAM_SIGNAL_LENGTH);
// }
// /**
// * \fn static void rq_com_stop_stream_after_boot(void)
// * \brief Send a jam signal to stop the sensor stream
// * and retry until the stream stops
// */
// static void rq_com_stop_stream_after_boot(void)
// {
// static INT_32 counter = 0;
// while(rq_com_stream_detected)
// {
// counter++;
// if(counter == 1)
// {
// rq_com_send_jam_signal();
// }
// else
// {
// rq_com_listen_stream();
// if(rq_com_stream_detected)
// {
// counter = 0;
// }
// else
// {
// counter = 0;
// }
// }
// }
// }
// /**
// * \fn INT_8 rq_com_start_stream(void)
// * \brief Starts the sensor streaming mode
// * \return 0 if the stream started, -1 otherwise
// */
// INT_8 rq_com_start_stream(void)
// {
// UINT_16 data[1] = {0x0200}; //0x0200 selects the stream output
// UINT_8 retries = 0;
// INT_8 rc = rq_com_send_fc_16(REGISTER_SELECT_OUTPUT, 2, data);
// if (rc == -1)
// {
// while(retries < 5)
// {
// rq_com_listen_stream();
// if(rq_com_stream_detected)
// {
// return 0;
// }
// Sleep(50);
// retries = retries + 1;
// }
// return -1;
// }
// return 0;
// }
// /**
// * \brief Sends a read request
// * \param base Address of the first register to read
// * \param n Number of bytes to read
// * \param data table into which data will be written
// */
// static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data)
// {
// UINT_8 bytes_read = 0;
// INT_32 i = 0;
// INT_32 cpt = 0;
// UINT_8 data_request[n];
// UINT_16 retries = 0;
// //precondition, null pointer
// if (data == NULL)
// {
// return -1;
// }
// //Send the read request
// rq_com_send_fc_03_request(base,n);
// //Read registers
// while (retries < 100 && bytes_read == 0)
// {
// Sleep(4);
// bytes_read = rq_com_wait_for_fc_03_echo(data_request);
// retries++;
// }
// if (bytes_read <= 0)
// {
// return -1;
// }
// if ((n%2) == 0){
// for (i = 0; i < n/2; i++ )
// {
// data[i] = (data_request[cpt++] * 256);
// data[i] = data[i] + data_request[cpt++];
// }
// }
// else{
// for (i = 0; i <= n/2; i++ )
// {
// data[i] = (data_request[cpt++] * 256);
// data[i] = data[i] + data_request[cpt++];
// }
// }
// return 0;
// }
// /**
// * \fn static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 * data)
// * \brief Sends a write request to a number of registers
// * \param base Address of the first register to write to
// * \param n Number of registers to write
// * \param data buffer that contains data to write
// */
// static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data)
// {
// INT_8 valid_answer = 0;
// UINT_8 data_request[n];
// UINT_16 retries = 0;
// UINT_32 i;
// //precondition, null pointer
// if (data == NULL)
// {
// return -1;
// }
// for (i = 0; i < n; i++ )
// {
// if(i % 2 == 0)
// {
// data_request[i] = (data[(i/2)] >> 8);
// }
// else
// {
// data_request[i] = (data[(i/2)] & 0xFF);
// }
// }
// rq_com_send_fc_16_request(base, n, data_request);
// while (retries < 100 && valid_answer == 0)
// {
// Sleep(10);
// valid_answer = rq_com_wait_for_fc_16_echo();
// retries++;
// }
// if(valid_answer == 1)
// {
// return 0;
// }
// return -1;
// }
// //
// //Public functions
// /**
// * \fn void rq_sensor_com_read_info_high_lvl(void)
// * \brief Reads and stores high level information from
// * the sensor. These include the firmware version,
// * the serial number and the production year.
// */
// void rq_sensor_com_read_info_high_lvl(void)
// {
// UINT_16 registers[4] = {0};//table de registre
// UINT_64 serial_number = 0;
// INT_32 result = 0;
// //Firmware Version
// result = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 6, registers);
// if (result != -1)
// {
// sprintf(rq_com_str_sensor_firmware_version, "%c%c%c-%hhu.%hhu.%hhu",
// registers[0] >> 8, registers[0] & 0xFF, registers[1] >> 8,
// registers[1] & 0xFF, registers[2] >> 8, registers[2] & 0xFF);
// }
// //Production Year
// result = rq_com_send_fc_03(REGISTER_PRODUCTION_YEAR, 2, registers);
// if (result != -1)
// {
// sprintf(rq_com_str_sensor_production_year, "%u", registers[0]);
// }
// //Serial Number
// result = rq_com_send_fc_03(REGISTER_SERIAL_NUMBER, 8, registers);
// if (result != -1)
// {
// serial_number = (UINT_64) ((registers[3] >> 8) * 256 * 256 * 256
// + (registers[3] & 0xFF) * 256 * 256 + (registers[2] >> 8) * 256
// + (registers[2] & 0xFF));
// if (serial_number == 0)
// {
// sprintf(rq_com_str_sensor_serial_number, "UNKNOWN");
// }
// else
// {
// sprintf(rq_com_str_sensor_serial_number, "%c%c%c-%.4lu", registers[0] >> 8,
// registers[0] & 0xFF, registers[1] >> 8, serial_number);
// }
// }
// }
// /**
// * \fn static INT_32 rq_com_read_port(UINT_8 * buf, UINT_32 buf_len)
// * \brief Reads incomming data on the com port
// * \param buf, contains the incomming data
// * \param buf_len maximum number of data to read
// * \return The number of character read
// */
// static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len)
// {
// #ifdef __unix__ //For Unix
// return read(fd_connexion, buf, buf_len);
// #elif defined(_WIN32)||defined(WIN32) //For Windows
// DWORD myBytesRead = 0;
// ReadFile(hSerial,buf,buf_len,&myBytesRead,NULL);
// return myBytesRead;
// #endif
// }
// /**
// * \fn static INT_32 rq_com_write_port(UINT_8 * buf, UINT_32 buf_len)
// * \brief Writes on the com port
// * \param buf data to write
// * \param buf_len numer of bytes to write
// * \return The number of characters written or -1 in case of an error
// */
// static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len)
// {
// //precondition
// if (buf == NULL)
// {
// return -1;
// }
// #ifdef __unix__ //For Unix
// return write(fd_connexion, buf, buf_len);
// #elif defined(_WIN32)||defined(WIN32) //For Windows
// DWORD n_bytes = 0;
// return (WriteFile(hSerial, buf, buf_len, &n_bytes, NULL)) ? n_bytes: -1;
// #endif
// }
// /**
// * \fn static UINT_16 rq_com_compute_crc(UINT_8 * adr, INT_32 length )
// * \param adr, Address of the first byte
// * \param length Length of the buffer on which the crc is computed
// * \return Value of the crc
// */
// static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length )
// {
// UINT_16 CRC_calc = 0xFFFF;
// INT_32 j=0;
// INT_32 k=0;
// //precondition, null pointer
// if (adr == NULL)
// {
// return 0;
// }
// //Tant qu'il reste des bytes dans le message
// while (j < length)
// {
// //Si c'est le premier byte
// if (j==0)
// {
// CRC_calc ^= *adr & 0xFF;
// }
// //Sinon on utilisera un XOR sur le word
// else
// {
// CRC_calc ^= *adr;
// }
// k=0;
// //Tant que le byte n'est pas complété
// while (k < 8)
// {
// //Si le dernier bit est un 1
// if (CRC_calc & 0x0001)
// {
// CRC_calc = (CRC_calc >> 1)^ 0xA001; //Shift de 1 bit vers la droite et XOR avec le facteur polynomial
// }
// else
// {
// CRC_calc >>= 1; //Shift de 1 bit vers la droite
// }
// k++;
// }
// //Incrémente l'adresse et le compteur d'adresse
// adr++;
// j++;
// }
// return CRC_calc;
// }
// /**
// * \fn static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n)
// * \brief Compute and send the fc03 request on the com port
// * \param base Address of the first register to read
// * \param n Number of bytes to read
// */
// static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n)
// {
// static UINT_8 buf[MP_BUFF_SIZE];
// INT_32 length = 0;
// UINT_8 reg[2];
// UINT_8 words[2];
// UINT_16 CRC;
// //Si le nombre de registre est impair
// if(n % 2 != 0)
// {
// n += 1;
// }
// //Scinder en LSB et MSB
// reg[0] = (UINT_8)(base >> 8); //MSB to the left
// reg[1] = (UINT_8)(base & 0x00FF); //LSB to the right
// words[0] = (UINT_8)((n/2) >> 8); //MSB to the left
// words[1] = (UINT_8)((n/2) & 0x00FF); //LSB to the right
// //Build the request
// buf[length++] = 9; //slave address
// buf[length++] = 3;
// buf[length++] = reg[0];
// buf[length++] = reg[1];
// buf[length++] = words[0];
// buf[length++] = words[1];
// //CRC computation
// CRC = rq_com_compute_crc(buf, length);
// //Append the crc
// buf[length++] = (UINT_8)(CRC & 0x00FF);
// buf[length++] = (UINT_8)(CRC >> 8);
// //Send the request
// rq_com_write_port(buf, length);
// }
// /**
// * \fn static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 data[])
// * \brief Reads the reply to a fc03 request
// * \param base Address of the buffer that will store the reply
// * \return The number of character read
// */
// static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data)
// {
// static UINT_8 buf[MP_BUFF_SIZE];
// static INT_32 length = 0;
// static INT_32 old_length = 0;
// static INT_32 counter_no_new_data = 0;
// UINT_8 n = 0;
// UINT_16 CRC = 0;
// INT_32 j = 0;
// INT_32 ret = rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
// if(ret != -1)
// {
// length = length + ret;
// }
// //If there is no new data, the buffer is cleared
// if(length == old_length)
// {
// if(counter_no_new_data < 5)
// {
// counter_no_new_data++;
// }
// else
// {
// length = 0;
// }
// }
// else
// {
// counter_no_new_data = 0;
// }
// old_length = length;
// if(length > 0)
// {
// //If there is not enough data, return
// if(length <= 5)
// {
// return 0;
// }
// else
// {
// if(buf[1] == 3) //3 indicates the response to a fc03 query
// {
// n = buf[2];
// if(length < 5 + n)
// {
// return 0;
// }
// }
// else //unknown fc code
// {
// length = 0;
// return 0;
// }
// }
// CRC = rq_com_compute_crc(buf, length - 2);
// //Verifies the crc and the slave ID
// if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
// {
// //On clear le buffer
// buf[0] = 0;
// length = 0;
// return 0;
// }
// else
// {
// n = buf[2];
// //Writes the bytes to the return buffer
// for(j = 0; j < n; j++)
// {
// data[j] = buf[j + 3];
// }
// //Clears the buffer
// buf[0] = 0;
// length = 0;
// return n;
// }
// }
// return 0;
// }
// /**
// * \fn void modbus_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 * data)
// * \brief Sends a fc16 write request
// * \param base Address of the first register to write to
// * \param n Number of bytes to write
// */
// static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data)
// {
// static UINT_8 buf[MP_BUFF_SIZE];
// INT_32 length = 0;
// //Byte of the query
// UINT_8 reg[2];
// UINT_8 words[2];
// UINT_16 CRC;
// INT_32 n2 = 0;
// INT_32 i = 0;
// if (data == NULL)
// {
// return;
// }
// //Manage if the number of bytes to write is odd or even
// if(n %2 != 0)
// {
// n2 = n+1;
// }
// else
// {
// n2 = n;
// }
// //Split the address and the number of bytes between MSB ans LSB
// reg[0] = (UINT_8)(base >> 8); //MSB to the left
// reg[1] = (UINT_8)(base & 0x00FF); //LSB to the right
// words[0] = (UINT_8)((n2/2) >> 8); //MSB to the left
// words[1] = (UINT_8)((n2/2) & 0x00FF); //LSB to the right
// //Build the query
// buf[length++] = 9; //slave address
// buf[length++] = 16;
// buf[length++] = reg[0];
// buf[length++] = reg[1];
// buf[length++] = words[0];
// buf[length++] = words[1];
// buf[length++] = n2;
// //Copy data to the send buffer
// for(i = 0; i < n; i++)
// {
// buf[length++] = data[i];
// }
// if(n != n2)
// {
// buf[length++] = 0;
// }
// CRC = rq_com_compute_crc(buf, length);
// //Append the crc to the query
// buf[length++] = (UINT_8)(CRC & 0x00FF);
// buf[length++] = (UINT_8)(CRC >> 8);
// //Send the query
// rq_com_write_port(buf, length);
// }
// /**
// * \fn static INT_32 rq_com_wait_for_fc_16_echo(void)
// * \brief Reads the response to a fc16 write query
// * \return 0 for an invalid response, 1 otherwise
// */
// static INT_32 rq_com_wait_for_fc_16_echo(void)
// {
// static UINT_8 buf[MP_BUFF_SIZE];
// static INT_32 length = 0;
// static INT_32 old_length = 0;
// static INT_32 counter_no_new_data = 0;
// UINT_16 CRC = 0;
// length = length + rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
// //Clear the buffer if no new data
// if(length == old_length)
// {
// if(counter_no_new_data < 5)
// {
// counter_no_new_data++;
// }
// else
// {
// length = 0;
// }
// }
// else
// {
// counter_no_new_data = 0;
// }
// old_length = length;
// if(length > 0)
// {
// //If not enough data, return
// if(length < 8)
// {
// return 0;
// }
// else
// {
// //if it's a reply to a fc16 query then proceed
// if(buf[1] == 16)
// {
// length = 8;
// CRC = rq_com_compute_crc(buf, length - 2);
// //Check the crc an the slave ID
// if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
// {
// //Clear the buffer
// length = 0;
// return 0;
// }
// else
// {
// //Clear the buffer
// length = 0;
// return 1;
// }
// }
// else //Clear the buffer
// {
// length = 0;
// return 0;
// }
// }
// }
// return 0;
// }
// /**
// * \brief Retrieves the sensor serial number
// * \param serial_number address of the return buffer
// */
// void rq_com_get_str_serial_number(INT_8 * serial_number)
// {
// strcpy(serial_number, rq_com_str_sensor_serial_number);
// }
// /**
// * \brief Retrieves the sensor firmware version
// * \param firmware_version Address of the return buffer
// */
// void rq_com_get_str_firmware_version(INT_8 * firmware_version)
// {
// strcpy(firmware_version, rq_com_str_sensor_firmware_version);
// }
// /**
// * \brief Retrieves the sensor firmware version
// * \param production_year Address of the return buffer
// */
// void rq_com_get_str_production_year(INT_8 * production_year)
// {
// strcpy(production_year,rq_com_str_sensor_production_year);
// }
// /**
// * \brief retrieves the sensor firmware version
// * \param production_year Address of the return buffer
// */
// bool rq_com_get_stream_detected()
// {
// return rq_com_stream_detected;
// }
// /**
// * \brief returns if the stream message is valid
// */
// bool rq_com_get_valid_stream()
// {
// return rq_com_valid_stream;
// }
// /**
// * \brief Return an effort component
// * \param i Index of the component. 0 to 2 for Fx, Fy and Fz.
// * 3 to 5 for Mx, My and Mz.
// */
// float rq_com_get_received_data(UINT_8 i)
// {
// if(i >= 0 && i <= 5)
// {
// return rq_com_received_data[i] - rq_com_received_data_offset[i];
// }
// return 0.0;
// }
// /**
// * \brief Returns true if a new valid stream message has been decoded and
// * is available.
// * \details When this function is called, the variable that indicates if a
// new message is available is set to false even if the message
// hasn't beed read.
// */
// bool rq_com_got_new_message()
// {
// bool tmp = rq_com_new_message;
// rq_com_new_message = false;
// return tmp;
// }
// /**
// * \brief Set the "zero sensor" flag to 1. When the next stream message will
// be decoded, the effort values will be stored as offsets a
// substracted from the next values
// */
// void rq_com_do_zero_force_flag()
// {
// rq_com_zero_force_flag = 1;
// }
// /**
// * \brief close the serial port.
// * \warning Only valid under Windows.
// */
// void stop_connection()
// {
// #if defined(_WIN32)||defined(WIN32) //For Windows
// CloseHandle(hSerial);
// hSerial = INVALID_HANDLE_VALUE;
// #endif
// }
// /**
// * \author Pierre-Olivier Proulx
// * \brief try to discover a com port by polling each serial port
// * \return 1 if a device is found, 0 otherwise
// * \details For linux, we user the command fuser to determine if the
// * tty is used by another process. If so, we do not scan it.
// */
// #ifdef __unix__ //For Unix
// static UINT_8 rq_com_identify_device(INT_8 const * const d_name)
// {
// FILE * fuser_output;
// INT_8 fuser_cmd[40] = {0};
// INT_8 fuser_ret[30] = {0};
// INT_8 dirParent[20] = {0};
// INT_8 port_com[15] = {0};
// strcpy(dirParent, "/dev/");
// strcat(dirParent, d_name);
// strcpy(port_com, dirParent);
// /* si un process utilise le tty, on le le scan pas. */
// snprintf (fuser_cmd, 40, "fuser %s 2> /dev/null", port_com);
// fuser_output = popen (fuser_cmd, "r");
// if (fuser_output != NULL)
// {
// fgets (fuser_ret, 30, fuser_output);
// fclose(fuser_output);
// }
// if (strlen (fuser_ret) != 0)
// {
// return 0;
// }
// fprintf(stderr, "fuser ok\n");
// fprintf(stderr, "attempting to open %s\n", port_com);
// fd_connexion = open(port_com, O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL);
// //The serial port is open
// if(fd_connexion != -1)
// {
// fprintf(stderr, "fd_connexion != 1\n");
// if(set_com_attribs(fd_connexion,B19200) != -1)
// {
// //Try connecting to the sensor
// if (rq_com_tentative_connexion() == 1)
// {
// return 1;
// }
// }
// //The device is identified, close the connection
// close(fd_connexion);
// fprintf(stderr, "close connection\n");
// } else {
// perror("uhhhhhh");
// fprintf(stderr, "fd_connexion == 1\n");
// }
// return 0;
// }
// #endif
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_sensor_com.c
* \date June 18, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
//
//Includes
//Platform specific
#ifdef __unix__ /*For Unix*/
#define _BSD_SOURCE
#include <termios.h>
#include <unistd.h>
#include <dirent.h>
#include <fcntl.h>
#include <unistd.h>
#include <time.h>
#define Sleep(x) (usleep (x*1000))
#elif defined(_WIN32)||defined(WIN32) /*For Windows*/
#include <windows.h>
#endif
#include <string.h>
#include <errno.h>
//application specific
#include "robotiq_ft/rq_sensor_com.h"
// Definitions
#define REGISTER_SELECT_OUTPUT 410
#define REGISTER_SERIAL_NUMBER 510
#define REGISTER_PRODUCTION_YEAR 514
#define REGISTER_FIRMWARE_VERSION 500
#define RQ_COM_MAX_STR_LENGTH 20
#define RQ_COM_JAM_SIGNAL_CHAR 0xff
#define RQ_COM_JAM_SIGNAL_LENGTH 50
#define RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE 20 //20 * 4ms = 80ms sans bytes avant de dire qu'on n'a pas le stream
#define RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE 40 //40 * 4ms = 160ms sans messages valide afin de dire que la communication fonctionne mal (16 mauvais messages)
// Private variables
static float rq_com_received_data[6] = {0.0};
static float rq_com_received_data_offset[6] = {0.0};
static UINT_16 rq_com_computed_crc = 0;
static UINT_16 rq_com_crc = 0;
static INT_8 rq_com_str_sensor_serial_number[RQ_COM_MAX_STR_LENGTH];
static INT_8 rq_com_str_sensor_production_year[RQ_COM_MAX_STR_LENGTH];
static INT_8 rq_com_str_sensor_firmware_version[RQ_COM_MAX_STR_LENGTH];
static UINT_32 rq_com_msg_received = 0;
static UINT_8 rq_com_rcv_buff[MP_BUFF_SIZE];
static UINT_8 rq_com_snd_buff[MP_BUFF_SIZE];
static UINT_8 rq_com_rcv_buff2[MP_BUFF_SIZE];
static INT_32 rq_com_rcv_len;
static INT_32 rq_com_rcv_len2 = 0;
static INT_32 rq_com_zero_force_flag = 0;
static INT_32 rq_state_sensor_watchdog = 0;
//variables related to the communication status
static bool rq_com_stream_detected = false;
static bool rq_com_valid_stream = false;
static bool rq_com_new_message = false;
static INT_32 rq_com_timer_for_stream_detection = 0;
static INT_32 rq_com_timer_for_valid_stream = 0;
///
//Private functions declaration
static INT_8 rq_com_tentative_connexion(void);
static void rq_com_send_jam_signal(void);
static void rq_com_stop_stream_after_boot(void);
static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len);
static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len);
//Modbus functions
static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length );
static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n);
static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data);
static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data);
static INT_32 rq_com_wait_for_fc_16_echo(void);
static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data);
static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data);
#ifdef __unix__ //For Unix
static UINT_8 rq_com_identify_device(INT_8 const * const d_name);
#endif //For Unix
#ifdef __unix__ //For Unix
static INT_32 fd_connexion = -1;
static INT_8 set_com_attribs (INT_32 fd, speed_t speed);
#elif defined(_WIN32)||defined(WIN32) //For Windows
HANDLE hSerial;
#endif
//
//Function definitions
/**
* \fn void rq_sensor_com(void)
* \brief Discovers and initialize the communication with the sensor
* \details The functions loops through all the serial com ports
* and polls them to discover the sensor
*/
INT_8 rq_sensor_com()
{
#ifdef __unix__ //For Unix
UINT_8 device_found = 0;
DIR *dir = NULL;
struct dirent *entrydirectory = NULL;
//Close a previously opened connection to a device
close(fd_connexion);
if ((dir = opendir("/sys/class/tty/")) == NULL)
{
return -1;
}
//Loops through the files in the /sys/class/tty/ directory
while ((entrydirectory = readdir(dir)) != NULL && device_found == 0)
{
//Look for a serial device
if (strstr(entrydirectory->d_name, "ttyS") ||
strstr(entrydirectory->d_name, "ttyUSB"))
{
device_found = rq_com_identify_device(entrydirectory->d_name);
}
}
closedir(dir);
if (device_found == 0)
{
return -1;
}
#elif defined(_WIN32)||defined(WIN32) //For Windows
DCB dcb;
INT_32 i;
INT_8 port[13];
for(i = 0;i < 256; i++){
sprintf(port,"\\\\.\\COM%d",i);
hSerial = CreateFileA(port, GENERIC_READ | GENERIC_WRITE,
0,NULL,OPEN_EXISTING,0,NULL);
if(hSerial != INVALID_HANDLE_VALUE){
dcb.DCBlength = sizeof(DCB);
if (!GetCommState(hSerial, &dcb)){
CloseHandle(hSerial);
hSerial = INVALID_HANDLE_VALUE;
continue;//Permet de recommencer la boucle
}
dcb.BaudRate = CBR_19200;
dcb.ByteSize = 8;
dcb.StopBits = ONESTOPBIT;
dcb.Parity = NOPARITY;
dcb.fParity = FALSE;
/* No software handshaking */
dcb.fTXContinueOnXoff = TRUE;
dcb.fOutX = FALSE;
dcb.fInX = FALSE;
//dcb.fNull = FALSE;
/* Binary mode (it's the only supported on Windows anyway) */
dcb.fBinary = TRUE;
/* Don't want errors to be blocking */
dcb.fAbortOnError = FALSE;
/* Setup port */
if(!SetCommState(hSerial, &dcb)){
CloseHandle(hSerial);
continue;//Permet de recommencer la boucle
}
COMMTIMEOUTS timeouts={0};
timeouts.ReadIntervalTimeout=0;
timeouts.ReadTotalTimeoutConstant=1;
timeouts.ReadTotalTimeoutMultiplier=0;
timeouts.WriteTotalTimeoutConstant=1;
timeouts.WriteTotalTimeoutMultiplier=0;
if(!SetCommTimeouts(hSerial, &timeouts)){
CloseHandle(hSerial);
hSerial = INVALID_HANDLE_VALUE;
continue;//Permet de recommencer la boucle
}
if (rq_com_tentative_connexion() == 1){
return 0;
}
CloseHandle(hSerial);
}
}
return -1;
#else
#endif
return 0;
}
/**
* \fn static INT_8 rq_com_tentative_connexion()
* \brief Tries connecting to the sensor
* \returns 1 if the connection attempt succeeds, -1 otherwise
*/
static INT_8 rq_com_tentative_connexion()
{
INT_32 rc = 0;
UINT_16 firmware_version [1];
UINT_8 retries = 0;
while(retries < 5 && rq_com_stream_detected == false)
{
rq_com_listen_stream();
retries++;
}
rq_com_listen_stream();
if(rq_com_stream_detected)
{
rq_com_stop_stream_after_boot();
}
//Give some time to the sensor to switch to modbus
Sleep(100);
//If the device returns an F as the first character of the fw version,
//we consider its a sensor
rc = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 2, firmware_version);
if (rc != -1)
{
if (firmware_version[0] >> 8 == 'F')
{
return 1;
}
}
return -1;
}
/**
* \fn static INT_8 set_com_attribs()
* \brief Sets the com port parameters to match those of the sensor
* \param fd, file descriptor that points to the serial port
* \param speed, baudrate
* \return 0 in case of a success, -1 otherwise
*/
#ifdef __unix__ //For Unix
static INT_8 set_com_attribs (INT_32 fd, speed_t speed)
{
struct termios tty;
memset (&tty, 0, sizeof (struct termios));
if (tcgetattr (fd, &tty) != 0)
{
return -1;
}
cfsetospeed (&tty, speed);
cfsetispeed (&tty, speed);
tty.c_cflag |= (CLOCAL | CREAD);
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~PARENB;
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
tty.c_iflag &= ~INPCK;
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 1;
if (tcsetattr (fd, TCSANOW, &tty) != 0)
{
printf("error %d from tcsetattr", errno);
return -1;
}
return 0;
}
#endif
/**
* \fn void rq_com_listen_stream()
* \brief Listens and decode a valid stream input
*/
void rq_com_listen_stream(void)
{
static INT_32 last_byte = 0;
static INT_32 new_message = 0;
INT_32 i = 0;
INT_32 j = 0;
//Capture and store the current value of the sensor and use it to
//zero subsequent sensor values
if(rq_com_zero_force_flag == 1)
{
rq_com_zero_force_flag = 0;
for(i = 0; i < 6; i++)
{
rq_com_received_data_offset[i] = rq_com_received_data[i];
}
}
Sleep(10);
//Increment communication state counters
if(rq_com_timer_for_stream_detection++ > RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE)
{
rq_com_timer_for_stream_detection = RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE;
rq_com_stream_detected = false;
}
if(rq_com_timer_for_valid_stream++ > RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE)
{
rq_com_timer_for_valid_stream = RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE;
rq_com_valid_stream = false;
}
last_byte = 0;
new_message = 0;
//Data reception
rq_com_rcv_len = rq_com_read_port(rq_com_rcv_buff, MP_BUFF_SIZE);
//If at least a byte is received, we consider the sensor to stream
if(rq_com_rcv_len > 0)
{
rq_com_stream_detected = true;
rq_com_timer_for_stream_detection = 0;
}
//Copie les données au bout du buffer 2
for(i = 0; i < rq_com_rcv_len; i++)
{
//If the buffer overflows, set the index to the beginning
if(rq_com_rcv_len2 == MP_BUFF_SIZE)
{
//Next bytes will overwrite the begining of the buffer
rq_com_rcv_len2 = 0;
break;
}
rq_com_rcv_buff2[rq_com_rcv_len2++] = rq_com_rcv_buff[i];
}
//empty the buffers
memset( rq_com_rcv_buff, 0, sizeof(rq_com_rcv_buff));
memset( rq_com_snd_buff, 0, sizeof(rq_com_snd_buff));
//If there is enough characters,...
if(rq_com_rcv_len2 >= 16 && rq_com_rcv_len >= 1)
{
//Search for a valid stream message in the buffer
for(i = rq_com_rcv_len2 - 16; i >= 0; i--)
{
//Header
if(rq_com_rcv_buff2[i] == 0x20 && rq_com_rcv_buff2[i+1] == 0x4E && new_message == 0)
{
last_byte = i - 1;
rq_com_computed_crc = rq_com_compute_crc(rq_com_rcv_buff2 + i * sizeof(*rq_com_rcv_buff2), 14);
rq_com_crc = (rq_com_rcv_buff2[i+14] + rq_com_rcv_buff2[i+15] * 256);
rq_com_msg_received++;
//The crc is valid.. the message can be used
if(rq_com_computed_crc == rq_com_crc)
{
last_byte = i + 15; //will erase the message
//convert the efforts to floating point numbers
for(j = 0; j < 3; j++)
{
rq_com_received_data[j] = ((float)((INT_16)((UINT_8)rq_com_rcv_buff2[i+2+2*j] + ((INT_16)(UINT_8)rq_com_rcv_buff2[i+3+2*j]) * 256))) / 100 ;
}
for(j = 3; j < 6; j++)
{
rq_com_received_data[j] = ((float)((INT_16)((UINT_8)rq_com_rcv_buff2[i+2+2*j] + ((INT_16)(UINT_8)rq_com_rcv_buff2[i+3+2*j]) * 256))) / 1000;
}
//Signal the stream as valid
rq_com_valid_stream = true;
rq_com_new_message = true;
rq_com_timer_for_valid_stream = 0;
new_message = 1;
rq_state_sensor_watchdog = 1;
}
else
{
last_byte = i + 15;
new_message = 1;
}
}
}
if(last_byte > 0)
{
//On shift le buffer 2 afin de ne garder que ce qui dépasse le dernier caractère du dernier message complet
for(i = 0; i < (rq_com_rcv_len2 - last_byte - 1); i++)
{
rq_com_rcv_buff2[i] = rq_com_rcv_buff2[last_byte + 1 + i];
}
rq_com_rcv_len2 = rq_com_rcv_len2 - last_byte - 1;
}
}
}
/**
* \fn static void rq_com_send_jam_signal(void)
* \brief Send a signal that interrupts the streaming
*/
static void rq_com_send_jam_signal(void)
{
//Build the jam signal
memset(rq_com_snd_buff, RQ_COM_JAM_SIGNAL_CHAR, RQ_COM_JAM_SIGNAL_LENGTH);
//Send the jam signal
rq_com_write_port(rq_com_snd_buff,RQ_COM_JAM_SIGNAL_LENGTH);
}
/**
* \fn static void rq_com_stop_stream_after_boot(void)
* \brief Send a jam signal to stop the sensor stream
* and retry until the stream stops
*/
static void rq_com_stop_stream_after_boot(void)
{
static INT_32 counter = 0;
while(rq_com_stream_detected)
{
counter++;
if(counter == 1)
{
rq_com_send_jam_signal();
}
else
{
rq_com_listen_stream();
if(rq_com_stream_detected)
{
counter = 0;
}
else
{
counter = 0;
}
}
}
}
/**
* \fn INT_8 rq_com_start_stream(void)
* \brief Starts the sensor streaming mode
* \return 0 if the stream started, -1 otherwise
*/
INT_8 rq_com_start_stream(void)
{
UINT_16 data[1] = {0x0200}; //0x0200 selects the stream output
UINT_8 retries = 0;
INT_8 rc = rq_com_send_fc_16(REGISTER_SELECT_OUTPUT, 2, data);
if (rc == -1)
{
while(retries < 5)
{
rq_com_listen_stream();
if(rq_com_stream_detected)
{
return 0;
}
Sleep(50);
retries = retries + 1;
}
return -1;
}
return 0;
}
/**
* \brief Sends a read request
* \param base Address of the first register to read
* \param n Number of bytes to read
* \param data table into which data will be written
*/
static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data)
{
UINT_8 bytes_read = 0;
INT_32 i = 0;
INT_32 cpt = 0;
UINT_8 data_request[n];
UINT_16 retries = 0;
//precondition, null pointer
if (data == NULL)
{
return -1;
}
//Send the read request
rq_com_send_fc_03_request(base,n);
//Read registers
while (retries < 100 && bytes_read == 0)
{
Sleep(4);
bytes_read = rq_com_wait_for_fc_03_echo(data_request);
retries++;
}
if (bytes_read <= 0)
{
return -1;
}
if ((n%2) == 0){
for (i = 0; i < n/2; i++ )
{
data[i] = (data_request[cpt++] * 256);
data[i] = data[i] + data_request[cpt++];
}
}
else{
for (i = 0; i <= n/2; i++ )
{
data[i] = (data_request[cpt++] * 256);
data[i] = data[i] + data_request[cpt++];
}
}
return 0;
}
/**
* \fn static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 * data)
* \brief Sends a write request to a number of registers
* \param base Address of the first register to write to
* \param n Number of registers to write
* \param data buffer that contains data to write
*/
static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data)
{
INT_8 valid_answer = 0;
UINT_8 data_request[n];
UINT_16 retries = 0;
UINT_32 i;
//precondition, null pointer
if (data == NULL)
{
return -1;
}
for (i = 0; i < n; i++ )
{
if(i % 2 == 0)
{
data_request[i] = (data[(i/2)] >> 8);
}
else
{
data_request[i] = (data[(i/2)] & 0xFF);
}
}
rq_com_send_fc_16_request(base, n, data_request);
while (retries < 100 && valid_answer == 0)
{
Sleep(10);
valid_answer = rq_com_wait_for_fc_16_echo();
retries++;
}
if(valid_answer == 1)
{
return 0;
}
return -1;
}
//
//Public functions
/**
* \fn void rq_sensor_com_read_info_high_lvl(void)
* \brief Reads and stores high level information from
* the sensor. These include the firmware version,
* the serial number and the production year.
*/
void rq_sensor_com_read_info_high_lvl(void)
{
UINT_16 registers[4] = {0};//table de registre
UINT_64 serial_number = 0;
INT_32 result = 0;
//Firmware Version
result = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 6, registers);
if (result != -1)
{
sprintf(rq_com_str_sensor_firmware_version, "%c%c%c-%hhu.%hhu.%hhu",
registers[0] >> 8, registers[0] & 0xFF, registers[1] >> 8,
registers[1] & 0xFF, registers[2] >> 8, registers[2] & 0xFF);
}
//Production Year
result = rq_com_send_fc_03(REGISTER_PRODUCTION_YEAR, 2, registers);
if (result != -1)
{
sprintf(rq_com_str_sensor_production_year, "%u", registers[0]);
}
//Serial Number
result = rq_com_send_fc_03(REGISTER_SERIAL_NUMBER, 8, registers);
if (result != -1)
{
serial_number = (UINT_64) ((registers[3] >> 8) * 256 * 256 * 256
+ (registers[3] & 0xFF) * 256 * 256 + (registers[2] >> 8) * 256
+ (registers[2] & 0xFF));
if (serial_number == 0)
{
sprintf(rq_com_str_sensor_serial_number, "UNKNOWN");
}
else
{
sprintf(rq_com_str_sensor_serial_number, "%c%c%c-%.4lu", registers[0] >> 8,
registers[0] & 0xFF, registers[1] >> 8, serial_number);
}
}
}
/**
* \fn static INT_32 rq_com_read_port(UINT_8 * buf, UINT_32 buf_len)
* \brief Reads incomming data on the com port
* \param buf, contains the incomming data
* \param buf_len maximum number of data to read
* \return The number of character read
*/
static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len)
{
#ifdef __unix__ //For Unix
return read(fd_connexion, buf, buf_len);
#elif defined(_WIN32)||defined(WIN32) //For Windows
DWORD myBytesRead = 0;
ReadFile(hSerial,buf,buf_len,&myBytesRead,NULL);
return myBytesRead;
#endif
}
/**
* \fn static INT_32 rq_com_write_port(UINT_8 * buf, UINT_32 buf_len)
* \brief Writes on the com port
* \param buf data to write
* \param buf_len numer of bytes to write
* \return The number of characters written or -1 in case of an error
*/
static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len)
{
//precondition
if (buf == NULL)
{
return -1;
}
#ifdef __unix__ //For Unix
return write(fd_connexion, buf, buf_len);
#elif defined(_WIN32)||defined(WIN32) //For Windows
DWORD n_bytes = 0;
return (WriteFile(hSerial, buf, buf_len, &n_bytes, NULL)) ? n_bytes: -1;
#endif
}
/**
* \fn static UINT_16 rq_com_compute_crc(UINT_8 * adr, INT_32 length )
* \param adr, Address of the first byte
* \param length Length of the buffer on which the crc is computed
* \return Value of the crc
*/
static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length )
{
UINT_16 CRC_calc = 0xFFFF;
INT_32 j=0;
INT_32 k=0;
//precondition, null pointer
if (adr == NULL)
{
return 0;
}
//Tant qu'il reste des bytes dans le message
while (j < length)
{
//Si c'est le premier byte
if (j==0)
{
CRC_calc ^= *adr & 0xFF;
}
//Sinon on utilisera un XOR sur le word
else
{
CRC_calc ^= *adr;
}
k=0;
//Tant que le byte n'est pas complété
while (k < 8)
{
//Si le dernier bit est un 1
if (CRC_calc & 0x0001)
{
CRC_calc = (CRC_calc >> 1)^ 0xA001; //Shift de 1 bit vers la droite et XOR avec le facteur polynomial
}
else
{
CRC_calc >>= 1; //Shift de 1 bit vers la droite
}
k++;
}
//Incrémente l'adresse et le compteur d'adresse
adr++;
j++;
}
return CRC_calc;
}
/**
* \fn static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n)
* \brief Compute and send the fc03 request on the com port
* \param base Address of the first register to read
* \param n Number of bytes to read
*/
static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n)
{
static UINT_8 buf[MP_BUFF_SIZE];
INT_32 length = 0;
UINT_8 reg[2];
UINT_8 words[2];
UINT_16 CRC;
//Si le nombre de registre est impair
if(n % 2 != 0)
{
n += 1;
}
//Scinder en LSB et MSB
reg[0] = (UINT_8)(base >> 8); //MSB to the left
reg[1] = (UINT_8)(base & 0x00FF); //LSB to the right
words[0] = (UINT_8)((n/2) >> 8); //MSB to the left
words[1] = (UINT_8)((n/2) & 0x00FF); //LSB to the right
//Build the request
buf[length++] = 9; //slave address
buf[length++] = 3;
buf[length++] = reg[0];
buf[length++] = reg[1];
buf[length++] = words[0];
buf[length++] = words[1];
//CRC computation
CRC = rq_com_compute_crc(buf, length);
//Append the crc
buf[length++] = (UINT_8)(CRC & 0x00FF);
buf[length++] = (UINT_8)(CRC >> 8);
//Send the request
rq_com_write_port(buf, length);
}
/**
* \fn static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 data[])
* \brief Reads the reply to a fc03 request
* \param base Address of the buffer that will store the reply
* \return The number of character read
*/
static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data)
{
static UINT_8 buf[MP_BUFF_SIZE];
static INT_32 length = 0;
static INT_32 old_length = 0;
static INT_32 counter_no_new_data = 0;
UINT_8 n = 0;
UINT_16 CRC = 0;
INT_32 j = 0;
INT_32 ret = rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
if(ret != -1)
{
length = length + ret;
}
//If there is no new data, the buffer is cleared
if(length == old_length)
{
if(counter_no_new_data < 5)
{
counter_no_new_data++;
}
else
{
length = 0;
}
}
else
{
counter_no_new_data = 0;
}
old_length = length;
if(length > 0)
{
//If there is not enough data, return
if(length <= 5)
{
return 0;
}
else
{
if(buf[1] == 3) //3 indicates the response to a fc03 query
{
n = buf[2];
if(length < 5 + n)
{
return 0;
}
}
else //unknown fc code
{
length = 0;
return 0;
}
}
CRC = rq_com_compute_crc(buf, length - 2);
//Verifies the crc and the slave ID
if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
{
//On clear le buffer
buf[0] = 0;
length = 0;
return 0;
}
else
{
n = buf[2];
//Writes the bytes to the return buffer
for(j = 0; j < n; j++)
{
data[j] = buf[j + 3];
}
//Clears the buffer
buf[0] = 0;
length = 0;
return n;
}
}
return 0;
}
/**
* \fn void modbus_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 * data)
* \brief Sends a fc16 write request
* \param base Address of the first register to write to
* \param n Number of bytes to write
*/
static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data)
{
static UINT_8 buf[MP_BUFF_SIZE];
INT_32 length = 0;
//Byte of the query
UINT_8 reg[2];
UINT_8 words[2];
UINT_16 CRC;
INT_32 n2 = 0;
INT_32 i = 0;
if (data == NULL)
{
return;
}
//Manage if the number of bytes to write is odd or even
if(n %2 != 0)
{
n2 = n+1;
}
else
{
n2 = n;
}
//Split the address and the number of bytes between MSB ans LSB
reg[0] = (UINT_8)(base >> 8); //MSB to the left
reg[1] = (UINT_8)(base & 0x00FF); //LSB to the right
words[0] = (UINT_8)((n2/2) >> 8); //MSB to the left
words[1] = (UINT_8)((n2/2) & 0x00FF); //LSB to the right
//Build the query
buf[length++] = 9; //slave address
buf[length++] = 16;
buf[length++] = reg[0];
buf[length++] = reg[1];
buf[length++] = words[0];
buf[length++] = words[1];
buf[length++] = n2;
//Copy data to the send buffer
for(i = 0; i < n; i++)
{
buf[length++] = data[i];
}
if(n != n2)
{
buf[length++] = 0;
}
CRC = rq_com_compute_crc(buf, length);
//Append the crc to the query
buf[length++] = (UINT_8)(CRC & 0x00FF);
buf[length++] = (UINT_8)(CRC >> 8);
//Send the query
rq_com_write_port(buf, length);
}
/**
* \fn static INT_32 rq_com_wait_for_fc_16_echo(void)
* \brief Reads the response to a fc16 write query
* \return 0 for an invalid response, 1 otherwise
*/
static INT_32 rq_com_wait_for_fc_16_echo(void)
{
static UINT_8 buf[MP_BUFF_SIZE];
static INT_32 length = 0;
static INT_32 old_length = 0;
static INT_32 counter_no_new_data = 0;
UINT_16 CRC = 0;
length = length + rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
//Clear the buffer if no new data
if(length == old_length)
{
if(counter_no_new_data < 5)
{
counter_no_new_data++;
}
else
{
length = 0;
}
}
else
{
counter_no_new_data = 0;
}
old_length = length;
if(length > 0)
{
//If not enough data, return
if(length < 8)
{
return 0;
}
else
{
//if it's a reply to a fc16 query then proceed
if(buf[1] == 16)
{
length = 8;
CRC = rq_com_compute_crc(buf, length - 2);
//Check the crc an the slave ID
if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
{
//Clear the buffer
length = 0;
return 0;
}
else
{
//Clear the buffer
length = 0;
return 1;
}
}
else //Clear the buffer
{
length = 0;
return 0;
}
}
}
return 0;
}
/**
* \brief Retrieves the sensor serial number
* \param serial_number address of the return buffer
*/
void rq_com_get_str_serial_number(INT_8 * serial_number)
{
strcpy(serial_number, rq_com_str_sensor_serial_number);
}
/**
* \brief Retrieves the sensor firmware version
* \param firmware_version Address of the return buffer
*/
void rq_com_get_str_firmware_version(INT_8 * firmware_version)
{
strcpy(firmware_version, rq_com_str_sensor_firmware_version);
}
/**
* \brief Retrieves the sensor firmware version
* \param production_year Address of the return buffer
*/
void rq_com_get_str_production_year(INT_8 * production_year)
{
strcpy(production_year,rq_com_str_sensor_production_year);
}
/**
* \brief retrieves the sensor firmware version
* \param production_year Address of the return buffer
*/
bool rq_com_get_stream_detected()
{
return rq_com_stream_detected;
}
/**
* \brief returns if the stream message is valid
*/
bool rq_com_get_valid_stream()
{
return rq_com_valid_stream;
}
/**
* \brief Return an effort component
* \param i Index of the component. 0 to 2 for Fx, Fy and Fz.
* 3 to 5 for Mx, My and Mz.
*/
float rq_com_get_received_data(UINT_8 i)
{
if(i >= 0 && i <= 5)
{
return rq_com_received_data[i] - rq_com_received_data_offset[i];
}
return 0.0;
}
/**
* \brief Returns true if a new valid stream message has been decoded and
* is available.
* \details When this function is called, the variable that indicates if a
new message is available is set to false even if the message
hasn't beed read.
*/
bool rq_com_got_new_message()
{
bool tmp = rq_com_new_message;
rq_com_new_message = false;
return tmp;
}
/**
* \brief Set the "zero sensor" flag to 1. When the next stream message will
be decoded, the effort values will be stored as offsets a
substracted from the next values
*/
void rq_com_do_zero_force_flag()
{
rq_com_zero_force_flag = 1;
}
/**
* \brief close the serial port.
* \warning Only valid under Windows.
*/
void stop_connection()
{
#if defined(_WIN32)||defined(WIN32) //For Windows
CloseHandle(hSerial);
hSerial = INVALID_HANDLE_VALUE;
#endif
}
/**
* \author Pierre-Olivier Proulx
* \brief try to discover a com port by polling each serial port
* \return 1 if a device is found, 0 otherwise
* \details For linux, we user the command fuser to determine if the
* tty is used by another process. If so, we do not scan it.
*/
#ifdef __unix__ //For Unix
static UINT_8 rq_com_identify_device(INT_8 const * const d_name)
{
FILE * fuser_output;
INT_8 fuser_cmd[40] = {0};
INT_8 fuser_ret[30] = {0};
INT_8 dirParent[20] = {0};
INT_8 port_com[15] = {0};
strcpy(dirParent, "/dev/");
strcat(dirParent, d_name);
strcpy(port_com, dirParent);
/* si un process utilise le tty, on le le scan pas. */
snprintf (fuser_cmd, 40, "fuser %s 2> /dev/null", port_com);
fuser_output = popen (fuser_cmd, "r");
if (fuser_output != NULL)
{
fgets (fuser_ret, 30, fuser_output);
fclose(fuser_output);
}
if (strlen (fuser_ret) != 0)
{
return 0;
}
fd_connexion = open(port_com, O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL);
//The serial port is open
if(fd_connexion != -1)
{
if(set_com_attribs(fd_connexion,B19200) != -1)
{
//Try connecting to the sensor
if (rq_com_tentative_connexion() == 1)
{
return 1;
}
}
//The device is identified, close the connection
close(fd_connexion);
}
return 0;
}
#endif
rq_sensor_socket.c
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file socket.c
* \date May 20, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#include "robotiq_ft/rq_sensor_socket.h"
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include "robotiq_ft/mutex.h"
//pour les mutex
#ifdef __unix__ //For Unix
#include <pthread.h>
#define MUTEX pthread_mutex_t
#elif defined(_WIN32)||defined(WIN32) //For Windows
#include <windows.h>
#include <process.h>
#define MUTEX HANDLE
#endif
#define CRLF "\r\n"
#define PORT 63350
#define PORT_STREAM 63351
#define MAX_CLIENTS 1
#define BUF_SIZE 512
#define CAPTEUR_STREAM 0
#define CAPTEUR_ACCESSEUR 1
static uint8_t message[BUF_SIZE];//message recu par TCP
static SOCKET s_socket = 0;//Socket du serveur
static SOCKET s_socket_stream = 0;//Socket du serveurStream
static SOCKET c_socket_acc;//Socket du client accesseur
static SOCKET c_socket_capt = 0;//Socket du client capteur
static MUTEX mtx;// Mutex
static MUTEX mtxWrite; // Mutex pour la fonction write
static int read_client(SOCKET sock, uint8_t *buffer);
static void start_socket(int iSock);
/**
* \fn void init_connection(void)
* \brief Fonction qui ouvre le socket serveur et du stream et attend un client stream.
*/
int init_connection() {
while(s_socket_stream == 0){
usleep(500000);
start_socket(CAPTEUR_STREAM);
}
while(s_socket == 0){
usleep(500000);
start_socket(CAPTEUR_ACCESSEUR);
}
//Attente de connection au serveur
SOCKADDR_IN csin;
int sinsize = sizeof(csin);
c_socket_capt = accept(s_socket_stream, (SOCKADDR *)&csin, &sinsize);//accept attend la connexion d'un client
return s_socket;
}
/**
* \fn void end_connection(int sock)
* \brief Fonction qui ferme le socket passer en parametre.
* \param sock, integer du socket a fermer.
*/
void end_connection(int sock) {
closesocket(sock);
}
/**
* \fn static int read_client(SOCKET sock, char *buffer)
* \brief Fonction qui lis ce que le client a envoyer.
* \param sock, SOCKET client
* \param *buffer, adresse memoire d'un buffer de char pour le message
* \return n, nombre de caractere dans le buffer
*/
static int read_client(SOCKET sock, uint8_t *buffer) {
int n = 0;
if ((n = recv(sock, buffer, BUF_SIZE - 1, 0)) < 0) {
return 0;//signifie une erreur
}
else{
MUTEX_LOCK(&mtx);
strcpy(message,buffer);
message[n] = 0;
MUTEX_UNLOCK(&mtx);
buffer[n] = 0;
return n;
}
}
/**
* \fn void write_client(SOCKET sock, const char *buffer)
* \brief Fonction qui envoie un message au client.
* \param sock, SOCKET client
* \param *buffer, adresse memoire d'un buffer de char pour le message a envoyer
*/
void write_client(SOCKET sock, uint8_t *buffer) {
MUTEX_LOCK(&mtxWrite);
int ret = send(sock, buffer, strlen(buffer), MSG_DONTWAIT);
if(ret == -1 && errno == EAGAIN){
ret = send(sock, buffer, strlen(buffer), MSG_DONTWAIT);
}
else if (ret == -1 && errno == ECONNRESET){
SOCKADDR_IN csin;
int sinsize = sizeof(csin);
if(sock == c_socket_capt){
close(c_socket_capt);
close(s_socket_stream);
start_socket(CAPTEUR_STREAM);
c_socket_capt = 0;
while(c_socket_capt == 0){
c_socket_capt = accept(s_socket_stream, (SOCKADDR *)&csin, &sinsize);//accept attend la connexion d'un client
}
}
else if(sock == c_socket_acc){
close(c_socket_acc);
close(s_socket);
start_socket(CAPTEUR_ACCESSEUR);
c_socket_acc = 0;
while(c_socket_acc == 0){
c_socket_acc = accept(s_socket, (SOCKADDR *)&csin, &sinsize);//accept attend la connexion d'un client
}
}
}
MUTEX_UNLOCK(&mtxWrite);
}
/**
* \fn void *Read_socket(void *t)
* \brief Fonction utiliser par le thread pour faire une boucle
* infini de lecture de message recu par le client et si client en erreur,
* on attend le prochain client qui se connecte et continu la boucle.
* \param *t, paramettre bidon pour que le thread fonctionne.
*/
#ifdef __unix__ //For Unix
void *Read_socket(void *t)
#elif defined(_WIN32)||defined(WIN32) //For Windows
void __cdecl Read_socket( void *t )
#endif
{
SOCKADDR_IN csin;
int sinsize = sizeof(csin);
char testConnexion[BUF_SIZE];
c_socket_acc = accept(s_socket, (SOCKADDR *)&csin, &sinsize);
while(1){
if(read_client(c_socket_acc, testConnexion) == 0){
close(c_socket_acc);
close(s_socket);
start_socket(CAPTEUR_ACCESSEUR);
//Si on perd la connexion on attend la prochaine.
c_socket_acc = accept(s_socket, (SOCKADDR *)&csin, &sinsize);
}
}
}
/**
* \fn void set_socket_message_to_null(char * chr_return)
* \brief Fonction qui vide la variable globale message.
*/
void set_socket_message_to_null(void){
MUTEX_LOCK(&mtx);
message[0] = 0;
MUTEX_UNLOCK(&mtx);
}
/**
* \fn void socket_message(char * chr_return)
* \brief Fonction qui retourne le message via le paramettre.
* \param * chr_return, adresse memoire pour evoie du message via le parammetre
* \param *buffer, adresse memoire d'un buffer de char pour le message a envoyer
*/
void socket_message(uint8_t * chr_return){
MUTEX_LOCK(&mtx);
strcpy(chr_return,message);
MUTEX_UNLOCK(&mtx);
}
/**
* \fn SOCKET get_client_sock_accessor(void)
* \brief Fonction qui retourne socket du client presentement connecter.
*/
SOCKET get_client_sock_accessor(void){
return c_socket_acc;
}
/**
* \fn SOCKET get_client_sock_stream(void)
* \brief Fonction qui retourne socket du capteur presentement connecter.
*/
SOCKET get_client_sock_stream(void){
return c_socket_capt;
}
/**
* \fn SOCKET get_socket_stream(void)
* \brief Fonction qui retourne socket serveur pour le stream.
*/
SOCKET get_socket_stream(){
return s_socket_stream;
}
static void start_socket(int iSock) {
SOCKET sock = socket(AF_INET, SOCK_STREAM, 0);
SOCKADDR_IN sin = { 0 };
if (sock == INVALID_SOCKET) {
return;
}
//Pour le socket des accesseur
sin.sin_addr.s_addr = htonl(INADDR_ANY);
if(iSock == CAPTEUR_ACCESSEUR) sin.sin_port = htons(PORT);
if(iSock == CAPTEUR_STREAM) sin.sin_port = htons(PORT_STREAM);
sin.sin_family = AF_INET;
//Permet d'ouvrir les sockets sur les bons ports
if(bind(sock,(SOCKADDR *) &sin, sizeof sin) == SOCKET_ERROR){
return;
}
//Permet l'ecoute des client
if(listen(sock, MAX_CLIENTS) == SOCKET_ERROR){
return;
}
if(iSock == CAPTEUR_ACCESSEUR) s_socket = sock;
if(iSock == CAPTEUR_STREAM) s_socket_stream = sock;
}
rq_sensor_state.c
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_sensor_state.c
* \date June 19, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
//
//Includes
#include <string.h>
#include "robotiq_ft/rq_sensor_state.h"
#include "robotiq_ft/rq_sensor_com.h"
///
//Private variables
static enum rq_sensor_state_values current_state = RQ_STATE_INIT;
///
//Private functions
static INT_8 rq_state_init_com();
static void rq_state_read_info_high_lvl();
static void rq_state_start_stream();
static void rq_state_run();
//
//Function definitions
/**
* \fn void rq_sensor_state(void)
* \brief Manages the states of the sensor driver
* \returns -1 if an error occurs, 0 otherwise
*/
INT_8 rq_sensor_state(void)
{
INT_8 ret;
switch (current_state)
{
case RQ_STATE_INIT:
ret = rq_state_init_com();
if(ret == -1)
{
return -1;
}
break;
case RQ_STATE_READ_INFO:
rq_state_read_info_high_lvl();
break;
case RQ_STATE_START_STREAM:
rq_state_start_stream();
break;
case RQ_STATE_RUN:
rq_state_run();
break;
default:
printf("rq_state(): Unknown error\r\n");
return -1;
break;
}
return 0;
}
/**
* \fn void rq_state_init_com(void)
* \brief Initialize communication with the sensor and set the
* next state to \ref RQ_STATE_READ_INFO
*/
static INT_8 rq_state_init_com()
{
if(rq_sensor_com() == -1)
{
return -1;
}
current_state = RQ_STATE_READ_INFO;
return 0;
}
/**
* \fn void rq_state_read_info_high_lvl(void)
* \brief Reads the high level information from the
* sensor and set the next state to
* \ref RQ_STATE_START_STREAM
*/
static void rq_state_read_info_high_lvl()
{
rq_sensor_com_read_info_high_lvl();
current_state = RQ_STATE_START_STREAM;
}
/**
* \fn void rq_state_start_stream(void)
* \brief Send the command to start the streaming mode
* If the stream doesn't start, return to state init.
* Set the next state to \ref RQ_STATE_RUN
*/
static void rq_state_start_stream()
{
if(rq_com_start_stream() == -1)
{
#if defined(_WIN32)||defined(WIN32) //For Windows
stop_connection();
#endif
current_state = RQ_STATE_INIT;
}
current_state = RQ_STATE_RUN;
}
/**
* \fn void rq_state_run()
* \brief Capture and read the stream from the sensor.
* If the stream is not valid, return to state
* \ref RQ_STATE_INIT
*/
static void rq_state_run()
{
rq_com_listen_stream();
if(rq_com_get_valid_stream() == false)
{
#if defined(_WIN32)||defined(WIN32) //For Windows
stop_connection();
#endif
current_state = RQ_STATE_INIT;
}
}
/**
* \fn float rq_state_get_received_data()
* \brief Returns the force/torque component
* \param i, index of the component
* \pre i has a value between 0 and 5
*/
float rq_state_get_received_data(UINT_8 i)
{
if(i >= 0 && i <= 5)
{
return rq_com_get_received_data(i);
}
else
{
return 0.0;
}
}
/**
* \fn int rq_state_get_command(char* name, char *value)
* \brief Gets the value of high level information from the sensor
* \param name the name of the information field. The value can be
* either "SNU", "FMW" or "PYE"
* \param value A string
* \return 0 in case of succes, -1 otherwise
*/
void rq_state_get_command(INT_8 const * const name, INT_8 * const value)
{
//precondition, null pointer
if( value == NULL || name == NULL)
{
return;
}
if(strstr(name, "SNU"))
{
rq_com_get_str_serial_number( value);
}
else if(strstr(name, "FWV"))
{
rq_com_get_str_firmware_version( value);
}
else if(strstr(name, "PYE"))
{
rq_com_get_str_production_year( value);
}
}
/**
* \fn enum rq_sensor_state_values rq_sensor_get_current_state()
* \brief Returns this module's state machine current state
*/
enum rq_sensor_state_values rq_sensor_get_current_state()
{
return current_state;
}
/**
* \brief Returns true if a stream message is available
*/
bool rq_state_got_new_message()
{
return rq_com_got_new_message();
}
/**
* \brief Command a zero on the sensor
*/
void rq_set_zero()
{
rq_com_do_zero_force_flag();
}
rq_thread.c
/* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Robotiq, 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 Robotiq, Inc. 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.
*
* Copyright (c) 2014, Robotiq, Inc
*/
/*
* \file rq_thread.c
* \date July 3, 2014
* \author Jonathan Savoie <jonathan.savoie@robotic.com>
* \maintener Nicolas Lauzier <nicolas@robotiq.com>
*/
#include "robotiq_ft/rq_thread.h"
#ifdef __unix__ //For Unix
#include <pthread.h>
#include <signal.h>
static pthread_t threads[5];
static int libre[5] = {0,0,0,0,0};
int create_Thread(void *(*start_rtn)(void *))
{
int i = 0;
long t = 0;
for(i = 0;i < 5; i++){
if (libre[i] == 0){
libre[i] = 1;
break;
}
}
int rc = pthread_create(&threads[i], NULL, start_rtn, (void *)t);
if (rc != 0){
//printf("ERROR; return code from pthread_create() is %d\n", rc);
return -1;
}
return i;
}
int close_Thread(int iId){
if(iId >= 0 || iId < 5){
pthread_kill(threads[iId], SIGKILL);
libre[iId] = 0;
threads[iId] = 0;
return 0;
}
else{
return 1;
}
}
void wait_thread(int iId){
if(iId >= 0 || iId < 5){
pthread_join(threads[iId], NULL);
libre[iId] = 0;
threads[iId] = 0;
}
}
#elif defined(_WIN32)||defined(WIN32) //For Windows
#include <process.h>
#include <windows.h>
HANDLE pID[5];
int libre[5] = {0,0,0,0,0};
//int create_Thread(int iId,unsigned (__stdcall *start_address)( void * ))
int create_Thread(void( __cdecl *start_address )( void * ))
{
int i;
for(i = 0;i < 5; i++){
if (libre[i] == 0){
libre[i] = 1;
break;
}
}
pID[i] = (HANDLE)_beginthread(start_address,0,NULL);
if(pID[i] == NULL){
return -1;
}
return i;
}
int close_Thread(int iId){
if(iId >= 0 || iId < 5){
CloseHandle(pID[iId]);
libre[iId] == 0;
pID[iId] = NULL;
}
}
void wait_thread(int iID){
if(iID >= 0 || iID < 5){
WaitForSingleObject(pID[iID], INFINITE);
}
}
#endif
7.Ctrl+Shift+B编译项目
8.连接力矩传感器
将力矩传感器USB连接电脑，给力矩传感器供电。可参考教程Robotiq F/T 300S力矩传感器教程
9. 启动roscore
roscore10.发布话题消息
重新打开一个终端，启动launch文件，发布话题消息，会发布力矩传感器采集的六维力和力矩信息。ROS通信可参考视频教程ROS通信bilibili和官方文档教程ROS课程文档
source ./devel/setup.bash
roslaunch robotiq_ft my_robotiq_ft.launch11.接收话题消息
重新打开一个终端，启动robotiq_sub文件，接收话题消息
source ./devel/setup.bash
rosrun robotiq_ft robotiq_sub标签：数据通信,INT,void,UINT,static,力矩,ROS,com,rq From： https://blog.csdn.net/bulletstart/article/details/144527685如果通信不成功，先用官方的给的Linux文件先试一遍力矩传感器传输数据是否可行