diff --git a/src/driver/communication_interface.cc b/src/driver/communication_interface.cc index 8c84888..5a8bcc9 100644 --- a/src/driver/communication_interface.cc +++ b/src/driver/communication_interface.cc @@ -50,6 +50,10 @@ #include // for steady_clock, for duration #include // for move +#include + +using json = nlohmann::json; + #include "robot_msgs/bool_t.hpp" #include "robot_msgs/pause_cmd.hpp" // for pause_cmd #include "robot_msgs/trigger_t.hpp" // for trigger_t @@ -62,6 +66,59 @@ using franka_driver::CommunicationInterface; using franka_driver::PlanTimepoints; using utils::PauseCommandType; +void trim(std::string& _string, const char* end_chars) { + _string.erase(0, _string.find_first_not_of(end_chars)); + _string.erase(_string.find_last_not_of(end_chars) + 1); +} + +const Eigen::IOFormat VectorXdFmt(6, 0, ", ", "\n", "[", "]"); + +std::string to_string(Eigen::VectorXd const& ev) { + return fmt::format("{}", ev.transpose().format(VectorXdFmt)); +} + +int64_t get_current_utime() { + struct timeval tv; + gettimeofday(&tv, NULL); + int64_t current_utime = int64_t(tv.tv_sec * 1e6 + tv.tv_usec); + return current_utime; +} + +std::vector split_string(const std::string_view str) { + std::vector str_vec; + std::string token; + std::stringstream ss {str.data()}; + while (std::getline(ss, token, ',')) { + str_vec.push_back(token); + } + return str_vec; +} + +std::vector string_to_double_vector( + const std::string_view str) { + // works for vector string that looks like: [1, 2, 3, 4] + // whitespace negligable + try { + std::string mutable_input {str}; + std::vector output_vec; + + // trim input string to just numbers with comma delimeter + trim(mutable_input, "' \"\t\n\r\f\v[]"); + + auto string_vector {split_string(mutable_input)}; + + for (const auto& num : string_vector) { + output_vec.push_back(std::stod(num)); + } + + return output_vec; + } catch (std::exception& e) { + dexai::log()->error( + "string_to_double_vector: failed to find vector in string '{}'", str); + return {}; + } +} + CommunicationInterface::CommunicationInterface(const RobotParameters& params, const double lcm_publish_rate, const bool simulated) @@ -75,8 +132,14 @@ CommunicationInterface::CommunicationInterface(const RobotParameters& params, this); lcm_.subscribe(params_.lcm_compliant_push_req_channel, &CommunicationInterface::HandleCompliantPushReq, this); + lcm_.subscribe(params_.lcm_sim_driver_event_trigger_channel, &CommunicationInterface::HandleSimDriverEventTrigger, this); + //lcm_.subscribe("SHOKUNIN_FRANKA_JOINT_CURRENT", &CommunicationInterface::HandleCurrentJoint, this); + + // TODO(@anyone): define this in parameters file + lcm_driver_status_channel_ = params_.robot_name + "_DRIVER_STATUS"; + lcm_compliant_push_req_channel_ = params_.robot_name + "_COMPLIANT_PUSH_REQ"; dexai::log()->info("Plan channel:\t\t\t\t{}", params_.lcm_plan_channel); dexai::log()->info("Stop channel:\t\t\t\t{}", params_.lcm_stop_channel); @@ -270,6 +333,7 @@ void CommunicationInterface::PublishLcmAndPauseStatus() { while (running_) { auto time_start = std::chrono::steady_clock::now(); PublishRobotStatus(); + PublishRobotCommands(); // Sleep dynamically to achieve the desired print rate. auto time_end = std::chrono::steady_clock::now(); @@ -288,6 +352,19 @@ void CommunicationInterface::PublishLcmAndPauseStatus() { } } +void CommunicationInterface::PublishRobotCommands() { + // Try to lock data to avoid read write collisions. + std::unique_lock lock {robot_data_mutex_}; + + robot_msgs::string_t msg; + json json_payload; + json_payload["commanded_positions"] = to_string(robot_data_.robot_plan_next_conf); + msg.data = json_payload.dump(); + msg.utime = get_current_utime(); + + lcm_.publish("SHOKUNIN_FRANKA_JOINT_COMMAND", &msg); +} + void CommunicationInterface::PublishRobotStatus() { // Try to lock data to avoid read write collisions. std::unique_lock lock {robot_data_mutex_}; @@ -598,6 +675,21 @@ void CommunicationInterface::HandlePlan( new_plan_buffer_.utime, ms_accept); } +void CommunicationInterface::HandleCurrentJoint( + const ::lcm::ReceiveBuffer*, const std::string&, + const robot_msgs::string_t* curr_joint_msg) { + try { + json data = json::parse(curr_joint_msg->data); + const std::string curr_joint_string {data["current_joint_positions"]}; + curr_joint_vector_ = string_to_double_vector(curr_joint_string); + } catch (const std::exception& ex) { + dexai::log()->error( + "MCD:HandleCurrentJoint: Caught exception ", + ex.what()); + return; + } +} + void CommunicationInterface::HandlePause( const ::lcm::ReceiveBuffer*, const std::string&, const robot_msgs::pause_cmd* pause_cmd_msg) { diff --git a/src/driver/communication_interface.h b/src/driver/communication_interface.h index cbb9348..82bf220 100644 --- a/src/driver/communication_interface.h +++ b/src/driver/communication_interface.h @@ -63,6 +63,7 @@ #include #include #include +#include #include "franka/robot_state.h" #include "utils/robot_parameters.h" @@ -234,6 +235,10 @@ class CommunicationInterface { return pause_data_.pause_sources; } + std::vector GetCurrentJoints() const { + return curr_joint_vector_; + } + void SetPlanCompletion(const int64_t plan_utime, const bool success = true, const std::string& driver_status_string = ""); @@ -275,6 +280,7 @@ class CommunicationInterface { /// indicating the contents of the pause message. void PublishLcmAndPauseStatus(); void PublishRobotStatus(); + void PublishRobotCommands(); void PublishTriggerToChannel(const int64_t utime, std::string_view lcm_channel, const bool success = true, @@ -288,6 +294,8 @@ class CommunicationInterface { /// @deprecated void HandleCompliantPushReq(const ::lcm::ReceiveBuffer*, const std::string&, const robot_msgs::bool_t* msg); + void HandleCurrentJoint(const ::lcm::ReceiveBuffer*, const std::string&, + const robot_msgs::string_t* curr_joint_msg); /// Handler for control exception and u-stop triggers for simulated driver so /// we can test full spectrum of driver states. @@ -334,6 +342,8 @@ class CommunicationInterface { std::string cancel_plan_source_; double lcm_publish_rate_; // Hz + + std::vector curr_joint_vector_ {0, 0, 0, 0, 0, 0, 0}; }; } // namespace franka_driver