commit 8052007699577bbf59ea7830ca2d3fbeae5c7974 Author: M. Stokroos Date: Wed Sep 14 10:58:29 2022 +0200 First commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e595ad2 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,216 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rofish) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + std_msgs + joy +) + +## 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 rofish +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/rofish.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/rofish_node.cpp) +add_executable(rofish_teleop_joy src/rofish_teleop_joy.cpp) +add_executable(red_rofish_telemetry src/red_rofish_telemetry.cpp) +add_executable(yellow_rofish_telemetry src/yellow_rofish_telemetry.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}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + target_link_libraries(rofish_teleop_joy + ${catkin_LIBRARIES} ) + target_link_libraries(red_rofish_telemetry + ${catkin_LIBRARIES} ) + target_link_libraries(yellow_rofish_telemetry + ${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 +# install(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} +# 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 +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_rofish.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) diff --git a/README.md b/README.md new file mode 100644 index 0000000..0ac29fc --- /dev/null +++ b/README.md @@ -0,0 +1,38 @@ +# ROFISH - ROS PACKAGE FOR ROBOTIC FISH +This ROS package controls two robotic fish (red and yellow) from two Logitech wireless Gamepad F710 controllers. The robotic fish are the Bionic Koi types from the Design Lab of the Peking University: [https://en.ibdl.pku.edu.cn/news/highlights/895537.htm](https://en.ibdl.pku.edu.cn/news/highlights/895537.htm) +The right joy-handle controls the speed of the fish by pushing the handle forward. The right joy-handle also steers the fish by pushing the handle to the left or right. + +## Preparations +- The project assumes a correct working ROS Noetic setup and workspace environment +- Connect the two radios for the telemetry to the USB port +- Connect the two dongles of the Gamepad controllers to the USB port +- Turn on the red and the yellow robotic fish + +## Launch the package +- start the roscore by typing 'roscore' in the terminal window. +- Open a new terminal and launch the ROS package by typing: `roslaunch rofish rofish_teleop_joy.launch` + + +## Diagnostics +Start the diagnostics in another terminal: + +``` +$ rostopic list +/diagnostics +/red/cmd_vel +/red/joy +/red/joy/set_feedback +/rosout +/rosout_agg +/yellow/cmd_vel +/yellow/joy +/yellow/joy/set_feedback + +$ rosrun rqt_graph rqt_graph + +``` + +Output from rqtgraph: +![rosgraph](rosgraph.png "rosgraph") + + diff --git a/launch/rofish_teleop_joy.launch b/launch/rofish_teleop_joy.launch new file mode 100644 index 0000000..afdc783 --- /dev/null +++ b/launch/rofish_teleop_joy.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..c8ca55c --- /dev/null +++ b/package.xml @@ -0,0 +1,65 @@ + + + rofish + 0.0.0 + The rofish package + + + + + p229763 + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + diff --git a/rosgraph.png b/rosgraph.png new file mode 100644 index 0000000..d88b642 Binary files /dev/null and b/rosgraph.png differ diff --git a/src/red_rofish_telemetry.cpp b/src/red_rofish_telemetry.cpp new file mode 100644 index 0000000..85aed11 --- /dev/null +++ b/src/red_rofish_telemetry.cpp @@ -0,0 +1,186 @@ +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include +#include + +#define LOOP_RATE 20 //Hz +#define QUEUE_SIZE 64 //subscriber buffer size + + +// -------------- Serial Port ----------------- +int fd; +bool isConnected = false; +const char *port0 = "/dev/ttyACM0"; //red +//const char *port1 = "/dev/ttyACM1"; //yellow + +int connectit(const char *dev, int baud); +static void initTermios(int fd, uint32_t baud); +struct termios sTermios; +int writecommand(char speed, char dir, bool mode); + + +// --------------- Fish ID -------------------- +const unsigned char prefix[3] = { 0x21, 0x43, 0x00}; +const unsigned char fish_ID[9] = { 0x52, 0x71, 0xa7, 0x29, 0xbc, 0xd6, 0xd9, 0x00, 0xe5}; //red +//const unsigned char fish_ID[9] = { 0x52, 0x71, 0x63, 0x4c, 0xb9, 0xd6, 0xda, 0x00, 0xe5}; //yellow + + +// global vars +float lin_vel = 0; +float ang_vel = 0; + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* Twist message Callback +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +void velCallback (const geometry_msgs::Twist::ConstPtr& vel_msg) { + + lin_vel = vel_msg->linear.x; //Get linear velocity from Twist message. + ang_vel = vel_msg->angular.z; //Get the direction from the Twist message. This is not a speed in reality, but steering action. +} + + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* initTermios. Serial I/O +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void initTermios(int fd, uint32_t baud) +{ + tcgetattr(fd, &sTermios); /*Get the attributes set by default*/ + + /* ---------- Serial Port Config ------- */ + sTermios.c_cflag |=(CLOCAL|CREAD); + sTermios.c_cflag &= ~(CSTOPB|CSIZE); + sTermios.c_cflag &= ~(PARENB); + sTermios.c_cflag |= CS8; + sTermios.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG); + sTermios.c_cc[VMIN] = 0; + sTermios.c_cc[VTIME] = (unsigned int)(100); + sTermios.c_cc[VSTART] = 0x13; + sTermios.c_cc[VSTOP] = 0x19; + + sTermios.c_iflag &= ~(IXON|IXOFF|IXANY|ICRNL|INLCR); + sTermios.c_oflag &= ~OPOST; + cfsetispeed(&sTermios, baud); + cfsetospeed(&sTermios, baud); + /* -------- Serial Port Config END---- */ + + tcsetattr(fd, TCSANOW, &sTermios); +} + + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* connectit. Open serial port +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +int connectit(const char *dev, int baud) { + int fd; + fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd == -1) { perror("serconnect "); return fd; } + initTermios(fd, baud); + return fd; +} + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* writecommand +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +int writecommand(char speed, char dir, bool mode) { + + char msg[15]; + if(!isConnected) return -1; + + if(dir > 8) dir = 8; + if(dir < -7) dir = -7; + + if(speed > 15) speed = 15; + if(speed < 0) speed = 0; + + char direction = dir + 7; + char modeH = mode ? 0x10 : 0x00; + char dirMode = direction + modeH; + char checksum = 0xff - speed - direction - modeH; + for(int i=0;i<3;i++){ + msg[i] = prefix[i]; + } + msg[3] = speed; + msg[4] = dirMode; + msg[5] = checksum; + for(int i=0;i<9;i++){ + msg[6+i] = fish_ID[i]; + } + return write(fd, msg, 15); + //return 0; +} + + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* Main +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +int main(int argc, char **argv) +{ + bool mode=false; //purpose? + float x_move=0; + float y_move=2; + int direction=0; + int speed=0; + int prevDirection=0; + int prevSpeed=0; + + fd = connectit(port0,115200); + if (fd != -1) { + isConnected = true; + ROS_INFO("Serial port ttyACM0 connected..."); + sleep(1); + tcflush(fd,TCIOFLUSH); + } + else { + ROS_ERROR("Error!: Serial port ttyACM0 not found!"); + } + + ros::init(argc, argv, "red_rofish_telemetry"); + ros::NodeHandle nh; + ros::Subscriber sub = nh.subscribe("red/cmd_vel", QUEUE_SIZE, velCallback); + ros::Rate loop_rate(LOOP_RATE); + + while(ros::ok()) { + ros::spinOnce(); + + //do processing here + if (ang_vel >= 0) { + direction = (char) 8*ang_vel; + } + if (ang_vel < 0) { + direction = (char) 7*ang_vel; + } + if (lin_vel >= 0){ + speed = (char) 15*lin_vel; + } + + writecommand(speed, direction, mode); + + if( (direction!=prevDirection) || (speed!=prevSpeed) ){ + ROS_INFO("direction=%d, speed=%d\n", direction, speed); + } + prevDirection = direction; + prevSpeed = speed; + + loop_rate.sleep(); + } +return 0; //never reaches here +} diff --git a/src/rofish_teleop_joy.cpp b/src/rofish_teleop_joy.cpp new file mode 100644 index 0000000..4b537f5 --- /dev/null +++ b/src/rofish_teleop_joy.cpp @@ -0,0 +1,45 @@ +#include +#include +#include +#include + +#define QUEUE_SIZE 64 //subscriber buffer size + +using namespace std; +float max_linear_vel = 1.0; +float max_angular_vel = 1.0; + +class TeleopJoy{ +public: + TeleopJoy(); +private: + void callBack(const sensor_msgs::Joy::ConstPtr& joy); + ros::NodeHandle n; + ros::Publisher pub; + ros::Subscriber sub; + int i_velLinear , i_velAngular; +}; + +TeleopJoy::TeleopJoy() +{ i_velLinear = 1; + i_velAngular = 0; + n.param("axis_linear", i_velLinear, i_velLinear); + n.param("axis_angular", i_velAngular, i_velAngular); + pub = n.advertise("cmd_vel", 1); + sub = n.subscribe("joy", QUEUE_SIZE, &TeleopJoy::callBack, this); +} + +void TeleopJoy::callBack(const sensor_msgs::Joy::ConstPtr& joy) +{ + geometry_msgs::Twist vel; + vel.angular.z = -max_angular_vel*joy->axes[2]; // right handle left-to-right + vel.linear.x = max_linear_vel*joy->axes[3]; // right handle up + pub.publish(vel); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "rofish_teleop_joy"); + TeleopJoy teleop_robot; + ros::spin(); +} diff --git a/src/yellow_rofish_telemetry.cpp b/src/yellow_rofish_telemetry.cpp new file mode 100644 index 0000000..4fedbf1 --- /dev/null +++ b/src/yellow_rofish_telemetry.cpp @@ -0,0 +1,186 @@ +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include +#include + +#define LOOP_RATE 20 //Hz +#define QUEUE_SIZE 64 //subscriber buffer size + + +// -------------- Serial Port ----------------- +int fd; +bool isConnected = false; +//const char *port0 = "/dev/ttyACM0"; //red +const char *port1 = "/dev/ttyACM1"; //yellow + +int connectit(const char *dev, int baud); +static void initTermios(int fd, uint32_t baud); +struct termios sTermios; +int writecommand(char speed, char dir, bool mode); + + +// --------------- Fish ID -------------------- +const unsigned char prefix[3] = { 0x21, 0x43, 0x00}; +//const unsigned char fish_ID[9] = { 0x52, 0x71, 0xa7, 0x29, 0xbc, 0xd6, 0xd9, 0x00, 0xe5}; //red +const unsigned char fish_ID[9] = { 0x52, 0x71, 0x63, 0x4c, 0xb9, 0xd6, 0xda, 0x00, 0xe5}; //yellow + + +// global vars +float lin_vel = 0; +float ang_vel = 0; + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* Twist message Callback +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +void velCallback (const geometry_msgs::Twist::ConstPtr& vel_msg) { + + lin_vel = vel_msg->linear.x; //Get linear velocity from Twist message. + ang_vel = vel_msg->angular.z; //Get the direction from the Twist message. This is not a speed in reality, but steering action. +} + + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* initTermios. Serial I/O +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +static void initTermios(int fd, uint32_t baud) +{ + tcgetattr(fd, &sTermios); /*Get the attributes set by default*/ + + /* ---------- Serial Port Config ------- */ + sTermios.c_cflag |=(CLOCAL|CREAD); + sTermios.c_cflag &= ~(CSTOPB|CSIZE); + sTermios.c_cflag &= ~(PARENB); + sTermios.c_cflag |= CS8; + sTermios.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG); + sTermios.c_cc[VMIN] = 0; + sTermios.c_cc[VTIME] = (unsigned int)(100); + sTermios.c_cc[VSTART] = 0x13; + sTermios.c_cc[VSTOP] = 0x19; + + sTermios.c_iflag &= ~(IXON|IXOFF|IXANY|ICRNL|INLCR); + sTermios.c_oflag &= ~OPOST; + cfsetispeed(&sTermios, baud); + cfsetospeed(&sTermios, baud); + /* -------- Serial Port Config END---- */ + + tcsetattr(fd, TCSANOW, &sTermios); +} + + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* connectit. Open serial port +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +int connectit(const char *dev, int baud) { + int fd; + fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd == -1) { perror("serconnect "); return fd; } + initTermios(fd, baud); + return fd; +} + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* writecommand +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +int writecommand(char speed, char dir, bool mode) { + + char msg[15]; + if(!isConnected) return -1; + + if(dir > 8) dir = 8; + if(dir < -7) dir = -7; + + if(speed > 15) speed = 15; + if(speed < 0) speed = 0; + + char direction = dir + 7; + char modeH = mode ? 0x10 : 0x00; + char dirMode = direction + modeH; + char checksum = 0xff - speed - direction - modeH; + for(int i=0;i<3;i++){ + msg[i] = prefix[i]; + } + msg[3] = speed; + msg[4] = dirMode; + msg[5] = checksum; + for(int i=0;i<9;i++){ + msg[6+i] = fish_ID[i]; + } + return write(fd, msg, 15); + //return 0; +} + + + + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * +* +* Main +* +* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +int main(int argc, char **argv) +{ + bool mode=false; //purpose? + float x_move=0; + float y_move=2; + int direction=0; + int speed=0; + int prevDirection=0; + int prevSpeed=0; + + fd = connectit(port1,115200); + if (fd != -1) { + isConnected = true; + ROS_INFO("Serial port ttyACM1 connected..."); + sleep(1); + tcflush(fd,TCIOFLUSH); + } + else { + ROS_ERROR("Error!: Serial port ttyACM1 not found!"); + } + + ros::init(argc, argv, "yellow_rofish_telemetry"); + ros::NodeHandle nh; + ros::Subscriber sub = nh.subscribe("yellow/cmd_vel", QUEUE_SIZE, velCallback); + ros::Rate loop_rate(LOOP_RATE); + + while(ros::ok()) { + ros::spinOnce(); + + //do processing here + if (ang_vel >= 0) { + direction = (char) 8*ang_vel; + } + if (ang_vel < 0) { + direction = (char) 7*ang_vel; + } + if (lin_vel >= 0){ + speed = (char) 15*lin_vel; + } + + writecommand(speed, direction, mode); + + if( (direction!=prevDirection) || (speed!=prevSpeed) ){ + ROS_INFO("direction=%d, speed=%d\n", direction, speed); + } + prevDirection = direction; + prevSpeed = speed; + + loop_rate.sleep(); + } +return 0; //never reaches here +}