HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

229
Devices/Libraries/Ros/md_controller/CMakeLists.txt Executable file
View File

@@ -0,0 +1,229 @@
cmake_minimum_required(VERSION 3.0.2)
project(md_controller)
## Compile as C++17, supported in ROS Kinetic and newer
add_compile_options(-std=c++17)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
libserial
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}_lib
CATKIN_DEPENDS roscpp rospy std_msgs libserial
DEPENDS Boost
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}_lib
src/md_controller.cpp
src/diagnostic.cpp
src/md_controller_subscriber.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/md_controller_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
install(TARGETS ${PROJECT_NAME}_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}_lib
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
install(DIRECTORY
launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
motorInfomation.yaml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_md_controller.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

15
Devices/Libraries/Ros/md_controller/README.md Executable file
View File

@@ -0,0 +1,15 @@
#KHởi tạo USB->rs485:
Bước 1: Cấu hình USB->rs485:
- Mở cổng terminal, Gõ lệnh "sudo touch /etc/udev/rules.d/99-usb-serial.rules"
- Mở file rules "sudo nano /etc/udev/rules.d/99-usb-serial.rules"
- Copy 2 dòng lệnh sau:
- KERNEL=="ttyUSB[0-9]*", OWNER="robotics"
- SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{manufacturer}=="PRATI", ATTRS{product}=="USB_MD200", SYMLINK+="USB_MD200"
- Save và exit
- kiểm tra thuộc tính "udevadm info --name=/dev/ttyUSB0 --attribute-walk"
- Nếu đã cấu hình rồi thì bỏ qua bước 1, IPC sẽ tự động thực hiện và cấp quyền cho cổng serial mỗi khi khởi động. Đồng thời, Không cần dùng lệnh "sudo chmod 666 /dev/ttyUSB*".
Bước 2:
- Kiểm tra USB->rs485 đã kết nối ok chưa. Gõ lệnh "ls /dev". Bạn sẽ thấy thiết bị tên "AGV-BLDV20KM". Nếu không có thì chưa kết nối được
- KHởi chạy chương trình "roslaunch driver_blvd_controller agvrun.launch"

112
Devices/Libraries/Ros/md_controller/include/md_controller/define.h Executable file
View File

@@ -0,0 +1,112 @@
#ifndef __MD_CTRL_DEFINE_H_INCLUDED
#define __MD_CTRL_DEFINE_H_INCLUDED
#define HEADER_RMID_SEND 0xB7
#define HEADER_TMID_SEND 0XAC
#define MAX_BUFFER_LENGHT 2084
#define PACKAGE_MSG_MAIN_DATA 22
#define MAX_ID_LENGHT 253
#define TIME_PUBLISH_TO_SEND 7500 //us
/* Frame of messgae package*/
typedef struct _Header
{
uint8_t RMID = HEADER_RMID_SEND;
uint8_t TMID;
}header_t;
typedef struct _ID_Number
{
uint8_t adr[MAX_ID_LENGHT];
uint8_t size;
}id_number_t;
typedef struct _Parameter_ID
{
/* data */
uint8_t PID;
}param_id_t;
typedef struct Data_number
{
uint8_t NUM;
}dt_num_t;
/***********************************************************************************************************/
typedef struct _filter
{
uint8_t _buffer[MAX_BUFFER_LENGHT];
uint8_t result[PACKAGE_MSG_MAIN_DATA];
unsigned long r_index;
unsigned long w_index;
unsigned long c_flag;
}filter_t;
enum PID_COMMAND
{
CMD_TQ_OFF = 2, // Tq-off, motor free state
CMD_BRAKE = 4, // Erectric brake
CMD_MAIN_BC_ON = 5, // PID_MAIN_DATA broadcasting ON
CMD_MAIN_BC_OFF = 6, // broadcasing OFF
CMD_ALARM_RESET = 8, // Reset alarm
CMD_POSI_RESET = 10, // Position reset(set position to zero)
CMD_MONITOR_BC_ON = 11, // PID_MONITOR broadcasting ON
CMD_MONITOR_BC_OFF = 12, // Broadcasting off
CMD_IO_MONITOR_ON = 13, // PID_IO_MONITOR BC ON
CMD_IO_MONITOR_OFF = 14, // PID_IO_MONITOR BC OFF
CMD_FAN_ON = 15, // Fan ON(motor cooling fan)
CMD_FAN_OFF = 16, // Fan OFF
CMD_CLUTCH_ON = 17, // Mechanical brake(clutch) ON
CMD_CLUTCH_OFF = 18, // Mechanical breka OFF
CMD_TAR_VEL_OFF = 20, // Erase target vel, set by PID_TAR_VEL
CMD_SLOW_START_OFF = 21, // Erase target slow/start value
CMD_SLOW_DOWN_OFF = 22, // Erase target slow/down vaule
CMD_CAN_RESEND_ON = 23, // Send CAN data to RS485 serial port.
CMD_CAN_RESEND_OFF = 24, // Turn off resending of CAN data
CMD_MAX_TQ_OFF = 25, // Erase target limit load(max. current)
CMD_ENC_OFF = 26, // Cancel the use of encoder sensor.
CMD_LOW_SPEED_LIMIT_OF = 27, // Cancel the set of low speed limit
F_HIGH = 28, // Cancel the set of high speed limit.
CMD_SPEED_LI_OFF =29, // Cancel the set of low/high speed limits
CMD_CURVE_OFF = 31, // Cancel set of curve fitting func.
CMD_STEP_OFF = 32, // Cancel step input mode
CMD_UICOM_OFF = 44, // I/O control(ctrl 11pin cnt) available
CMD_UICOM_ON = 45, // I/O control disable(when comm. is used)
CMD_MAX_RP_OFF = 46, // Cancel max. speed set by DIP SW
CMD_HALL_TY_OFF = 47, // Cancel set of motor hall type
CMD_LOW_POT_OFF = 48, // Cancel set of low limit of POT input
CMD_HIGH_POT_OFF = 49, // Cancel set of high limit of POT input
CMD_MAIN_BC_ON2 = 50, // PID_MAIN_DATA, BC ON for 2nd motor
CMD_MAIN_BC_OFF2 = 51, // PID_MAIN_DATA, BC OFF for 2nd motor
CMD_MONIT_BC_ON2 = 52, // PID_MONITOR, BC ON for 2nd motor
CMD_MONIT_BC_OFF2 = 53, // PID_MONITOR, BC OFF for 2nd motor
CMD_IO_MONIT_BC_ON2 = 54, // PID_IO_MONITOR, BC ON for 2nd motor
CMD_IO_MONIT_BC_OFF2 = 55 // PID_IO_MONITOR, BC OFF for 2nd motor
};
enum _BIT
{
_OFF = 0x00u,
_ON = 0x01u
};
enum _BAUD_OF_MD200
{
_9600bps = 1,
_19200bps = 2,
_38400bps = 3,
_57600bps = 4,
_115200bps = 5
};
typedef enum EXIT_CODES_ENUM
{
EXIT_OK = 0,
EXIT_ERROR = 1,
EXIT_CONFIG_ERROR = -1
} EXIT_CODES;
#endif // __MD_CTRL_DEFINE_H_INCLUDED

94
Devices/Libraries/Ros/md_controller/include/md_controller/diagnostic.h Executable file
View File

@@ -0,0 +1,94 @@
#ifndef __MD_CTRL_DIAGNOTICS_H_INCLUDED
#define __MD_CTRL_DIAGNOTICS_H_INCLUDED
#include <ros/ros.h>
namespace MD
{
typedef enum DIAGNOSTIC_STATUS_ENUM
{
OK, // status okay, no errors
EXIT, // delta exiting
ERROR_STATUS, // device signaled an error
CONFIGURATION_ERROR, // invalid configuration, check configuration files
INITIALIZATION_ERROR, // initialization of driver failed
INTERNAL_ERROR // internal error, should never happen
} DIAGNOSTIC_STATUS;
/*
* class Diagnostic publishes diagnostic messages for sick_line_guidance
*/
class Diagnostic
{
public:
/*
* Initializes the global diagnostic handler.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
static void init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
{
g_diagnostic_handler.init(nh, publish_topic, component);
}
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
static void update(DIAGNOSTIC_STATUS status, const std::string & message = "")
{
g_diagnostic_handler.update(status, message);
}
protected:
/*
* class DiagnosticImpl implements diagnostic for sick_line_guidance
*/
class DiagnosticImpl
{
public:
/*
* Constructor.
*/
DiagnosticImpl();
/*
* Initialization.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component);
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
void update(DIAGNOSTIC_STATUS status, const std::string & message = "");
protected:
/*
* member data.
*/
bool m_diagnostic_initialized; // flag indicating proper initialization of diagnostic
ros::Publisher m_diagnostic_publisher; // publishes diagnostic messages
std::string m_diagnostic_component; // name of the component publishing diagnostic messages
}; // class DiagnosticImpl
static DiagnosticImpl g_diagnostic_handler; // singleton, implements the diagnostic for sick_line_guidance
}; // class Diagnostic
}
#endif // __MD_CTRL_DIAGNOTICS_H_INCLUDED

78
Devices/Libraries/Ros/md_controller/include/md_controller/md_controller.h Executable file
View File

@@ -0,0 +1,78 @@
#ifndef __MD_CTRL_H_INCLUDED
#define __MD_CTRL_H_INCLUDED
#include <ros/ros.h>
#include <vector>
#include "libserial/rs485.h"
#include "md_controller/define.h"
#include "md_controller/diagnostic.h"
#include <boost/thread/thread.hpp>
namespace MD
{
// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
class MdController: public rs485
{
public:
MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit);
virtual ~MdController();
// virtual void set_slave_id(int *ids, uint8_t lenght);
// virtual void publish_speed(int16_t *speed);
// virtual void main_BC_ON();
// virtual void main_BC_OFF();
/* COMMAND */
virtual void pid_defaut_set(uint8_t address);
virtual void pid_tq_off(uint8_t address);
virtual void pid_break(uint8_t address);
virtual void pid_stop(uint8_t address);
virtual void pid_command(uint8_t address, PID_COMMAND cmd);
virtual void pid_alarm_reset(uint8_t address);
virtual void pid_posi_reset(uint8_t address);
virtual void pid_main_bc_state(uint8_t address, _BIT data);
virtual void pid_monitor_bc_state(uint8_t address, _BIT data);
virtual void pid_preset_save(uint8_t address, uint8_t data);
virtual void pid_preset_recall(uint8_t address, uint8_t data);
virtual void pid_vel_cmd(uint8_t address, int16_t speed);
virtual void pid_vel_cmd2(uint8_t address, int16_t high_speed);
virtual void pid_open_vel_cmd(uint8_t address, int16_t data);
bool err{};
int err_no{};
private:
id_number_t slave_id;
uint8_t rate = 100;
boost::thread ThreadOne,ThreadTwo;
bool theadEnable;
filter_t cs;
ros::NodeHandlePtr nh_;
XmlRpc::XmlRpcValue drivers;
virtual uint8_t GetcheckSum(uint length, uint8_t *to_send);
virtual void buidRequest(uint8_t *to_send,header_t hd ,uint8_t add, param_id_t pid, dt_num_t num) const;
virtual int Read(uint8_t *read_buf, ssize_t &num);
virtual int Write(header_t hd ,uint8_t address, param_id_t pid, dt_num_t num, uint8_t *value);
virtual bool check_baudrate(unsigned int baud);
virtual void set_bad_con(void);
// virtual void loadParam(void);
/*Write*/
virtual void pid_id_setting(uint8_t id_setting);
virtual void pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting);
/* READ */
// static void cmdMainBCdoStuff(void *payload, uint8_t *publish_rate);
};
} // namespace MD
#endif // __MD_CTRL_H_INCLUDED

