diff --git a/examples/kobuki/kobuki_main.cxx b/examples/kobuki/kobuki_main.cxx index deed39d81..76e8d0126 100644 --- a/examples/kobuki/kobuki_main.cxx +++ b/examples/kobuki/kobuki_main.cxx @@ -33,8 +33,11 @@ #include #include +#include #include +#include +#include #include #include @@ -97,37 +100,55 @@ display_mallinfo(void) KobukiRobot *r; void commandVelCallback(const void * msgin) { - const geometry_msgs__msg__Twist * twist = (const geometry_msgs__msg__Twist *)msgin; + const drive_base_msgs__msg__TRVCommand * cmd = (const drive_base_msgs__msg__TRVCommand *)msgin; + if(cmd == nullptr) + return; + numberMsgCmdVel++; - if ( twist != NULL ) { - ROS_DEBUG("Received speed cmd %f/%f\n", (float)twist->linear.x, (float)twist->angular.z); - r->setSpeed((float)twist->linear.x, (float)twist->angular.z); - r->sendControls(); - } else { - ROS_ERROR("Error in callback commandVelCallback Twist message expected but got %p!\n", msgin); + // check message age + rcutils_time_point_value_t msgtime = RCUTILS_S_TO_NS(cmd->header.stamp.sec) + cmd->header.stamp.nanosec; + rcutils_time_point_value_t curtime; + rcutils_system_time_now(&curtime); + rcutils_duration_value_t duration_ns = curtime - msgtime; + long duration_ms = RCUTILS_NS_TO_MS(duration_ns); + //fprintf(stdout, "Cur time ns %llu, msg time ns %llu, difference %lld / %ld\n", + // curtime, msgtime, duration_ns, duration_ms); + + const unsigned int period = cmd->header.expected_period != 0 ? cmd->header.expected_period : 100; + if(labs(duration_ms) > period/2) { + fprintf(stderr, "WARN: Ignoring outdated message (age %ld ms > %d ms [period/2])\n", duration_ms, period/2); + return; } + + ROS_DEBUG("Received speed cmd %f/%f\n", cmd->translational_velocity, cmd->rotational_velocity); + r->setSpeed(cmd->translational_velocity, cmd->rotational_velocity); + r->sendControls(); } void* kobuki_run(void *np) { KobukiRobot robot; r = &robot; KobukiNode *node = (KobukiNode*)np; - robot.connect("/dev/ttyS1"); - - struct pollfd pf = { .fd = robot._serial_fd, .events = POLLIN, .revents = 0 }; - int32_t packetCount = 0, count = 0; - while(true) { - struct timespec ts; - poll(&pf, 1, 0); - robot.receiveData(ts); - if(packetCount != robot.packetCount()) { - packetCount = robot.packetCount(); - ++count; - if((count % 5) == 0) { - node->update_state(ts, robot); - } - } + try { + robot.connect("/dev/ttyS1"); + + struct pollfd pf = robot.getPollFD(); + int32_t packetCount = 0, count = 0; + while(true) { + struct timespec ts; + poll(&pf, 1, 0); + robot.receiveData(ts); + if(packetCount != robot.packetCount()) { + packetCount = robot.packetCount(); + ++count; + if((count % 5) == 0) { + node->update_state(ts, robot); + } + } + } + } catch(const std::exception& ex) { + fprintf(stderr, "Error on reading from robot %s\n", ex.what()); } } @@ -156,22 +177,8 @@ int kobuki_main(int argc, char* argv[]) // name must match '$APPNAME_main' in Ma fprintf(stderr, "Failed to create kobuki thread: %d.\n", result); } - //create publisher - const char* pose_topic = "robot_pose"; - - rcl_publisher_t pub_odom = rcl_get_zero_initialized_publisher(); - const rosidl_message_type_support_t * pub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3); - rcl_publisher_options_t pub_opt = rcl_publisher_get_default_options(); - - CHECK_RET(rcl_publisher_init( - &pub_odom, - &(node.node), - pub_type_support, - pose_topic, - &pub_opt)) - //create subscription - const char * cmd_vel_topic_name = "cmd_vel"; + const char * cmd_vel_topic_name = "drive_cmd"; rcl_subscription_t sub_cmd_vel = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); @@ -182,7 +189,7 @@ int kobuki_main(int argc, char* argv[]) // name must match '$APPNAME_main' in Ma RMW_QOS_POLICY_DURABILITY_VOLATILE, false }; - const rosidl_message_type_support_t * sub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist); + const rosidl_message_type_support_t * sub_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(drive_base_msgs, msg, TRVCommand); CHECK_RET(rcl_subscription_init( &sub_cmd_vel, @@ -191,49 +198,29 @@ int kobuki_main(int argc, char* argv[]) // name must match '$APPNAME_main' in Ma cmd_vel_topic_name, &subscription_ops)) - // get empty wait set - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - CHECK_RET(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &(node.context), rcl_get_default_allocator())) - while(true) { - node.publish_status_info(); - // set rmw fields to NULL - CHECK_RET(rcl_wait_set_clear(&wait_set)); - - size_t index = 0; // is never used - denotes the index of the subscription in the storage container - CHECK_RET(rcl_wait_set_add_subscription(&wait_set, &sub_cmd_vel, &index)); - - rc = rcl_wait(&wait_set, RCL_MS_TO_NS(timeout_ms)); - if (rc == RCL_RET_TIMEOUT) { - continue; - } - - if (rc != RCL_RET_OK && rc != RCL_RET_TIMEOUT) { - PRINT_RCL_ERROR(rcl_wait); - continue; - } - - if (wait_set.subscriptions[0] ){ - geometry_msgs__msg__Twist msg; - rmw_message_info_t messageInfo; - rc = rcl_take(&sub_cmd_vel, &msg, &messageInfo, NULL); - if(rc != RCL_RET_OK) { - if(rc != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - fprintf(stderr, "error return on rcl_take: %d\n", rc); - PRINT_RCL_ERROR(rcl_take); - } - continue; - } + // initialize and configure executor + rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + CHECK_RET(rclc_executor_init( + &executor, + &node.context, + 1, + &allocator)) - commandVelCallback( &msg ); - } else { - //sanity check - fprintf(stderr, "[spin_node_once] wait_set returned empty.\n"); - } + // add subscription for topic 'cmd_vel' + drive_base_msgs__msg__TRVCommand msg; + CHECK_RET(rclc_executor_add_subscription( + &executor, + &sub_cmd_vel, + &msg, + commandVelCallback, + ON_NEW_DATA)) - + while (true) { + node.publish_status_info(); + CHECK_RET(rclc_executor_spin_some(&executor,RCL_MS_TO_NS(timeout_ms))) } - WARN_RET(rcl_wait_set_fini(&wait_set)); } catch(const std::exception& ex) { printf("%s\n", ex.what()); } catch(...) { diff --git a/examples/kobuki/kobuki_node.cxx b/examples/kobuki/kobuki_node.cxx index 782085163..cf18d0cb5 100644 --- a/examples/kobuki/kobuki_node.cxx +++ b/examples/kobuki/kobuki_node.cxx @@ -101,7 +101,7 @@ namespace kobuki void KobukiNode::update_state(const struct timespec &ts, const KobukiRobot& robot) { pthread_mutex_lock(&update_mutex); - msg_base_info.hw_timestamp = robot._timestamp; + msg_base_info.hw_timestamp = robot.getHWTime(); msg_base_info.stamp.sec = ts.tv_sec; msg_base_info.stamp.nanosec = ts.tv_nsec; @@ -112,10 +112,11 @@ namespace kobuki msg_base_info.power_supply = drive_base_msgs__msg__BaseInfo__POWER_SUPPLY_STATUS_CHARGING ? robot.charger() : drive_base_msgs__msg__BaseInfo__POWER_SUPPLY_STATUS_DISCHARGING; // diagnostics info - msg_base_info.overcurrent = robot._overcurrent_flags; + msg_base_info.overcurrent = robot.overcurrentAny(); msg_base_info.blocked = false; // TODO: read out from laser - msg_base_info.in_collision = robot._bumper; - msg_base_info.at_cliff = robot._cliff; + msg_base_info.in_collision = robot.inCollision(); + msg_base_info.at_cliff = robot.atCliff(); + msg_base_info.safety_state = robot.getSafetyState(); dirty = true; pthread_mutex_unlock(&update_mutex); @@ -123,16 +124,23 @@ namespace kobuki void KobukiNode::publish_status_info() { + drive_base_msgs__msg__BaseInfo msg_copy; pthread_mutex_lock(&update_mutex); if(!dirty) { pthread_mutex_unlock(&update_mutex); return; } - rcl_ret_t rc = rcl_publish(&pub_base_info, &msg_base_info, NULL); + msg_copy = msg_base_info; + pthread_mutex_unlock(&update_mutex); + + rcl_ret_t rc = rcl_publish(&pub_base_info, &msg_copy, NULL); if(rc != RCL_RET_OK) { + ++sequential_communication_errors_; + ++total_communication_errors_; fprintf(stderr, "Error publishing BaseInfo: %s\n", rcutils_get_error_string().str); + } else { + sequential_communication_errors_ = 0; } dirty = false; - pthread_mutex_unlock(&update_mutex); } } \ No newline at end of file diff --git a/examples/kobuki/kobuki_node.h b/examples/kobuki/kobuki_node.h index 3eea9d74c..f99338e3f 100644 --- a/examples/kobuki/kobuki_node.h +++ b/examples/kobuki/kobuki_node.h @@ -62,6 +62,9 @@ namespace kobuki { bool dirty; pthread_mutex_t update_mutex; + + unsigned int sequential_communication_errors_ { 0 }; + unsigned long total_communication_errors_ { 0 }; }; } diff --git a/examples/kobuki/kobuki_robot.cxx b/examples/kobuki/kobuki_robot.cxx index 5256ddbd4..4474a03ef 100644 --- a/examples/kobuki/kobuki_robot.cxx +++ b/examples/kobuki/kobuki_robot.cxx @@ -94,7 +94,71 @@ void KobukiRobot::playSound(uint8_t duration, uint16_t note) { _control_packet->_payloads.push_back(sp); } +void KobukiRobot::applySafetyConstraints(float& tv, float &rv) const +{ + if(!_safety_enabled) + return; + + if(!(_safety_state & OPERATIONAL)) { + tv = 0; + rv = 0; + return; + } + if((_safety_state & LOW_SPEED)) { + tv = copysign(std::max(LOW_SPEED_TV, (float)fabs(tv)), tv); + rv = copysign(std::max(LOW_SPEED_RV, (float)fabs(rv)), rv); + } else { + tv = copysign(std::min(MAX_SPEED_TV, (float)fabs(tv)), tv); + } + if((_safety_state & NO_BACKWARD) && tv < 0) { + tv = 0; + } else if((_safety_state & NO_FORWARD) && tv > 0) { + tv = 0; + } + if((_safety_state & NO_ROTATE)) { + rv = 0; + } +} + +namespace { + void update_bits(uint16_t& state, int bits, bool condition) { + if(condition) { + state |= bits; + } else { + state &= ~bits; + } + } +} + +void KobukiRobot::updateSafetyState() { + const int old_state = _safety_state; + + // we can operate as long as no wheel drop is detected + update_bits(_safety_state, OPERATIONAL, _wheel_drop == 0); + // if the cliff sensors are triggered, we can only drive backwards + // and if the front bumper is pressed, we can only rotate or drive back + if(atCliff()) { + update_bits(_safety_state, NO_FORWARD | NO_ROTATE, atCliff()); + } else if(inCollision()) { + update_bits(_safety_state, NO_FORWARD, inCollision()); + } else { + update_bits(_safety_state, NO_FORWARD | NO_ROTATE, false); + } + + // if safety state has changed, update speed accordingly + if(_safety_enabled && (old_state != _safety_state)) { + setSpeed(cmd_tv, cmd_rv); + sendControls(); + } +} + void KobukiRobot::setSpeed(float tv, float rv) { + // store commanded velocities + cmd_tv = tv; + cmd_rv = rv; + + applySafetyConstraints(tv, rv); + BaseControlPayload* bp = new BaseControlPayload; // convert to mm; tv *=1000; @@ -123,8 +187,6 @@ void KobukiRobot::setSpeed(float tv, float rv) { _control_packet->_payloads.size()); } -//TODO time -//void KobukiRobot::receiveData(ros::Time& timestamp) { void KobukiRobot::receiveData(struct timespec& timestamp) { if (_serial_fd < 0) throw std::runtime_error("robot not connected"); @@ -134,6 +196,7 @@ void KobukiRobot::receiveData(struct timespec& timestamp) { if(n > 0) { clock_gettime(CLOCK_REALTIME, ×tamp); } + for (int i = 0; i #include #include +#include #include "kobuki_protocol.h" +static const float LOW_SPEED_TV = 0.1f; // -> 0.1m/s +static const float MAX_SPEED_TV = 0.5f; +static const float LOW_SPEED_RV = 0.78f; // ~45°/s class Packet; class PacketSyncFinder; @@ -87,8 +91,20 @@ class KobukiRobot { float voltage() const; float battery() const; //percentage bool overcurrent(Side s) const ; + bool overcurrentAny() const { + return _overcurrent_flags != 0; + } + bool inCollision() const { + return _bumper != 0; + } float current(Side s) const ; //ampere int cliffData(Side s) const ; + bool atCliff() const { + return _cliff != 0; + } + uint16_t getHWTime() const { + return _timestamp; + } /** Flag will be setted when signal is detected @@ -109,7 +125,43 @@ class KobukiRobot { inline float gyroRate(); */ -public: + struct pollfd getPollFD() { + struct pollfd pf = { .fd = _serial_fd, .events = POLLIN, .revents = 0 }; + return pf; + } + + // return current state as bitfield according to drive_base_msgs::BaseInfo constants + int getSafetyState() const { + return _safety_state; + } + +private: + /*** + * These are bits that can be OR'red together + * OPERATIONAL -> operations generally allowed, possibly restricted + * LOW_SPEED -> max .1m/s + * NO_TRANSLATE -> no translation allowed (only rotation) + * NO_FORWARD -> forward motion is prohibited + * NO_ROTATE -> rotation is prohibited + */ + enum SafetyState { + OPERATIONAL = 1, + LOW_SPEED = 2, + NO_FORWARD = 4, + NO_BACKWARD = 8, + NO_ROTATE = 16 + }; + /*** Modify speeds according to safey constraints */ + void applySafetyConstraints(float& tv, float &rv) const; + + /*** + * Inspect currently stored sensor data to derive safety state. + * If necessary, this will send an appropriate speed command to the robot. + */ + void updateSafetyState(); + +protected: + uint16_t _safety_state { 0 }; uint16_t _timestamp; uint8_t _bumper; uint8_t _wheel_drop; @@ -130,11 +182,13 @@ class KobukiRobot { uint16_t _digital_input; uint32_t _udid[3]; uint32_t _P,_I,_D; + float cmd_tv { 0 }, cmd_rv { 0 }; float _x, _y, _theta; float _velocity_x, _velocity_theta; float _initial_heading, _heading; float _baseline, _left_ticks_per_m, _right_ticks_per_m; bool _first_round; + bool _safety_enabled { true }; int _packet_count; void processOdometry(uint16_t left_encoder_, uint16_t right_encoder_,