diff --git a/.github/workflows/slate-noetic.yaml b/.github/workflows/slate-noetic.yaml new file mode 100644 index 0000000..7df52a2 --- /dev/null +++ b/.github/workflows/slate-noetic.yaml @@ -0,0 +1,34 @@ +name: build-slate-noetic + +on: + push: + branches: + - main + - devel + pull_request: + branches: + - main + - devel + workflow_dispatch: + +defaults: + run: + shell: bash + +jobs: + slate-noetic: + strategy: + matrix: + env: + - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_tools} + - {ROS_DISTRO: noetic, ROS_REPO: main, BUILDER: catkin_make} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Create src directory for slate + run: | + rm interbotix_ros_slate/CATKIN_IGNORE + mkdir src + mv interbotix_ros_slate src + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/interbotix_ros_slate/CATKIN_IGNORE b/interbotix_ros_slate/CATKIN_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt b/interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt new file mode 100644 index 0000000..01df3be --- /dev/null +++ b/interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(interbotix_ros_slate) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/interbotix_ros_slate/interbotix_ros_slate/README.md b/interbotix_ros_slate/interbotix_ros_slate/README.md new file mode 100644 index 0000000..0f97e03 --- /dev/null +++ b/interbotix_ros_slate/interbotix_ros_slate/README.md @@ -0,0 +1,3 @@ +# interbotix_ros_slate + +This metapackage groups together the core ROS Packages for the Interbotix Slate mobile base. diff --git a/interbotix_ros_slate/interbotix_ros_slate/package.xml b/interbotix_ros_slate/interbotix_ros_slate/package.xml new file mode 100644 index 0000000..a86d493 --- /dev/null +++ b/interbotix_ros_slate/interbotix_ros_slate/package.xml @@ -0,0 +1,19 @@ + + + interbotix_ros_slate + 0.0.0 + The interbotix_ros_slate metapackage + Trossen Robotics + BSD-3-Clause + + Trossen Robotics + + catkin + + interbotix_slate_driver + interbotix_slate_msgs + + + + + diff --git a/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt b/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt new file mode 100755 index 0000000..a02b393 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.10.0) +project(interbotix_slate_driver) + +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-Wall -g -std=c++11 -O0 ") + +set(serial_driver "chassis_driver") + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + interbotix_slate_msgs + nav_msgs + roscpp + sensor_msgs + std_srvs + tf +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + geometry_msgs + interbotix_slate_msgs + nav_msgs + roscpp + sensor_msgs + std_srvs + tf +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib) + +add_library(slate_base + src/slate_base.cpp + src/base_driver.cpp +) +add_dependencies(slate_base ${slate_base_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(slate_base + ${serial_driver} + ${catkin_LIBRARIES} +) + +add_executable(slate_base_node + src/slate_base_node.cpp +) +add_dependencies(slate_base_node ${slate_base_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(slate_base_node + slate_base +) + +add_custom_command( + OUTPUT copy_lib + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/lib/* ${CATKIN_DEVEL_PREFIX}/lib +) +add_custom_target(COPY_CHASSIS_LIB ALL DEPENDS copy_lib slate_base_node) + +install( + TARGETS + slate_base_node + RUNTIME DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + FILES + lib/lib${serial_driver}.so + DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} +) diff --git a/interbotix_ros_slate/interbotix_slate_driver/README.md b/interbotix_ros_slate/interbotix_slate_driver/README.md new file mode 100644 index 0000000..b75584f --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/README.md @@ -0,0 +1 @@ +# interbotix_slate_driver diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.h b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.h new file mode 100755 index 0000000..eeb571e --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.h @@ -0,0 +1,44 @@ +#ifndef INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_ +#define INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_ + +#include + +#include "od_index.h" + +namespace base_driver +{ + +bool chassisInit(std::string &dev); +bool chassisControl(float aim_x_vel, float aim_z_omega); +bool getChassisInfo(float &x_vel, float &z_omega); +bool getChassisOdom(float &odom_x, float &odom_y, float &odom_theta); + +bool getBatteryInfo(float &vol, float &cur, int &percent); +bool getChassisState(SystemState &state); +bool getChassisCollision(int &collision); +bool getVersion(char *text); +bool getJoyState(int &state); + +// Limited to 100 characters +bool setText(const char *text); + +// 0 / 1 +bool setCharge(int charge); + +// 0 / 1 +bool setAlarm(int alarm); + +bool motorCtrl(int v); + +bool setIo(int io); + +bool getIo(int &io_state); + +// 0 ~ 100 +bool setLight(int light); + +bool setStateLight(int light); + +} // namespace base_driver + +#endif // INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/od_index.h b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/od_index.h new file mode 100755 index 0000000..4d94deb --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/od_index.h @@ -0,0 +1,58 @@ +#ifndef INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_ +#define INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_ + +#define PORT "chassis" + +typedef enum +{ + SYS_INIT = 0x00, + SYS_NORMAL, + SYS_REMOTE, + SYS_ESTOP, + SYS_CALIB, + SYS_TEST, + SYS_CHARGING, + + SYS_ERR = 0x10, + SYS_ERR_ID, + SYS_ERR_COM, + SYS_ERR_ENC, + SYS_ERR_COLLISION, + SYS_ERR_LOW_VOLTAGE, + SYS_ERR_OVER_VOLTAGE, + SYS_ERR_OVER_CURRENT, + SYS_ERR_OVER_TEMP, + + SYS_STATE_LEN, +} SystemState; + +typedef enum +{ + INDEX_SYS_STATE = 0, + INDEX_SYS_POWER_PERCENTAGE = 1, + + INDEX_CHASSIS_VEL = 2, + INDEX_CHASSIS_POS_OR_OMEGA = 3, + INDEX_CHASSIS_ODOM_X = 4, + INDEX_CHASSIS_ODOM_Y = 5, + INDEX_CHASSIS_ODOM_THETA = 6, + + INDEX_SYS_VOLTAGE = 7, + INDEX_SYS_CURRENT = 8, + + INDEX_AIM_CHASSIS_VEL = 9, + INDEX_AIM_CHASSIS_POS_OR_OMEGA = 10, + + INDEX_SYS_CHARGE = 11, + INDEX_SYS_ALARM = 12, + INDEX_SYS_TEXT = 13, + INDEX_SYS_LIGHT = 14, + INDEX_SYS_COLLISION = 15, + INDEX_SYS_VERSION = 16, + INDEX_STATE_LIGHT = 17, + INDEX_STATE_JOY = 18, + INDEX_STATE_IO = 21, + INDEX_SYS_CMD = 22, +} OdIndex; + +#endif // INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.h b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.h new file mode 100755 index 0000000..7339199 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.h @@ -0,0 +1,31 @@ +#ifndef INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_ +#define INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_ + +#include +#define ASSERT(expr, fail) (static_cast(expr) ? void(0) : (fail)) + +class SerialDriver +{ + public: + bool init(const std::string& portname, std::string& dev, int baudRate); + void close(); + + bool setEntry(int index, const float* data_address); + bool setEntry(int index, const int* data_address); + bool setEntry(int index, const char* data_address); + + bool getEntry(int index, float* data_address); + bool getEntry(int index, int* data_address); + bool getEntry(int index, char* data_address); + + private: + bool handleTimeout(int); + + int fd_; + bool isOpened_; + int conn_timeout_cnt_; + int baud_rate_ = 0; + std::string portname_; +}; + +#endif // INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h new file mode 100644 index 0000000..8c5e57c --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.h @@ -0,0 +1,160 @@ +#ifndef INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ +#define INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ + +#include "geometry_msgs/Twist.h" +#include "interbotix_slate_driver/base_driver.h" +#include "interbotix_slate_driver/serial_driver.h" +#include "interbotix_slate_msgs/SetString.h" +#include "nav_msgs/Odometry.h" +#include "ros/ros.h" +#include "sensor_msgs/BatteryState.h" +#include "std_srvs/SetBool.h" +#include "tf/transform_broadcaster.h" + +namespace slate_base +{ + +#define CMD_TIME_OUT 300 // ms +#define PORT "chassis" + +class SlateBase +{ +public: + + /** + * @brief Constructor for the SlateBase + * @param node_handle ROS NodeHandle + */ + explicit SlateBase(ros::NodeHandle * node_handle); + + /// @brief Destructor for the SlateBase + ~SlateBase() {}; + + /// @brief Process velocity commands and update robot state + void update(); + +private: + // NodeHandle + ros::NodeHandle node; + + // Linear velocity command + float cmd_vel_x_; + + // Angular velocity command + float cmd_vel_z_; + + // Time last velocity command was received + ros::Time cmd_vel_time_last_update_; + + // Odometry publisher + ros::Publisher pub_odom_; + + // BatteryState publisher + ros::Publisher pub_battery_state_; + + // Twist command subscriber + ros::Subscriber sub_cmd_vel_; + + // Screen info service server + ros::ServiceServer srv_info_; + + // Motor status service server + ros::ServiceServer srv_motor_torque_status_; + + // Name of odom frame + std::string odom_frame_name_; + + // Name of base frame + std::string base_frame_name_; + + // Update counter used to only update some values less frequently + int cnt_; + + // Odometry translation in the x-direction in meters + float x_; + + // Odometry translation in the y-direction in meters + float y_; + + // Odometry rotation about the z-axis in radians + float theta_; + + // Odometry forward velocity in meters per second + float x_vel_; + + // Odometry rotational velocity about the z-axis in radians per second + float z_omega_; + + // Whether or not we have received our first odometry update + bool is_first_odom_; + + // Array containing x and y translation in meters and rotation in radians + float pose_[3]; + + // Current of the right motor in Amps + float right_motor_c_; + + // Current of the left motor in Amps + float left_motor_c_; + + // The system state of the base + SystemState chassis_state_; + + // Whether or not to publish base_link->odom TF + bool publish_tf_; + + // Max linear velocity in the x-direction in meters per second + float max_vel_x_ = 1.0; + + // Max angular velocity about the z-axis in radians per second + float max_vel_z_ = 1.0; + + // If publish_tf_ is true, this is the broadcaster used to publish the odom->base_link TF + tf::TransformBroadcaster tf_broadcaster_odom_; + + // Time of the current update + ros::Time current_time_; + + // Time of the last update + ros::Time last_time_; + + // Timeout for base velocity + ros::Duration cmd_vel_timeout_; + + /** + * @brief Process incoming Twist command message + * @param msg Incoming Twist command message + */ + void cmd_vel_callback(const geometry_msgs::Twist & msg); + + /** + * @brief Process incoming screen text service request + * @param req Service request containing desired text + * @param res[out] Service response containing a success indication and a message + * @return True if service request succeeded, false otherwise + */ + bool set_text_callback( + interbotix_slate_msgs::SetString::Request & req, + interbotix_slate_msgs::SetString::Response & res); + + /** + * @brief Process incoming motor torque status service request + * @param req Service request containing desired motor torque status + * @param res[out] Service response containing a success indication and a message + * @return True if service request succeeded, false otherwise + */ + bool motor_torque_status_callback( + std_srvs::SetBool::Request & req, + std_srvs::SetBool::Response & res); + + /** + * @brief Wrap angle + * @param angle Angle to wrap in radians + * @return Wrapped angle + */ + float wrap_angle(float angle); +}; + +} // namespace slate_base + +#endif // INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/lib/libchassis_driver.so b/interbotix_ros_slate/interbotix_slate_driver/lib/libchassis_driver.so new file mode 100755 index 0000000..e87883e Binary files /dev/null and b/interbotix_ros_slate/interbotix_slate_driver/lib/libchassis_driver.so differ diff --git a/interbotix_ros_slate/interbotix_slate_driver/package.xml b/interbotix_ros_slate/interbotix_slate_driver/package.xml new file mode 100755 index 0000000..deaef22 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/package.xml @@ -0,0 +1,39 @@ + + + interbotix_slate_driver + 0.0.0 + The interbotix_slate_driver package + + Trossen Robotics + + BSD-3-Clause + + catkin + + geometry_msgs + interbotix_slate_msgs + nav_msgs + roscpp + sensor_msgs + std_srvs + tf + + geometry_msgs + interbotix_slate_msgs + nav_msgs + roscpp + sensor_msgs + std_srvs + tf + + geometry_msgs + interbotix_slate_msgs + nav_msgs + roscpp + sensor_msgs + std_srvs + tf + + + + diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp new file mode 100755 index 0000000..375a943 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp @@ -0,0 +1,137 @@ +#include "interbotix_slate_driver/base_driver.h" +#include "interbotix_slate_driver/serial_driver.h" + +namespace base_driver +{ + +SerialDriver *serial_driver = nullptr; + +bool chassisInit(std::string &dev) +{ + serial_driver = new SerialDriver(); + return serial_driver->init(PORT, dev, 115200); +} + +void exit() +{ + serial_driver->close(); + ::exit(-1); +} + +bool getBatteryInfo(float &vol, float &cur, int &percent) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_SYS_VOLTAGE, &vol) && + serial_driver->getEntry(INDEX_SYS_CURRENT, &cur) && + serial_driver->getEntry(INDEX_SYS_POWER_PERCENTAGE, &percent); +} + +bool getChassisCollision(int &collision) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_SYS_COLLISION, &collision); +} + +bool getChassisState(SystemState &state) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_SYS_STATE, (int *)&state); +} + +bool getVersion(char *text) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_SYS_VERSION, text); +} + +bool getJoyState(int &state) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_STATE_JOY, (int *)&state); +} + +bool setText(const char *text) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_SYS_TEXT, text); +} + +bool setCharge(int charge) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_SYS_CHARGE, &charge); +} + +bool setAlarm(int alarm) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_SYS_ALARM, &alarm); +} + +bool motorCtrl(int v) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_SYS_CMD, &v); +} + +bool setIo(int io) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_STATE_IO, &io); +} + +bool getIo(int &io_state) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_STATE_IO, &io_state); +} + +bool setLight(int light) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_SYS_LIGHT, &light); +} + +bool setStateLight(int light) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_STATE_LIGHT, &light); +} + +/** + * @brief Set the body velocity + * @param aim_x_vel forward velocity [m/s] + * @param aim_z_omega rotational velocity [rad/s] + */ +bool chassisControl(float aim_x_vel, float aim_z_omega) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->setEntry(INDEX_AIM_CHASSIS_VEL, &aim_x_vel) && + serial_driver->setEntry(INDEX_AIM_CHASSIS_POS_OR_OMEGA, &aim_z_omega); +} +/** + * @brief Get body velocity + * @param x_vel m/s + * @param z_omega rad/s + */ +bool getChassisInfo(float &x_vel, float &z_omega) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_CHASSIS_VEL, &x_vel) && + serial_driver->getEntry(INDEX_CHASSIS_POS_OR_OMEGA, &z_omega); +} +/** + * @brief Get chassis odometry + * @param odom_x odom along the x-axis + * @param odom_y odom along the y-axis + * @param odom_theta odom around the z-axis + */ +bool getChassisOdom(float &odom_x, float &odom_y, float &odom_theta) +{ + ASSERT(serial_driver != nullptr, exit()); + return serial_driver->getEntry(INDEX_CHASSIS_ODOM_X, &odom_x) && + serial_driver->getEntry(INDEX_CHASSIS_ODOM_Y, &odom_y) && + serial_driver->getEntry(INDEX_CHASSIS_ODOM_THETA, &odom_theta); +} + +} // namespace base_driver diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp new file mode 100644 index 0000000..e1e82af --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp @@ -0,0 +1,181 @@ +#include "interbotix_slate_driver/slate_base.h" + +namespace slate_base +{ + +SlateBase::SlateBase(ros::NodeHandle * node_handle) + : node(*node_handle), cmd_vel_x_(0.0), cmd_vel_z_(0.0), cnt_(0), x_(0.0), y_(0.0), theta_(0.0), + x_vel_(0.0), z_omega_(0.0), is_first_odom_(true), pose_{0}, right_motor_c_(0.0), + left_motor_c_(0.0), chassis_state_(SystemState::SYS_INIT), publish_tf_(false), max_vel_x_(1.0), + max_vel_z_(1.0), current_time_(ros::Time::now()), last_time_(ros::Time::now()), + cmd_vel_timeout_(ros::Duration(0, CMD_TIME_OUT * 1e6)) +{ + node.param("publish_tf", publish_tf_, false); + node.param("odom_frame_name", odom_frame_name_, "odom"); + node.param("base_frame_name", base_frame_name_, "base_link"); + + pub_odom_ = node.advertise("odom", 1); + pub_battery_state_ = node.advertise("battery_state", 1); + + sub_cmd_vel_ = node.subscribe("cmd_vel", 1, &SlateBase::cmd_vel_callback, this); + + srv_info_ = node.advertiseService( + "set_text", + &SlateBase::set_text_callback, + this); + srv_motor_torque_status_ = node.advertiseService( + "set_motor_torque_status", + &SlateBase::motor_torque_status_callback, + this); + + std::string dev; + if (!base_driver::chassisInit(dev)) { + ROS_FATAL("Failed to initialize base."); + ::exit(EXIT_FAILURE); + } + ROS_INFO("Initalized base at port '%s'.", dev.c_str()); + char version[32] = {0}; + if (base_driver::getVersion(version)) { + ROS_INFO("Base version: %s", version); + } +} + +void SlateBase::update() +{ + ros::spinOnce(); + current_time_ = ros::Time::now(); + + cnt_++; + sensor_msgs::BatteryState state; + if (cnt_ % 10 == 0) { + state.header.stamp = current_time_; + int percentage = 0; + if ( + base_driver::getBatteryInfo(state.voltage, state.current, percentage) && + base_driver::getChassisState(chassis_state_)) + { + if (state.current < 0) { + int c = -state.current; + int right_c = (c % 1000); + int left_c = (c - right_c) / 1000; + right_motor_c_ = right_c * 0.1f; + left_motor_c_ = left_c * 0.1f; + state.current = -(right_motor_c_ + left_motor_c_); + } + state.percentage = percentage; + state.power_supply_status = chassis_state_; + pub_battery_state_.publish(state); + } + } + + // time out velocity commands + if (current_time_ - cmd_vel_time_last_update_ > cmd_vel_timeout_) { + cmd_vel_x_ = 0.0f; + cmd_vel_z_ = 0.0f; + } + + cmd_vel_x_ = std::min(max_vel_x_, std::max(-max_vel_x_, cmd_vel_x_)); + cmd_vel_z_ = std::min(max_vel_z_, std::max(-max_vel_z_, cmd_vel_z_)); + + base_driver::getChassisInfo(x_vel_, z_omega_); + base_driver::getChassisOdom(x_, y_, theta_); + base_driver::chassisControl(cmd_vel_x_, cmd_vel_z_); + + if (is_first_odom_) { + pose_[0] = x_; + pose_[1] = y_; + pose_[2] = theta_; + is_first_odom_ = false; + } + + // create transform + geometry_msgs::Quaternion odom_quat = + tf::createQuaternionMsgFromYaw(wrap_angle(theta_ - pose_[2])); + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = current_time_; + odom_trans.header.frame_id = odom_frame_name_; + odom_trans.child_frame_id = base_frame_name_; + + odom_trans.transform.translation.x = + cos(-pose_[2]) * (x_ - pose_[0]) - sin(-pose_[2]) * (y_ - pose_[1]); + odom_trans.transform.translation.y = + sin(-pose_[2]) * (x_ - pose_[0]) + cos(-pose_[2]) * (y_ - pose_[1]); + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = odom_quat; + + // send the transform + if (publish_tf_) { + tf_broadcaster_odom_.sendTransform(odom_trans); + } + // next, we'll publish the odometry message over ROS + nav_msgs::Odometry odom; + odom.header.stamp = current_time_; + odom.header.frame_id = odom_frame_name_; + + // set position + odom.pose.pose.position.x = odom_trans.transform.translation.x; + odom.pose.pose.position.y = odom_trans.transform.translation.y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + odom.pose.covariance[0] = (chassis_state_ == SystemState::SYS_ESTOP) ? -1 : 1; + + // set velocity + odom.child_frame_id = base_frame_name_; + odom.twist.twist.linear.x = x_vel_; + odom.twist.twist.linear.y = 0; + odom.twist.twist.angular.z = z_omega_; + + pub_odom_.publish(odom); + last_time_ = current_time_; +} + +void SlateBase::cmd_vel_callback(const geometry_msgs::Twist & msg) +{ + cmd_vel_x_ = msg.linear.x; + cmd_vel_z_ = msg.angular.z; + cmd_vel_time_last_update_ = ros::Time::now(); +} + +bool SlateBase::set_text_callback( + interbotix_slate_msgs::SetString::Request & req, + interbotix_slate_msgs::SetString::Response & res) +{ + res.success = base_driver::setText(req.data.c_str()); + if (res.success) { + res.message = "Successfully set text to: '" + req.data + "'."; + ROS_INFO(res.message.c_str()); + } else { + res.message = "Failed to set text to: '" + req.data + "'."; + ROS_ERROR(res.message.c_str()); + } + return res.success; +} + +bool SlateBase::motor_torque_status_callback( + std_srvs::SetBool::Request & req, + std_srvs::SetBool::Response & res) +{ + res.success = base_driver::motorCtrl(!req.data); + std::string enabled_disabled = req.data ? "enable" : "disable"; + if (res.success) { + res.message = "Successfully " + enabled_disabled + "d motor torque."; + ROS_INFO(res.message.c_str()); + } else { + res.message = "Failed to " + enabled_disabled + " motor torque."; + ROS_ERROR(res.message.c_str()); + } + return res.success; +} + +float SlateBase::wrap_angle(float angle) +{ + if (angle > M_PI) { + angle = angle - 2.0 * M_PI; + } else if (angle < -M_PI) { + angle = angle + 2.0 * M_PI; + } + return angle; +} + +} // namespace tile_base diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp new file mode 100644 index 0000000..807afc6 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp @@ -0,0 +1,15 @@ +#include "interbotix_slate_driver/slate_base.h" + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "slate_base"); + ros::NodeHandle node = ros::NodeHandle(); + auto driver = slate_base::SlateBase(&node); + + auto r = ros::Rate(20); + while (ros::ok()) { + ros::spinOnce(); + driver.update(); + r.sleep(); + } +} diff --git a/interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt b/interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt new file mode 100755 index 0000000..eefdab3 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.10.0) +project(interbotix_slate_msgs) + +find_package(catkin REQUIRED COMPONENTS + message_generation + std_msgs +) + +add_service_files( + FILES + SetString.srv +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + message_runtime + std_msgs +) diff --git a/interbotix_ros_slate/interbotix_slate_msgs/package.xml b/interbotix_ros_slate/interbotix_slate_msgs/package.xml new file mode 100755 index 0000000..8dbdf62 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/package.xml @@ -0,0 +1,23 @@ + + + interbotix_slate_msgs + 0.0.0 + The interbotix_slate_msgs package + + Trossen Robotics + + BSD-3-Clause + + catkin + + message_generation + std_msgs + + std_msgs + + message_runtime + std_msgs + + + + diff --git a/interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv b/interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv new file mode 100644 index 0000000..b0de4d8 --- /dev/null +++ b/interbotix_ros_slate/interbotix_slate_msgs/srv/SetString.srv @@ -0,0 +1,4 @@ +string data +--- +bool success +string message diff --git a/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt b/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt index ae9b266..6144d2b 100644 --- a/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt +++ b/interbotix_ros_xseries/interbotix_xs_msgs/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(interbotix_xs_msgs) -find_package(catkin REQUIRED COMPONENTS +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs trajectory_msgs @@ -43,8 +43,8 @@ generate_messages( ) catkin_package( - CATKIN_DEPENDS - message_runtime - std_msgs + CATKIN_DEPENDS + message_runtime + std_msgs trajectory_msgs )