70
Devices/Libraries/Ros/md_controller/include/md_controller/md_controller_subscriber.h Executable file
View File

@@ -0,0 +1,70 @@
#ifndef __MD_CTRL_SUBSCRIBER_H_INCLUDED
#define __MD_CTRL_SUBSCRIBER_H_INCLUDED
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <std_msgs/Float32.h>
namespace MD
{
class Subscriber
{
public:
/*
* @brief Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] ros_topic topic for ros messages
* @param[in] subscribe_queue_size buffer size for ros messages
* @param[in] wait_time Time for checking to Wheel stop (1 second)
*/
Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size = 1, double schedule_delay = 0.05, double schedule_delay_lastest = 0.5);
Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object);
/**
* @brief returns true, if publishing of a wheel topic is publishing
*
*/
bool isWheelTriggered(void);
/**
* @brief returns true, if not subcribe wheel
*/
bool isWheelLastestTriggered(void);
/**
* @brief schedules WheelController function
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
void scheduleWheelController(bool schedule);
/*
* @brief Destructor.
*/
virtual ~Subscriber();
boost::mutex m_measurement_mutex;
int16_t speed;
uint8_t slave_id;
double ratio;
protected:
/*
* @brief Callbacks for ros messages from wheel topic.
*/
virtual void wheelCallback(const std_msgs::Float32::Ptr & msg);
ros::NodeHandle nh_;
std::string topic_name_;
int m_subscribe_queue_size_;
ros::Subscriber control_wheel_sub_;
ros::Time publish_time_;
ros::Time publish_lastest_time_;
ros::Duration schedule_delay_;
ros::Duration schedule_lastest_delay_;
boost::mutex publish_mutex_;
};
}
#endif // __MD_CTRL_SUBSCRIBER_H_INCLUDED

13
Devices/Libraries/Ros/md_controller/launch/run.launch Executable file
View File

@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<launch>
<rosparam command="load" file="$(find md_controller)/motorInfomation.yaml"/>
<arg name="node_name" default="md_controller"/>
<arg name="product" default=""/>
<arg name="port_name" default=""/>
<arg name="baudrate" default="19200"/>
<node pkg="md_controller" type="md_controller_node" name="$(arg node_name)" output="screen">
<param name="product" type="str" value="$(arg product)"/>
<param name="port_name" type="str" value="$(arg port_name)"/>
<param name="baudrate" type="int" value="$(arg baudrate)"/>
</node>
</launch>

15
Devices/Libraries/Ros/md_controller/motorInfomation.yaml Executable file
View File

@@ -0,0 +1,15 @@
port_name: /dev/ttyTHS0
baudrate: 19200
producte : no_name
drivers:
lelf_driver:
id: 0x02
ratio: -30.0
toppic: "/left_wheel"
subscribe_queue_size: 1
right_driver:
id: 0x01
ratio: 30.0
toppic: "/right_wheel"
subscribe_queue_size: 1

74
Devices/Libraries/Ros/md_controller/package.xml Executable file
View File

@@ -0,0 +1,74 @@
<?xml version="1.0"?>
<package format="2">
<name>md_controller</name>
<version>0.0.0</version>
<description>The md_controller package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/md_controller</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>libserial</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>libserial</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>libserial</exec_depend>
<!-- <depend>agv_define</depend> -->
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

76
Devices/Libraries/Ros/md_controller/src/diagnostic.cpp Executable file
View File

@@ -0,0 +1,76 @@
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include "md_controller/diagnostic.h"
/**
* g_diagnostic_handler: singleton, implements the diagnostic for sick_line_guidance
*/
MD::Diagnostic::DiagnosticImpl MD::Diagnostic::g_diagnostic_handler;
/*
* Constructor.
*/
MD::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("")
{
}
/*
* Initialization.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
void MD::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
{
m_diagnostic_publisher = nh.advertise<diagnostic_msgs::DiagnosticArray>(publish_topic, 1);
m_diagnostic_component = component;
m_diagnostic_initialized = true;
}
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
void MD::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message)
{
if (m_diagnostic_initialized)
{
static std::map<DIAGNOSTIC_STATUS, std::string> status_description = {
{DIAGNOSTIC_STATUS::OK, "OK"},
{DIAGNOSTIC_STATUS::EXIT, "EXIT"},
{DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"},
{DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"},
{DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"},
{DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"}
};
// create DiagnosticStatus
diagnostic_msgs::DiagnosticStatus msg;
msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation
msg.name = m_diagnostic_component; // description of the test/component reporting
msg.hardware_id = ""; // hardware unique string (tbd)
msg.values.clear(); // array of values associated with the status
// description of the status
msg.message = status_description[status];
if(msg.message.empty())
{
msg.message = "ERROR";
}
if (!message.empty())
{
msg.message = msg.message + ": " + message;
}
// publish DiagnosticStatus in DiagnosticArray
diagnostic_msgs::DiagnosticArray msg_array;
msg_array.header.stamp = ros::Time::now();
msg_array.header.frame_id = "";
msg_array.status.clear();
msg_array.status.push_back(msg);
m_diagnostic_publisher.publish(msg_array);
}
}

666
Devices/Libraries/Ros/md_controller/src/md_controller.cpp Executable file
View File

@@ -0,0 +1,666 @@
#include "md_controller/md_controller.h"
MD::MdController::~MdController(){}
/*
* Object
* @param devfile Path name of dev
* @param baud Baudrate
* @param parity Parity
* @param data_bit Data bit
* @param stop_bit Stop bit
*/
MD::MdController::MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit)
: nh_(nh), rs485(devfile,baud, parity, data_bit, stop_bit)
{
if(this->_connected == true)
{
if(!this->check_baudrate(baud))
{
std::stringstream msg;
msg << "DRIVER not support baudrate" << baud << "bps";
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, msg.str());
ROS_ERROR_STREAM("md_controller_node: " << msg.str());
this->close_port();
exit(-1);
}
else
{
std::stringstream msg;
msg << " Connected "<<devfile<<" with "<<baud<<" bps";
this->err = false;
this->err_no = 0;
ROS_INFO_STREAM("md_controller_node: " << msg.str());
ros::Duration(0.7).sleep(); // sleep for 0.5 second
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::OK, msg.str());
}
}
else
{
ros::Duration(2).sleep();
std::stringstream err;
err << error_msg;
ROS_ERROR_STREAM(error_msg);
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, error_msg);
}
}
// /**
// * Get parameter from yaml file
// */
// void MD::MdController::loadParam(void)
// {
// int node_id[this->drivers.size()];
// int count = 0;
// for(XmlRpc::XmlRpcValue::iterator _iter = this->drivers.begin(); _iter != this->drivers.end(); ++_iter)
// {
// std::string driver_name = readMember<std::string>(_iter->second, "name", _iter->first);
// if(_iter->second.hasMember("id"))
// {
// node_id[count] = _iter->second["id"];
// ROS_INFO("md_controller: initializing driver_name \"%s\", id \"0x0%d\"...", driver_name.c_str(),node_id[count]);
// count++;
// } else
// {
// ROS_ERROR_STREAM("Node '" << _iter->first << "' has no id");
// MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " Node name has no id");
// }
// }
// this->set_slave_id(node_id,count);
// }
/*
* Check baudrate has supported for device
* @param baud Package Array size byte
* @param return true/false
*/
bool MD::MdController::check_baudrate(unsigned int baud)
{
bool status = false;
unsigned int data_defaut[] = {9600, 19200, 38400, 57600, 115200};
for(uint8_t i = 0; i < (uint8_t)sizeof(data_defaut)/sizeof(unsigned int); i++)
{
if(baud == data_defaut[i])
{
status = true;
break;
}
}
return status;
}
/*
* Checksum
* @param length Package Array size byte
* @param to_send Package Array
* @param return byte
*/
uint8_t MD::MdController::GetcheckSum(uint length, uint8_t *to_send)
{
uint8_t byTmp = 0;
for(short i = 0; i < length; i++ ) byTmp += *(to_send + i);
return (~byTmp +1);
} //GetcheckSum
/**
* Set connection is bad
*/
void MD::MdController::set_bad_con(void)
{
this->err = true;
ROS_ERROR("BAD CONNECTION");
}
/*
* Modbus Request Builder
* @param to_send Message Buffer input
* @param hd Header
* @param add ID Number
* @param pid Parameter ID
* @param num Data number
*/
void MD::MdController::buidRequest(uint8_t *to_send,header_t hd ,
uint8_t address, param_id_t pid, dt_num_t num) const
{
to_send[0] = hd.RMID;
to_send[1] = hd.TMID;
to_send[2] = address;
to_send[3] = pid.PID;
to_send[4] = num.NUM;
} // buidRequest
/*
* Write Request Builder and Sender
* @param address Reference Address
* @param lenght Amount of data to be Written
* @param value Data to Be Written
*/
int MD::MdController::Write(header_t hd ,uint8_t address, param_id_t pid,
dt_num_t num, uint8_t *value)
{
int status = 0;
uint length = num.NUM + 5;
uint8_t to_send[length+1];
this->buidRequest(to_send,hd,address,pid,num);
for(uint8_t i = 0; i < num.NUM; i++) to_send[length -num.NUM +i] = value[i];
to_send[length] = this->GetcheckSum(length,to_send);
struct stat sb;
if(stat(this->ctx._port, &sb) < 0) this->reconnect();
if(this->_connected)
{
status = this->sendMsgs(to_send,length+1);
}
return status;
}
/*
* Read Request Builder and Sender
* @param address Reference Address
* @param lenght Amount of Data to Read
* @param return EXIT_CODES Type
*/
int MD::MdController::Read(uint8_t *read_buf, ssize_t &num)
{
ssize_t k = this->receiveMsgs(read_buf);
num = k;
if(k < 0)
{
this->set_bad_con();
return EXIT_CONFIG_ERROR;
}
else{
return EXIT_OK;
}
}
/*
* Default setting
* @param address ID
* Data : 0x55(CHECK)
* 183, 172, ID, 3, 1, 0x55, CHK
*/
void MD::MdController::pid_defaut_set(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 3;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0x55u; //Data : 0x55(CHECK)
this->Write(hd,address,pid,num,value);
}
/*
* Stop naturally
* @param address ID
* Stop motor naturally, data don’t care(x).
* 183, 172, ID, 5, 1, x, CHK
*/
void MD::MdController::pid_tq_off(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 5;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0X01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Erectric brake
* @param address ID
* Stop motor urgently(electric braking mode)
* 183, 172, ID, 6, 1, x, CHK
*/
void MD::MdController::pid_break(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 6;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0x01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* stop
* @param address ID
* Stop motor urgently(electric braking mode)
* 183, 172, ID, 6, 1, x, CHK
*/
void MD::MdController::pid_stop(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 5;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0x01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Pid command
* @param address ID
* @param cmd Contents on CMD number (forllow page 6-CommunicationProtocolOnRS485_V44)
* 183, 172, ID, 10, 1, CMD, CHK
*/
void MD::MdController::pid_command(uint8_t address, PID_COMMAND cmd)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 10;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = cmd;
this->Write(hd,address,pid,num,value);
}
/*
* Reset alarm
* @param address ID
* Data don’t care(x)
* 183, 172, ID, 12, 1, x, CHK
*/
void MD::MdController::pid_alarm_reset(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 12;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0X01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Reset position, Motor position to zero
* @param address ID
* Data don’t care(x)
* 183, 172, ID, 13, 1, x, CHK
*/
void MD::MdController::pid_posi_reset(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 13;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0X01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Request broadcasting of PID_MAIN_DATA
* @param address ID
* @param data DATA 1 : PID 193 broadcasting on OR 0 : broasdcasting off
* 183, 172, ID, 14, 1, DATA, CHK
*/
void MD::MdController::pid_main_bc_state(uint8_t address, _BIT data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 14;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
/*
* Request BC on/off of PID_MONITOR
* @param address ID
* @param data DATA 1 : PID 196 broadcasting on OR 0 : broasdcasting off
* 183, 172, ID, 15, 1, DATA, CHK
*/
void MD::MdController::pid_monitor_bc_state(uint8_t address, _BIT data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 15;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
/*
* Save preset position
* @param address ID
* @param data DATA Preset number(address, 1~20)
* Set current position to the preset address
* 183, 172, ID, 31, 1, DATA, CHK
*/
void MD::MdController::pid_preset_save(uint8_t address, uint8_t data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 31;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
if(data > 0 && data <=20)
{
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_PRESET_SAVE' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44");
}
}
/*
* Go to the recalled preset position
* @param address ID
* @param data DATA Preset number(address, 1~20)
* Recall the saved preset data and move to thatposition.(position control)
* 183, 172, ID, 32, 1, DATA, CHK
*/
void MD::MdController::pid_preset_recall(uint8_t address, uint8_t data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 32;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
if(data > 0 && data <=20)
{
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44");
}
}
/*
* Velocity command
* @param address ID
* @param speed Speed(rpm) = (D1 | D2<<8)
* Speed>0, CCW direction
* Speed<0, CW direction
* 183, 172, ID, 130, 2, D1, D2, CHK
*/
void MD::MdController::pid_vel_cmd(uint8_t address, int16_t speed)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 130;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
value[num.NUM - 2] = speed & 0x00FFu;
value[num.NUM - 1] = speed >> 8u;
this->Write(hd,address,pid,num,value);
}
/*
* Velocity command used more than 25,000rpm
* @param address ID
* @param high_speed Speed(rpm) = (D1 | D2<<8) x 10
* Speed>0, CCW direction
* Speed<0, CW direction
* 183, 172, ID, 131, 2, D1, D2, CHK
*/
void MD::MdController::pid_vel_cmd2(uint8_t address, int16_t high_speed)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 131;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
value[num.NUM - 2] = high_speed*10 & 0x00FFu;
value[num.NUM - 1] = high_speed*10 >> 8u;
this->Write(hd,address,pid,num,value);
}
/*
* Velocity command used more than 25,000rpm
* @param address ID
* @param data D1, D2: Open-loop velocity
* Range : -1023~1023
* 183, 172, ID, 134, 2, D1, D2, CHK
*/
void MD::MdController::pid_open_vel_cmd(uint8_t address, int16_t data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 134;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
if(data >= -1023 && data <= 1023)
{
value[num.NUM - 2] = data & 0x00FFu;
value[num.NUM - 1] = data >> 8u;
this->Write(hd,address,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (-1023 ~ 1023). Prefer page 10 CommunicationProtocolOnRS485_V44");
}
}
/*
* ID setting
* @param id_setting ID : 1~253 : setting ID
* Write command(0xaa)
* 183, 172, 254, 133, 2, 0xaa, ID, CHK
*/
void MD::MdController::pid_id_setting(uint8_t id_setting)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 133;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
if(id_setting > 0 && id_setting <=253)
{
value[num.NUM - 2] = 0xAAu;
value[num.NUM - 1] = id_setting;
this->Write(hd,254,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_ID' element 'id_setting' number must in ranges (1 ~ 253). Prefer page 12 CommunicationProtocolOnRS485_V44");
}
}
/*
* Set the baudrate, BAUD
* @param address ID
* @param baudrate_setting Baudrate setting(RS485)
* Write command(0xaa)
* 183, 172, ID, 135, 2, 0xaa, BAUD, CHK
*/
void MD::MdController::pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 133;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
value[num.NUM - 2] = 0xAAu;
value[num.NUM - 1] = (uint8_t)baudrate_setting;
this->Write(hd,address,pid,num,value);
}
// /*
// * Set slave id
// * @param ids Pointer save array slave id
// * @param lenght Lenght of ids array (sizeof(ids)/sizeof(uint8_t))
// */
// void MD::MdController::set_slave_id(int *ids, uint8_t lenght)
// {
// uint8_t n = lenght;
// for(uint8_t i = 0; i < n; i++)
// {
// for(int j = 0; j < n; j++)
// {
// if(ids[j] == ids[i] && j != i)
// {
// ids[j] = ids[j+1];
// n--;
// }
// }
// }
// if(n != lenght) ROS_WARN("There are %d ID input to be same!!", lenght - n);
// this->slave_id.size = n;
// for(uint8_t k = 0; k < n; k++) this->slave_id.adr[k] = ids[k];
// }
// /*
// * Set slave id
// * @param speed Speed list input
// */
// void MD::MdController::publish_speed(int16_t *speed)
// {
// if(this->slave_id.size > 0)
// {
// for(uint8_t i = 0; i < this->slave_id.size; i++)
// {
// if(abs(speed[i]) > 0)
// this->pid_vel_cmd(this->slave_id.adr[i],speed[i]);
// else
// this->pid_break(this->slave_id.adr[i]);
// }
// }else
// {
// ros::Duration(3).sleep(); // sleep for 3 second
// ROS_ERROR("There are not slave ID of devices MD Motor");
// }
// }
// /*
// * PID_MAIN_DATA broadcasting ON
// * @param CMD_MAIN_BC_ON 5
// */
// void MD::MdController::main_BC_ON()
// {
// if(this->slave_id.size > 0)
// {
// for(uint8_t i = 0; i < this->slave_id.size; i++)
// {
// this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_ON);
// usleep(30000);
// }
// this->theadEnable = true;
// if(this->ThreadOne.joinable() == false)
// {
// this->ThreadOne = boost::thread(this->cmdMainBCdoStuff,this,&this->rate);
// }
// }else
// {
// ros::Duration(3).sleep(); // sleep for 3 second
// ROS_ERROR("There are not slave ID of devices MD Motor");
// }
// }
// /*
// * PID_MAIN_DATA broadcasting OFF
// * @param CMD_MAIN_BC_OFF 6
// */
// void MD::MdController::main_BC_OFF()
// {
// if(this->slave_id.size > 0)
// {
// for(uint8_t i = 0; i < this->slave_id.size; i++)
// {
// this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_OFF);
// usleep(30000);
// }
// this->theadEnable = false;
// if(this->ThreadOne.joinable() == true)
// this->ThreadOne.join();
// }else
// {
// ros::Duration(3).sleep(); // sleep for 3 second
// ROS_ERROR("There are not slave ID of devices MD Motor");
// }
// }
// /*
// * Read data
// * @param payload This
// * @param publish_rate Loop rate
// */
// void MD::MdController::cmdMainBCdoStuff(void *payload, uint8_t *publish_rate)
// {
// MD::MdController *as = (MD::MdController *)payload;
// ros::Rate loop_rate(*publish_rate);
// ROS_INFO("TheadOne to read msg devices motor is ready !!");
// while(ros::ok() && as->theadEnable)
// {
// ssize_t num;
// uint8_t msg[256];
// as->Read(msg,num);
// as->cs.w_index += num;
// if(as->cs.w_index >= MAX_BUFFER_LENGHT) {
// uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.w_index - num);
// for(uint8_t i = 0; i < l_part; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i];
// as->cs.w_index = num - l_part;
// for(uint8_t i = l_part; i < num; i++) as->cs._buffer[as->cs.w_index - num + l_part + i] = msg[i];
// as->cs.c_flag++;
// }
// else {
// for(uint8_t i = 0; i < num; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i];
// }
// if(as->cs.w_index - as->cs.r_index > PACKAGE_MSG_MAIN_DATA || as->cs.c_flag > 0)
// {
// if(as->cs._buffer[as->cs.r_index] == 172 && as->cs._buffer[as->cs.r_index + 1] == 183)
// {
// int num_data = as->cs._buffer[as->cs.r_index + 4];
// if(as->cs.w_index - as->cs.r_index >= 6 + num_data)
// {
// as->cs.r_index += 6 + num_data;
// if(as->cs.r_index >= MAX_BUFFER_LENGHT)
// {
// as->cs.c_flag--;
// uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.r_index - PACKAGE_MSG_MAIN_DATA);
// for(uint8_t i = 0; i < l_part; i++)
// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i];
// as->cs.r_index = PACKAGE_MSG_MAIN_DATA - l_part;
// for(uint8_t i = l_part; i < PACKAGE_MSG_MAIN_DATA; i++)
// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + l_part + i ];
// }
// else
// {
// for(uint8_t i = 0; i < PACKAGE_MSG_MAIN_DATA; i++)
// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i];
// }
// std::cout << "num_data " << std::dec << num_data << " ";
// // std::cout << "w_index " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " ";
// // std::cout << " c_flags = " << std::dec << (int)(as->cs.c_flag);
// //std::cout << " result = ";
// // for(int i = 0; i < 22; i++)
// // {
// // std::cout << std::hex << (int)as->cs.result[i] << " ";
// // }
// std::cout << std:: endl;
// }
// }
// else
// {
// std::cout << " buffer = ";
// for(int i = as->cs.r_index; i < as->cs.r_index + 22; i++)
// {
// std::cout << std::hex << (int)as->cs._buffer[i] << " ";
// }
// std::cout << std:: endl;
// as->cs.r_index ++;
// }
// if(as->cs.c_flag > 1)
// {
// ROS_WARN("ERROR DATA");
// exit(-1);
// }
// }
// std::cout << "w_index = " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " ";
// std::cout << " c_flags = " << std::dec << (int)(as->cs.c_flag);
// // std::cout << " buffer = ";
// // for(int i = 0; i < 300; i++)
// // {
// // std::cout << std::hex << (int)as->cs._buffer[i] << " ";
// // }
// std::cout << std:: endl;
// loop_rate.sleep();
// ros::spinOnce();
// }
// }

146
Devices/Libraries/Ros/md_controller/src/md_controller_node.cpp Executable file
View File

@@ -0,0 +1,146 @@
#include <ros/ros.h>
#include <XmlRpcValue.h>
#include <boost/thread/thread.hpp>
#include "md_controller/md_controller.h"
#include "md_controller/diagnostic.h"
#include "md_controller/md_controller_subscriber.h"
#include "libserial/udevadm_info.h"
#include <XmlRpcException.h>
/* define struct */
typedef struct device{
std::string _port;
int _baud;
}device_t;
bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) {
ROS_INFO("loadParam() - node_name: %s", node_name.c_str());
if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud))
{
if(!nh->getParam("baudrate", dv->_baud))
return false;
}
ROS_INFO("product");
std::string product; //port name
if(!nh->param(node_name + "/product", product, product)) {}
else if(!nh->getParam("product", product)) {}
else
{
udevadmInfo *ludev = new udevadmInfo(product.c_str());
if(ludev->init() == EXIT_FAILURE) {}
if(ludev->scanDevices())
{
strcpy(dv->_port.data(), ludev->port);
return true;
}
}
ROS_INFO("port_name");
if(!nh->param(node_name + "/port_name", dv->_port, dv->_port))
{
if(!nh->getParam("port_name", dv->_port))
return false;
}
ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str());
return true;
}
/**
* @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
*/
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
try
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
catch(const XmlRpc::XmlRpcException& e)
{
ROS_WARN_STREAM( "'"<<member << "' "<< e.getMessage() << '\n');
return defaultvalue;
}
}
/**********************************************************************
* MAIN
***********************************************************************/
int main(int argc,char **argv)
{
/* create ros ndoe */
ros::init(argc, argv, "MD");
ros::NodeHandlePtr nh = boost::make_shared<ros::NodeHandle>();
std::string node_name = ros::this_node::getName();
ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str());
/* connect with the server */
std::string diagnostic_topic = "MD_diagnostic";
MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller");
XmlRpc::XmlRpcValue drivers;
if(!nh->getParam("drivers", drivers) || drivers.size() < 1)
{
ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str());
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file");
return 2;
}
std::vector<boost::shared_ptr<MD::Subscriber>> p_md_controller_subscriber;
for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter)
{
std::string topic_name = readMember<std::string>(object_iter->second, "toppic", "");
int subscribe_queue_size = readMember<int>(object_iter->second, "subscribe_queue_size", 1);
ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size);
p_md_controller_subscriber.push_back(boost::make_shared<MD::Subscriber>(*nh, object_iter));
}
device_t dv;
if(findDevice(nh, node_name, &dv))
{
ROS_INFO("%s: found device successfull", node_name.c_str());
}
else
{
ROS_ERROR("%s: Error when find device", node_name.c_str());
return 2;
}
uint8_t rate = 20; // Hz
ros::Rate loop_rate(rate);
while (ros::ok())
{
ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud);
boost::shared_ptr<MD::MdController> md_controller = boost::make_shared<MD::MdController>(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1);
while(ros::ok() && md_controller->_connected)
{
for(auto &p_sub : p_md_controller_subscriber)
{
if(p_sub->isWheelTriggered())
{
double rotate_speed_wheel;
{
boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
rotate_speed_wheel = p_sub->speed;
}
if(abs(rotate_speed_wheel) > 0)
md_controller->pid_vel_cmd(p_sub->slave_id, rotate_speed_wheel);
else
md_controller->pid_break(p_sub->slave_id);
p_sub->scheduleWheelController(false);
}
}
loop_rate.sleep();
ros::spinOnce();
}
ros::spinOnce();
md_controller->close_port();
}
ros::spin();
return 0;
}

102
Devices/Libraries/Ros/md_controller/src/md_controller_subscriber.cpp Executable file
View File

@@ -0,0 +1,102 @@
#include "md_controller/md_controller_subscriber.h"
#include <XmlRpcException.h>
/**
* @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
*/
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
try
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
catch(const XmlRpc::XmlRpcException& e)
{
return defaultvalue;
}
}
MD::Subscriber::Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size, double schedule_delay, double schedule_delay_lastest)
: nh_(nh), slave_id(0),
topic_name_(ros_topic),
m_subscribe_queue_size_(subscribe_queue_size),
schedule_delay_(ros::Duration(schedule_delay)),
schedule_lastest_delay_(ros::Duration(schedule_delay_lastest)),
publish_time_(ros::Time(0)),
publish_lastest_time_(ros::Time(0))
{
control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
}
MD::Subscriber::Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object)
: nh_(nh), slave_id(0),
publish_time_(ros::Time(0)),
publish_lastest_time_(ros::Time(0))
{
topic_name_ = readMember<std::string>(object->second, "toppic", "");
m_subscribe_queue_size_ = readMember<int>(object->second, "subscribe_queue_size", 1);
slave_id = (uint8_t)readMember<int>(object->second, "id", 1);
ratio = (double)readMember<int>(object->second, "ratio", 0.0);
ratio = readMember<double>(object->second, "ratio", ratio);
double schedule_delay = readMember<double>(object->second, "schedule_delay", 0.05);
double schedule_delay_lastest = readMember<double>(object->second, "schedule_delay_lastest", 0.5);
this->schedule_delay_ = ros::Duration(schedule_delay);
this->schedule_lastest_delay_ = ros::Duration(schedule_delay_lastest);
control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
}
MD::Subscriber::~Subscriber() = default;
void MD::Subscriber::wheelCallback(const std_msgs::Float32::Ptr & msg)
{
boost::lock_guard<boost::mutex> schedule_lockguard(this->m_measurement_mutex);
// check that we don't have multiple publishers on the command topic
if (control_wheel_sub_.getNumPublishers() > 1)
{
ROS_ERROR_STREAM_THROTTLE(1.0, "Detected " << control_wheel_sub_.getNumPublishers()
<< " publishers " << topic_name_ << ". Only 1 publisher is allowed. Going to brake.");
speed = 0;
return;
}
if (!std::isfinite(msg->data))
{
ROS_WARN_THROTTLE(1.0, "Received NaN in %s command. Ignoring.", topic_name_.c_str());
speed = 0;
return;
}
speed = (int16_t)(msg->data * ratio * 60 / 2 / M_PI);
this->scheduleWheelController(true);
}
bool MD::Subscriber::isWheelTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}
bool MD::Subscriber::isWheelLastestTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}
void MD::Subscriber::scheduleWheelController(bool schedule)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
if(schedule && publish_time_.isZero())
{
publish_time_ = ros::Time::now() + schedule_delay_;
publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
}
if(!schedule && !publish_time_.isZero())
{
publish_time_ = ros::Time(0);
publish_lastest_time_ = ros::Time(0);
}
}