diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index 58da43ad..f6f6b383 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -17,7 +17,7 @@ if(NOT CMAKE_BUILD_TYPE) endif() ## find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib control_msgs urdf moveit_ros_planning_interface) find_package(Boost REQUIRED COMPONENTS system) file(GLOB_RECURSE generated_files RELATIVE ${PROJECT_SOURCE_DIR} "src/generated/robot/*.cpp" "src/generated/simulation/*.cpp") diff --git a/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h index e5a38be3..0a04e424 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/actuatorconfig_services.h @@ -28,45 +28,45 @@ class ActuatorConfigSimulationServices : public IActuatorConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function GetAxisOffsetsHandler = nullptr; + std::function GetAxisOffsetsHandler = nullptr; virtual bool GetAxisOffsets(kortex_driver::GetAxisOffsets::Request &req, kortex_driver::GetAxisOffsets::Response &res) override; - std::function SetAxisOffsetsHandler = nullptr; + std::function SetAxisOffsetsHandler = nullptr; virtual bool SetAxisOffsets(kortex_driver::SetAxisOffsets::Request &req, kortex_driver::SetAxisOffsets::Response &res) override; - std::function SetTorqueOffsetHandler = nullptr; + std::function SetTorqueOffsetHandler = nullptr; virtual bool SetTorqueOffset(kortex_driver::SetTorqueOffset::Request &req, kortex_driver::SetTorqueOffset::Response &res) override; - std::function ActuatorConfig_GetControlModeHandler = nullptr; + std::function ActuatorConfig_GetControlModeHandler = nullptr; virtual bool ActuatorConfig_GetControlMode(kortex_driver::ActuatorConfig_GetControlMode::Request &req, kortex_driver::ActuatorConfig_GetControlMode::Response &res) override; - std::function SetControlModeHandler = nullptr; + std::function SetControlModeHandler = nullptr; virtual bool SetControlMode(kortex_driver::SetControlMode::Request &req, kortex_driver::SetControlMode::Response &res) override; - std::function GetActivatedControlLoopHandler = nullptr; + std::function GetActivatedControlLoopHandler = nullptr; virtual bool GetActivatedControlLoop(kortex_driver::GetActivatedControlLoop::Request &req, kortex_driver::GetActivatedControlLoop::Response &res) override; - std::function SetActivatedControlLoopHandler = nullptr; + std::function SetActivatedControlLoopHandler = nullptr; virtual bool SetActivatedControlLoop(kortex_driver::SetActivatedControlLoop::Request &req, kortex_driver::SetActivatedControlLoop::Response &res) override; - std::function GetControlLoopParametersHandler = nullptr; + std::function GetControlLoopParametersHandler = nullptr; virtual bool GetControlLoopParameters(kortex_driver::GetControlLoopParameters::Request &req, kortex_driver::GetControlLoopParameters::Response &res) override; - std::function SetControlLoopParametersHandler = nullptr; + std::function SetControlLoopParametersHandler = nullptr; virtual bool SetControlLoopParameters(kortex_driver::SetControlLoopParameters::Request &req, kortex_driver::SetControlLoopParameters::Response &res) override; - std::function SelectCustomDataHandler = nullptr; + std::function SelectCustomDataHandler = nullptr; virtual bool SelectCustomData(kortex_driver::SelectCustomData::Request &req, kortex_driver::SelectCustomData::Response &res) override; - std::function GetSelectedCustomDataHandler = nullptr; + std::function GetSelectedCustomDataHandler = nullptr; virtual bool GetSelectedCustomData(kortex_driver::GetSelectedCustomData::Request &req, kortex_driver::GetSelectedCustomData::Response &res) override; - std::function SetCommandModeHandler = nullptr; + std::function SetCommandModeHandler = nullptr; virtual bool SetCommandMode(kortex_driver::SetCommandMode::Request &req, kortex_driver::SetCommandMode::Response &res) override; - std::function ActuatorConfig_ClearFaultsHandler = nullptr; + std::function ActuatorConfig_ClearFaultsHandler = nullptr; virtual bool ActuatorConfig_ClearFaults(kortex_driver::ActuatorConfig_ClearFaults::Request &req, kortex_driver::ActuatorConfig_ClearFaults::Response &res) override; - std::function SetServoingHandler = nullptr; + std::function SetServoingHandler = nullptr; virtual bool SetServoing(kortex_driver::SetServoing::Request &req, kortex_driver::SetServoing::Response &res) override; - std::function MoveToPositionHandler = nullptr; + std::function MoveToPositionHandler = nullptr; virtual bool MoveToPosition(kortex_driver::MoveToPosition::Request &req, kortex_driver::MoveToPosition::Response &res) override; - std::function GetCommandModeHandler = nullptr; + std::function GetCommandModeHandler = nullptr; virtual bool GetCommandMode(kortex_driver::GetCommandMode::Request &req, kortex_driver::GetCommandMode::Response &res) override; - std::function GetServoingHandler = nullptr; + std::function GetServoingHandler = nullptr; virtual bool GetServoing(kortex_driver::GetServoing::Request &req, kortex_driver::GetServoing::Response &res) override; - std::function GetTorqueOffsetHandler = nullptr; + std::function GetTorqueOffsetHandler = nullptr; virtual bool GetTorqueOffset(kortex_driver::GetTorqueOffset::Request &req, kortex_driver::GetTorqueOffset::Response &res) override; - std::function SetCoggingFeedforwardModeHandler = nullptr; + std::function SetCoggingFeedforwardModeHandler = nullptr; virtual bool SetCoggingFeedforwardMode(kortex_driver::SetCoggingFeedforwardMode::Request &req, kortex_driver::SetCoggingFeedforwardMode::Response &res) override; - std::function GetCoggingFeedforwardModeHandler = nullptr; + std::function GetCoggingFeedforwardModeHandler = nullptr; virtual bool GetCoggingFeedforwardMode(kortex_driver::GetCoggingFeedforwardMode::Request &req, kortex_driver::GetCoggingFeedforwardMode::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h index d38aa0b2..65566751 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/base_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/base_services.h @@ -28,307 +28,307 @@ class BaseSimulationServices : public IBaseServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function CreateUserProfileHandler = nullptr; + std::function CreateUserProfileHandler = nullptr; virtual bool CreateUserProfile(kortex_driver::CreateUserProfile::Request &req, kortex_driver::CreateUserProfile::Response &res) override; - std::function UpdateUserProfileHandler = nullptr; + std::function UpdateUserProfileHandler = nullptr; virtual bool UpdateUserProfile(kortex_driver::UpdateUserProfile::Request &req, kortex_driver::UpdateUserProfile::Response &res) override; - std::function ReadUserProfileHandler = nullptr; + std::function ReadUserProfileHandler = nullptr; virtual bool ReadUserProfile(kortex_driver::ReadUserProfile::Request &req, kortex_driver::ReadUserProfile::Response &res) override; - std::function DeleteUserProfileHandler = nullptr; + std::function DeleteUserProfileHandler = nullptr; virtual bool DeleteUserProfile(kortex_driver::DeleteUserProfile::Request &req, kortex_driver::DeleteUserProfile::Response &res) override; - std::function ReadAllUserProfilesHandler = nullptr; + std::function ReadAllUserProfilesHandler = nullptr; virtual bool ReadAllUserProfiles(kortex_driver::ReadAllUserProfiles::Request &req, kortex_driver::ReadAllUserProfiles::Response &res) override; - std::function ReadAllUsersHandler = nullptr; + std::function ReadAllUsersHandler = nullptr; virtual bool ReadAllUsers(kortex_driver::ReadAllUsers::Request &req, kortex_driver::ReadAllUsers::Response &res) override; - std::function ChangePasswordHandler = nullptr; + std::function ChangePasswordHandler = nullptr; virtual bool ChangePassword(kortex_driver::ChangePassword::Request &req, kortex_driver::ChangePassword::Response &res) override; - std::function CreateSequenceHandler = nullptr; + std::function CreateSequenceHandler = nullptr; virtual bool CreateSequence(kortex_driver::CreateSequence::Request &req, kortex_driver::CreateSequence::Response &res) override; - std::function UpdateSequenceHandler = nullptr; + std::function UpdateSequenceHandler = nullptr; virtual bool UpdateSequence(kortex_driver::UpdateSequence::Request &req, kortex_driver::UpdateSequence::Response &res) override; - std::function ReadSequenceHandler = nullptr; + std::function ReadSequenceHandler = nullptr; virtual bool ReadSequence(kortex_driver::ReadSequence::Request &req, kortex_driver::ReadSequence::Response &res) override; - std::function DeleteSequenceHandler = nullptr; + std::function DeleteSequenceHandler = nullptr; virtual bool DeleteSequence(kortex_driver::DeleteSequence::Request &req, kortex_driver::DeleteSequence::Response &res) override; - std::function ReadAllSequencesHandler = nullptr; + std::function ReadAllSequencesHandler = nullptr; virtual bool ReadAllSequences(kortex_driver::ReadAllSequences::Request &req, kortex_driver::ReadAllSequences::Response &res) override; - std::function PlaySequenceHandler = nullptr; + std::function PlaySequenceHandler = nullptr; virtual bool PlaySequence(kortex_driver::PlaySequence::Request &req, kortex_driver::PlaySequence::Response &res) override; - std::function PlayAdvancedSequenceHandler = nullptr; + std::function PlayAdvancedSequenceHandler = nullptr; virtual bool PlayAdvancedSequence(kortex_driver::PlayAdvancedSequence::Request &req, kortex_driver::PlayAdvancedSequence::Response &res) override; - std::function StopSequenceHandler = nullptr; + std::function StopSequenceHandler = nullptr; virtual bool StopSequence(kortex_driver::StopSequence::Request &req, kortex_driver::StopSequence::Response &res) override; - std::function PauseSequenceHandler = nullptr; + std::function PauseSequenceHandler = nullptr; virtual bool PauseSequence(kortex_driver::PauseSequence::Request &req, kortex_driver::PauseSequence::Response &res) override; - std::function ResumeSequenceHandler = nullptr; + std::function ResumeSequenceHandler = nullptr; virtual bool ResumeSequence(kortex_driver::ResumeSequence::Request &req, kortex_driver::ResumeSequence::Response &res) override; - std::function CreateProtectionZoneHandler = nullptr; + std::function CreateProtectionZoneHandler = nullptr; virtual bool CreateProtectionZone(kortex_driver::CreateProtectionZone::Request &req, kortex_driver::CreateProtectionZone::Response &res) override; - std::function UpdateProtectionZoneHandler = nullptr; + std::function UpdateProtectionZoneHandler = nullptr; virtual bool UpdateProtectionZone(kortex_driver::UpdateProtectionZone::Request &req, kortex_driver::UpdateProtectionZone::Response &res) override; - std::function ReadProtectionZoneHandler = nullptr; + std::function ReadProtectionZoneHandler = nullptr; virtual bool ReadProtectionZone(kortex_driver::ReadProtectionZone::Request &req, kortex_driver::ReadProtectionZone::Response &res) override; - std::function DeleteProtectionZoneHandler = nullptr; + std::function DeleteProtectionZoneHandler = nullptr; virtual bool DeleteProtectionZone(kortex_driver::DeleteProtectionZone::Request &req, kortex_driver::DeleteProtectionZone::Response &res) override; - std::function ReadAllProtectionZonesHandler = nullptr; + std::function ReadAllProtectionZonesHandler = nullptr; virtual bool ReadAllProtectionZones(kortex_driver::ReadAllProtectionZones::Request &req, kortex_driver::ReadAllProtectionZones::Response &res) override; - std::function CreateMappingHandler = nullptr; + std::function CreateMappingHandler = nullptr; virtual bool CreateMapping(kortex_driver::CreateMapping::Request &req, kortex_driver::CreateMapping::Response &res) override; - std::function ReadMappingHandler = nullptr; + std::function ReadMappingHandler = nullptr; virtual bool ReadMapping(kortex_driver::ReadMapping::Request &req, kortex_driver::ReadMapping::Response &res) override; - std::function UpdateMappingHandler = nullptr; + std::function UpdateMappingHandler = nullptr; virtual bool UpdateMapping(kortex_driver::UpdateMapping::Request &req, kortex_driver::UpdateMapping::Response &res) override; - std::function DeleteMappingHandler = nullptr; + std::function DeleteMappingHandler = nullptr; virtual bool DeleteMapping(kortex_driver::DeleteMapping::Request &req, kortex_driver::DeleteMapping::Response &res) override; - std::function ReadAllMappingsHandler = nullptr; + std::function ReadAllMappingsHandler = nullptr; virtual bool ReadAllMappings(kortex_driver::ReadAllMappings::Request &req, kortex_driver::ReadAllMappings::Response &res) override; - std::function CreateMapHandler = nullptr; + std::function CreateMapHandler = nullptr; virtual bool CreateMap(kortex_driver::CreateMap::Request &req, kortex_driver::CreateMap::Response &res) override; - std::function ReadMapHandler = nullptr; + std::function ReadMapHandler = nullptr; virtual bool ReadMap(kortex_driver::ReadMap::Request &req, kortex_driver::ReadMap::Response &res) override; - std::function UpdateMapHandler = nullptr; + std::function UpdateMapHandler = nullptr; virtual bool UpdateMap(kortex_driver::UpdateMap::Request &req, kortex_driver::UpdateMap::Response &res) override; - std::function DeleteMapHandler = nullptr; + std::function DeleteMapHandler = nullptr; virtual bool DeleteMap(kortex_driver::DeleteMap::Request &req, kortex_driver::DeleteMap::Response &res) override; - std::function ReadAllMapsHandler = nullptr; + std::function ReadAllMapsHandler = nullptr; virtual bool ReadAllMaps(kortex_driver::ReadAllMaps::Request &req, kortex_driver::ReadAllMaps::Response &res) override; - std::function ActivateMapHandler = nullptr; + std::function ActivateMapHandler = nullptr; virtual bool ActivateMap(kortex_driver::ActivateMap::Request &req, kortex_driver::ActivateMap::Response &res) override; - std::function CreateActionHandler = nullptr; + std::function CreateActionHandler = nullptr; virtual bool CreateAction(kortex_driver::CreateAction::Request &req, kortex_driver::CreateAction::Response &res) override; - std::function ReadActionHandler = nullptr; + std::function ReadActionHandler = nullptr; virtual bool ReadAction(kortex_driver::ReadAction::Request &req, kortex_driver::ReadAction::Response &res) override; - std::function ReadAllActionsHandler = nullptr; + std::function ReadAllActionsHandler = nullptr; virtual bool ReadAllActions(kortex_driver::ReadAllActions::Request &req, kortex_driver::ReadAllActions::Response &res) override; - std::function DeleteActionHandler = nullptr; + std::function DeleteActionHandler = nullptr; virtual bool DeleteAction(kortex_driver::DeleteAction::Request &req, kortex_driver::DeleteAction::Response &res) override; - std::function UpdateActionHandler = nullptr; + std::function UpdateActionHandler = nullptr; virtual bool UpdateAction(kortex_driver::UpdateAction::Request &req, kortex_driver::UpdateAction::Response &res) override; - std::function ExecuteActionFromReferenceHandler = nullptr; + std::function ExecuteActionFromReferenceHandler = nullptr; virtual bool ExecuteActionFromReference(kortex_driver::ExecuteActionFromReference::Request &req, kortex_driver::ExecuteActionFromReference::Response &res) override; - std::function ExecuteActionHandler = nullptr; + std::function ExecuteActionHandler = nullptr; virtual bool ExecuteAction(kortex_driver::ExecuteAction::Request &req, kortex_driver::ExecuteAction::Response &res) override; - std::function PauseActionHandler = nullptr; + std::function PauseActionHandler = nullptr; virtual bool PauseAction(kortex_driver::PauseAction::Request &req, kortex_driver::PauseAction::Response &res) override; - std::function StopActionHandler = nullptr; + std::function StopActionHandler = nullptr; virtual bool StopAction(kortex_driver::StopAction::Request &req, kortex_driver::StopAction::Response &res) override; - std::function ResumeActionHandler = nullptr; + std::function ResumeActionHandler = nullptr; virtual bool ResumeAction(kortex_driver::ResumeAction::Request &req, kortex_driver::ResumeAction::Response &res) override; - std::function GetIPv4ConfigurationHandler = nullptr; + std::function GetIPv4ConfigurationHandler = nullptr; virtual bool GetIPv4Configuration(kortex_driver::GetIPv4Configuration::Request &req, kortex_driver::GetIPv4Configuration::Response &res) override; - std::function SetIPv4ConfigurationHandler = nullptr; + std::function SetIPv4ConfigurationHandler = nullptr; virtual bool SetIPv4Configuration(kortex_driver::SetIPv4Configuration::Request &req, kortex_driver::SetIPv4Configuration::Response &res) override; - std::function SetCommunicationInterfaceEnableHandler = nullptr; + std::function SetCommunicationInterfaceEnableHandler = nullptr; virtual bool SetCommunicationInterfaceEnable(kortex_driver::SetCommunicationInterfaceEnable::Request &req, kortex_driver::SetCommunicationInterfaceEnable::Response &res) override; - std::function IsCommunicationInterfaceEnableHandler = nullptr; + std::function IsCommunicationInterfaceEnableHandler = nullptr; virtual bool IsCommunicationInterfaceEnable(kortex_driver::IsCommunicationInterfaceEnable::Request &req, kortex_driver::IsCommunicationInterfaceEnable::Response &res) override; - std::function GetAvailableWifiHandler = nullptr; + std::function GetAvailableWifiHandler = nullptr; virtual bool GetAvailableWifi(kortex_driver::GetAvailableWifi::Request &req, kortex_driver::GetAvailableWifi::Response &res) override; - std::function GetWifiInformationHandler = nullptr; + std::function GetWifiInformationHandler = nullptr; virtual bool GetWifiInformation(kortex_driver::GetWifiInformation::Request &req, kortex_driver::GetWifiInformation::Response &res) override; - std::function AddWifiConfigurationHandler = nullptr; + std::function AddWifiConfigurationHandler = nullptr; virtual bool AddWifiConfiguration(kortex_driver::AddWifiConfiguration::Request &req, kortex_driver::AddWifiConfiguration::Response &res) override; - std::function DeleteWifiConfigurationHandler = nullptr; + std::function DeleteWifiConfigurationHandler = nullptr; virtual bool DeleteWifiConfiguration(kortex_driver::DeleteWifiConfiguration::Request &req, kortex_driver::DeleteWifiConfiguration::Response &res) override; - std::function GetAllConfiguredWifisHandler = nullptr; + std::function GetAllConfiguredWifisHandler = nullptr; virtual bool GetAllConfiguredWifis(kortex_driver::GetAllConfiguredWifis::Request &req, kortex_driver::GetAllConfiguredWifis::Response &res) override; - std::function ConnectWifiHandler = nullptr; + std::function ConnectWifiHandler = nullptr; virtual bool ConnectWifi(kortex_driver::ConnectWifi::Request &req, kortex_driver::ConnectWifi::Response &res) override; - std::function DisconnectWifiHandler = nullptr; + std::function DisconnectWifiHandler = nullptr; virtual bool DisconnectWifi(kortex_driver::DisconnectWifi::Request &req, kortex_driver::DisconnectWifi::Response &res) override; - std::function GetConnectedWifiInformationHandler = nullptr; + std::function GetConnectedWifiInformationHandler = nullptr; virtual bool GetConnectedWifiInformation(kortex_driver::GetConnectedWifiInformation::Request &req, kortex_driver::GetConnectedWifiInformation::Response &res) override; - std::function Base_UnsubscribeHandler = nullptr; + std::function Base_UnsubscribeHandler = nullptr; virtual bool Base_Unsubscribe(kortex_driver::Base_Unsubscribe::Request &req, kortex_driver::Base_Unsubscribe::Response &res) override; - std::function OnNotificationConfigurationChangeTopicHandler = nullptr; + std::function OnNotificationConfigurationChangeTopicHandler = nullptr; virtual bool OnNotificationConfigurationChangeTopic(kortex_driver::OnNotificationConfigurationChangeTopic::Request &req, kortex_driver::OnNotificationConfigurationChangeTopic::Response &res) override; virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) override; - std::function OnNotificationMappingInfoTopicHandler = nullptr; + std::function OnNotificationMappingInfoTopicHandler = nullptr; virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) override; virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) override; - std::function OnNotificationControlModeTopicHandler = nullptr; + std::function OnNotificationControlModeTopicHandler = nullptr; virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) override; virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) override; - std::function OnNotificationOperatingModeTopicHandler = nullptr; + std::function OnNotificationOperatingModeTopicHandler = nullptr; virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) override; virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) override; - std::function OnNotificationSequenceInfoTopicHandler = nullptr; + std::function OnNotificationSequenceInfoTopicHandler = nullptr; virtual bool OnNotificationSequenceInfoTopic(kortex_driver::OnNotificationSequenceInfoTopic::Request &req, kortex_driver::OnNotificationSequenceInfoTopic::Response &res) override; virtual void cb_SequenceInfoTopic(Kinova::Api::Base::SequenceInfoNotification notif) override; - std::function OnNotificationProtectionZoneTopicHandler = nullptr; + std::function OnNotificationProtectionZoneTopicHandler = nullptr; virtual bool OnNotificationProtectionZoneTopic(kortex_driver::OnNotificationProtectionZoneTopic::Request &req, kortex_driver::OnNotificationProtectionZoneTopic::Response &res) override; virtual void cb_ProtectionZoneTopic(Kinova::Api::Base::ProtectionZoneNotification notif) override; - std::function OnNotificationUserTopicHandler = nullptr; + std::function OnNotificationUserTopicHandler = nullptr; virtual bool OnNotificationUserTopic(kortex_driver::OnNotificationUserTopic::Request &req, kortex_driver::OnNotificationUserTopic::Response &res) override; virtual void cb_UserTopic(Kinova::Api::Base::UserNotification notif) override; - std::function OnNotificationControllerTopicHandler = nullptr; + std::function OnNotificationControllerTopicHandler = nullptr; virtual bool OnNotificationControllerTopic(kortex_driver::OnNotificationControllerTopic::Request &req, kortex_driver::OnNotificationControllerTopic::Response &res) override; virtual void cb_ControllerTopic(Kinova::Api::Base::ControllerNotification notif) override; - std::function OnNotificationActionTopicHandler = nullptr; + std::function OnNotificationActionTopicHandler = nullptr; virtual bool OnNotificationActionTopic(kortex_driver::OnNotificationActionTopic::Request &req, kortex_driver::OnNotificationActionTopic::Response &res) override; virtual void cb_ActionTopic(Kinova::Api::Base::ActionNotification notif) override; - std::function OnNotificationRobotEventTopicHandler = nullptr; + std::function OnNotificationRobotEventTopicHandler = nullptr; virtual bool OnNotificationRobotEventTopic(kortex_driver::OnNotificationRobotEventTopic::Request &req, kortex_driver::OnNotificationRobotEventTopic::Response &res) override; virtual void cb_RobotEventTopic(Kinova::Api::Base::RobotEventNotification notif) override; - std::function PlayCartesianTrajectoryHandler = nullptr; + std::function PlayCartesianTrajectoryHandler = nullptr; virtual bool PlayCartesianTrajectory(kortex_driver::PlayCartesianTrajectory::Request &req, kortex_driver::PlayCartesianTrajectory::Response &res) override; - std::function PlayCartesianTrajectoryPositionHandler = nullptr; + std::function PlayCartesianTrajectoryPositionHandler = nullptr; virtual bool PlayCartesianTrajectoryPosition(kortex_driver::PlayCartesianTrajectoryPosition::Request &req, kortex_driver::PlayCartesianTrajectoryPosition::Response &res) override; - std::function PlayCartesianTrajectoryOrientationHandler = nullptr; + std::function PlayCartesianTrajectoryOrientationHandler = nullptr; virtual bool PlayCartesianTrajectoryOrientation(kortex_driver::PlayCartesianTrajectoryOrientation::Request &req, kortex_driver::PlayCartesianTrajectoryOrientation::Response &res) override; - std::function StopHandler = nullptr; + std::function StopHandler = nullptr; virtual bool Stop(kortex_driver::Stop::Request &req, kortex_driver::Stop::Response &res) override; - std::function GetMeasuredCartesianPoseHandler = nullptr; + std::function GetMeasuredCartesianPoseHandler = nullptr; virtual bool GetMeasuredCartesianPose(kortex_driver::GetMeasuredCartesianPose::Request &req, kortex_driver::GetMeasuredCartesianPose::Response &res) override; - std::function SendWrenchCommandHandler = nullptr; + std::function SendWrenchCommandHandler = nullptr; virtual bool SendWrenchCommand(kortex_driver::SendWrenchCommand::Request &req, kortex_driver::SendWrenchCommand::Response &res) override; - std::function SendWrenchJoystickCommandHandler = nullptr; + std::function SendWrenchJoystickCommandHandler = nullptr; virtual bool SendWrenchJoystickCommand(kortex_driver::SendWrenchJoystickCommand::Request &req, kortex_driver::SendWrenchJoystickCommand::Response &res) override; - std::function SendTwistJoystickCommandHandler = nullptr; + std::function SendTwistJoystickCommandHandler = nullptr; virtual bool SendTwistJoystickCommand(kortex_driver::SendTwistJoystickCommand::Request &req, kortex_driver::SendTwistJoystickCommand::Response &res) override; - std::function SendTwistCommandHandler = nullptr; + std::function SendTwistCommandHandler = nullptr; virtual bool SendTwistCommand(kortex_driver::SendTwistCommand::Request &req, kortex_driver::SendTwistCommand::Response &res) override; - std::function PlayJointTrajectoryHandler = nullptr; + std::function PlayJointTrajectoryHandler = nullptr; virtual bool PlayJointTrajectory(kortex_driver::PlayJointTrajectory::Request &req, kortex_driver::PlayJointTrajectory::Response &res) override; - std::function PlaySelectedJointTrajectoryHandler = nullptr; + std::function PlaySelectedJointTrajectoryHandler = nullptr; virtual bool PlaySelectedJointTrajectory(kortex_driver::PlaySelectedJointTrajectory::Request &req, kortex_driver::PlaySelectedJointTrajectory::Response &res) override; - std::function GetMeasuredJointAnglesHandler = nullptr; + std::function GetMeasuredJointAnglesHandler = nullptr; virtual bool GetMeasuredJointAngles(kortex_driver::GetMeasuredJointAngles::Request &req, kortex_driver::GetMeasuredJointAngles::Response &res) override; - std::function SendJointSpeedsCommandHandler = nullptr; + std::function SendJointSpeedsCommandHandler = nullptr; virtual bool SendJointSpeedsCommand(kortex_driver::SendJointSpeedsCommand::Request &req, kortex_driver::SendJointSpeedsCommand::Response &res) override; - std::function SendSelectedJointSpeedCommandHandler = nullptr; + std::function SendSelectedJointSpeedCommandHandler = nullptr; virtual bool SendSelectedJointSpeedCommand(kortex_driver::SendSelectedJointSpeedCommand::Request &req, kortex_driver::SendSelectedJointSpeedCommand::Response &res) override; - std::function SendGripperCommandHandler = nullptr; + std::function SendGripperCommandHandler = nullptr; virtual bool SendGripperCommand(kortex_driver::SendGripperCommand::Request &req, kortex_driver::SendGripperCommand::Response &res) override; - std::function GetMeasuredGripperMovementHandler = nullptr; + std::function GetMeasuredGripperMovementHandler = nullptr; virtual bool GetMeasuredGripperMovement(kortex_driver::GetMeasuredGripperMovement::Request &req, kortex_driver::GetMeasuredGripperMovement::Response &res) override; - std::function SetAdmittanceHandler = nullptr; + std::function SetAdmittanceHandler = nullptr; virtual bool SetAdmittance(kortex_driver::SetAdmittance::Request &req, kortex_driver::SetAdmittance::Response &res) override; - std::function SetOperatingModeHandler = nullptr; + std::function SetOperatingModeHandler = nullptr; virtual bool SetOperatingMode(kortex_driver::SetOperatingMode::Request &req, kortex_driver::SetOperatingMode::Response &res) override; - std::function ApplyEmergencyStopHandler = nullptr; + std::function ApplyEmergencyStopHandler = nullptr; virtual bool ApplyEmergencyStop(kortex_driver::ApplyEmergencyStop::Request &req, kortex_driver::ApplyEmergencyStop::Response &res) override; - std::function Base_ClearFaultsHandler = nullptr; + std::function Base_ClearFaultsHandler = nullptr; virtual bool Base_ClearFaults(kortex_driver::Base_ClearFaults::Request &req, kortex_driver::Base_ClearFaults::Response &res) override; - std::function Base_GetControlModeHandler = nullptr; + std::function Base_GetControlModeHandler = nullptr; virtual bool Base_GetControlMode(kortex_driver::Base_GetControlMode::Request &req, kortex_driver::Base_GetControlMode::Response &res) override; - std::function GetOperatingModeHandler = nullptr; + std::function GetOperatingModeHandler = nullptr; virtual bool GetOperatingMode(kortex_driver::GetOperatingMode::Request &req, kortex_driver::GetOperatingMode::Response &res) override; - std::function SetServoingModeHandler = nullptr; + std::function SetServoingModeHandler = nullptr; virtual bool SetServoingMode(kortex_driver::SetServoingMode::Request &req, kortex_driver::SetServoingMode::Response &res) override; - std::function GetServoingModeHandler = nullptr; + std::function GetServoingModeHandler = nullptr; virtual bool GetServoingMode(kortex_driver::GetServoingMode::Request &req, kortex_driver::GetServoingMode::Response &res) override; - std::function OnNotificationServoingModeTopicHandler = nullptr; + std::function OnNotificationServoingModeTopicHandler = nullptr; virtual bool OnNotificationServoingModeTopic(kortex_driver::OnNotificationServoingModeTopic::Request &req, kortex_driver::OnNotificationServoingModeTopic::Response &res) override; virtual void cb_ServoingModeTopic(Kinova::Api::Base::ServoingModeNotification notif) override; - std::function RestoreFactorySettingsHandler = nullptr; + std::function RestoreFactorySettingsHandler = nullptr; virtual bool RestoreFactorySettings(kortex_driver::RestoreFactorySettings::Request &req, kortex_driver::RestoreFactorySettings::Response &res) override; - std::function OnNotificationFactoryTopicHandler = nullptr; + std::function OnNotificationFactoryTopicHandler = nullptr; virtual bool OnNotificationFactoryTopic(kortex_driver::OnNotificationFactoryTopic::Request &req, kortex_driver::OnNotificationFactoryTopic::Response &res) override; virtual void cb_FactoryTopic(Kinova::Api::Base::FactoryNotification notif) override; - std::function GetAllConnectedControllersHandler = nullptr; + std::function GetAllConnectedControllersHandler = nullptr; virtual bool GetAllConnectedControllers(kortex_driver::GetAllConnectedControllers::Request &req, kortex_driver::GetAllConnectedControllers::Response &res) override; - std::function GetControllerStateHandler = nullptr; + std::function GetControllerStateHandler = nullptr; virtual bool GetControllerState(kortex_driver::GetControllerState::Request &req, kortex_driver::GetControllerState::Response &res) override; - std::function GetActuatorCountHandler = nullptr; + std::function GetActuatorCountHandler = nullptr; virtual bool GetActuatorCount(kortex_driver::GetActuatorCount::Request &req, kortex_driver::GetActuatorCount::Response &res) override; - std::function StartWifiScanHandler = nullptr; + std::function StartWifiScanHandler = nullptr; virtual bool StartWifiScan(kortex_driver::StartWifiScan::Request &req, kortex_driver::StartWifiScan::Response &res) override; - std::function GetConfiguredWifiHandler = nullptr; + std::function GetConfiguredWifiHandler = nullptr; virtual bool GetConfiguredWifi(kortex_driver::GetConfiguredWifi::Request &req, kortex_driver::GetConfiguredWifi::Response &res) override; - std::function OnNotificationNetworkTopicHandler = nullptr; + std::function OnNotificationNetworkTopicHandler = nullptr; virtual bool OnNotificationNetworkTopic(kortex_driver::OnNotificationNetworkTopic::Request &req, kortex_driver::OnNotificationNetworkTopic::Response &res) override; virtual void cb_NetworkTopic(Kinova::Api::Base::NetworkNotification notif) override; - std::function GetArmStateHandler = nullptr; + std::function GetArmStateHandler = nullptr; virtual bool GetArmState(kortex_driver::GetArmState::Request &req, kortex_driver::GetArmState::Response &res) override; - std::function OnNotificationArmStateTopicHandler = nullptr; + std::function OnNotificationArmStateTopicHandler = nullptr; virtual bool OnNotificationArmStateTopic(kortex_driver::OnNotificationArmStateTopic::Request &req, kortex_driver::OnNotificationArmStateTopic::Response &res) override; virtual void cb_ArmStateTopic(Kinova::Api::Base::ArmStateNotification notif) override; - std::function GetIPv4InformationHandler = nullptr; + std::function GetIPv4InformationHandler = nullptr; virtual bool GetIPv4Information(kortex_driver::GetIPv4Information::Request &req, kortex_driver::GetIPv4Information::Response &res) override; - std::function SetWifiCountryCodeHandler = nullptr; + std::function SetWifiCountryCodeHandler = nullptr; virtual bool SetWifiCountryCode(kortex_driver::SetWifiCountryCode::Request &req, kortex_driver::SetWifiCountryCode::Response &res) override; - std::function GetWifiCountryCodeHandler = nullptr; + std::function GetWifiCountryCodeHandler = nullptr; virtual bool GetWifiCountryCode(kortex_driver::GetWifiCountryCode::Request &req, kortex_driver::GetWifiCountryCode::Response &res) override; - std::function Base_SetCapSenseConfigHandler = nullptr; + std::function Base_SetCapSenseConfigHandler = nullptr; virtual bool Base_SetCapSenseConfig(kortex_driver::Base_SetCapSenseConfig::Request &req, kortex_driver::Base_SetCapSenseConfig::Response &res) override; - std::function Base_GetCapSenseConfigHandler = nullptr; + std::function Base_GetCapSenseConfigHandler = nullptr; virtual bool Base_GetCapSenseConfig(kortex_driver::Base_GetCapSenseConfig::Request &req, kortex_driver::Base_GetCapSenseConfig::Response &res) override; - std::function GetAllJointsSpeedHardLimitationHandler = nullptr; + std::function GetAllJointsSpeedHardLimitationHandler = nullptr; virtual bool GetAllJointsSpeedHardLimitation(kortex_driver::GetAllJointsSpeedHardLimitation::Request &req, kortex_driver::GetAllJointsSpeedHardLimitation::Response &res) override; - std::function GetAllJointsTorqueHardLimitationHandler = nullptr; + std::function GetAllJointsTorqueHardLimitationHandler = nullptr; virtual bool GetAllJointsTorqueHardLimitation(kortex_driver::GetAllJointsTorqueHardLimitation::Request &req, kortex_driver::GetAllJointsTorqueHardLimitation::Response &res) override; - std::function GetTwistHardLimitationHandler = nullptr; + std::function GetTwistHardLimitationHandler = nullptr; virtual bool GetTwistHardLimitation(kortex_driver::GetTwistHardLimitation::Request &req, kortex_driver::GetTwistHardLimitation::Response &res) override; - std::function GetWrenchHardLimitationHandler = nullptr; + std::function GetWrenchHardLimitationHandler = nullptr; virtual bool GetWrenchHardLimitation(kortex_driver::GetWrenchHardLimitation::Request &req, kortex_driver::GetWrenchHardLimitation::Response &res) override; - std::function SendJointSpeedsJoystickCommandHandler = nullptr; + std::function SendJointSpeedsJoystickCommandHandler = nullptr; virtual bool SendJointSpeedsJoystickCommand(kortex_driver::SendJointSpeedsJoystickCommand::Request &req, kortex_driver::SendJointSpeedsJoystickCommand::Response &res) override; - std::function SendSelectedJointSpeedJoystickCommandHandler = nullptr; + std::function SendSelectedJointSpeedJoystickCommandHandler = nullptr; virtual bool SendSelectedJointSpeedJoystickCommand(kortex_driver::SendSelectedJointSpeedJoystickCommand::Request &req, kortex_driver::SendSelectedJointSpeedJoystickCommand::Response &res) override; - std::function EnableBridgeHandler = nullptr; + std::function EnableBridgeHandler = nullptr; virtual bool EnableBridge(kortex_driver::EnableBridge::Request &req, kortex_driver::EnableBridge::Response &res) override; - std::function DisableBridgeHandler = nullptr; + std::function DisableBridgeHandler = nullptr; virtual bool DisableBridge(kortex_driver::DisableBridge::Request &req, kortex_driver::DisableBridge::Response &res) override; - std::function GetBridgeListHandler = nullptr; + std::function GetBridgeListHandler = nullptr; virtual bool GetBridgeList(kortex_driver::GetBridgeList::Request &req, kortex_driver::GetBridgeList::Response &res) override; - std::function GetBridgeConfigHandler = nullptr; + std::function GetBridgeConfigHandler = nullptr; virtual bool GetBridgeConfig(kortex_driver::GetBridgeConfig::Request &req, kortex_driver::GetBridgeConfig::Response &res) override; - std::function PlayPreComputedJointTrajectoryHandler = nullptr; + std::function PlayPreComputedJointTrajectoryHandler = nullptr; virtual bool PlayPreComputedJointTrajectory(kortex_driver::PlayPreComputedJointTrajectory::Request &req, kortex_driver::PlayPreComputedJointTrajectory::Response &res) override; - std::function GetProductConfigurationHandler = nullptr; + std::function GetProductConfigurationHandler = nullptr; virtual bool GetProductConfiguration(kortex_driver::GetProductConfiguration::Request &req, kortex_driver::GetProductConfiguration::Response &res) override; - std::function UpdateEndEffectorTypeConfigurationHandler = nullptr; + std::function UpdateEndEffectorTypeConfigurationHandler = nullptr; virtual bool UpdateEndEffectorTypeConfiguration(kortex_driver::UpdateEndEffectorTypeConfiguration::Request &req, kortex_driver::UpdateEndEffectorTypeConfiguration::Response &res) override; - std::function RestoreFactoryProductConfigurationHandler = nullptr; + std::function RestoreFactoryProductConfigurationHandler = nullptr; virtual bool RestoreFactoryProductConfiguration(kortex_driver::RestoreFactoryProductConfiguration::Request &req, kortex_driver::RestoreFactoryProductConfiguration::Response &res) override; - std::function GetTrajectoryErrorReportHandler = nullptr; + std::function GetTrajectoryErrorReportHandler = nullptr; virtual bool GetTrajectoryErrorReport(kortex_driver::GetTrajectoryErrorReport::Request &req, kortex_driver::GetTrajectoryErrorReport::Response &res) override; - std::function GetAllJointsSpeedSoftLimitationHandler = nullptr; + std::function GetAllJointsSpeedSoftLimitationHandler = nullptr; virtual bool GetAllJointsSpeedSoftLimitation(kortex_driver::GetAllJointsSpeedSoftLimitation::Request &req, kortex_driver::GetAllJointsSpeedSoftLimitation::Response &res) override; - std::function GetAllJointsTorqueSoftLimitationHandler = nullptr; + std::function GetAllJointsTorqueSoftLimitationHandler = nullptr; virtual bool GetAllJointsTorqueSoftLimitation(kortex_driver::GetAllJointsTorqueSoftLimitation::Request &req, kortex_driver::GetAllJointsTorqueSoftLimitation::Response &res) override; - std::function GetTwistSoftLimitationHandler = nullptr; + std::function GetTwistSoftLimitationHandler = nullptr; virtual bool GetTwistSoftLimitation(kortex_driver::GetTwistSoftLimitation::Request &req, kortex_driver::GetTwistSoftLimitation::Response &res) override; - std::function GetWrenchSoftLimitationHandler = nullptr; + std::function GetWrenchSoftLimitationHandler = nullptr; virtual bool GetWrenchSoftLimitation(kortex_driver::GetWrenchSoftLimitation::Request &req, kortex_driver::GetWrenchSoftLimitation::Response &res) override; - std::function SetControllerConfigurationModeHandler = nullptr; + std::function SetControllerConfigurationModeHandler = nullptr; virtual bool SetControllerConfigurationMode(kortex_driver::SetControllerConfigurationMode::Request &req, kortex_driver::SetControllerConfigurationMode::Response &res) override; - std::function GetControllerConfigurationModeHandler = nullptr; + std::function GetControllerConfigurationModeHandler = nullptr; virtual bool GetControllerConfigurationMode(kortex_driver::GetControllerConfigurationMode::Request &req, kortex_driver::GetControllerConfigurationMode::Response &res) override; - std::function StartTeachingHandler = nullptr; + std::function StartTeachingHandler = nullptr; virtual bool StartTeaching(kortex_driver::StartTeaching::Request &req, kortex_driver::StartTeaching::Response &res) override; - std::function StopTeachingHandler = nullptr; + std::function StopTeachingHandler = nullptr; virtual bool StopTeaching(kortex_driver::StopTeaching::Request &req, kortex_driver::StopTeaching::Response &res) override; - std::function AddSequenceTasksHandler = nullptr; + std::function AddSequenceTasksHandler = nullptr; virtual bool AddSequenceTasks(kortex_driver::AddSequenceTasks::Request &req, kortex_driver::AddSequenceTasks::Response &res) override; - std::function UpdateSequenceTaskHandler = nullptr; + std::function UpdateSequenceTaskHandler = nullptr; virtual bool UpdateSequenceTask(kortex_driver::UpdateSequenceTask::Request &req, kortex_driver::UpdateSequenceTask::Response &res) override; - std::function SwapSequenceTasksHandler = nullptr; + std::function SwapSequenceTasksHandler = nullptr; virtual bool SwapSequenceTasks(kortex_driver::SwapSequenceTasks::Request &req, kortex_driver::SwapSequenceTasks::Response &res) override; - std::function ReadSequenceTaskHandler = nullptr; + std::function ReadSequenceTaskHandler = nullptr; virtual bool ReadSequenceTask(kortex_driver::ReadSequenceTask::Request &req, kortex_driver::ReadSequenceTask::Response &res) override; - std::function ReadAllSequenceTasksHandler = nullptr; + std::function ReadAllSequenceTasksHandler = nullptr; virtual bool ReadAllSequenceTasks(kortex_driver::ReadAllSequenceTasks::Request &req, kortex_driver::ReadAllSequenceTasks::Response &res) override; - std::function DeleteSequenceTaskHandler = nullptr; + std::function DeleteSequenceTaskHandler = nullptr; virtual bool DeleteSequenceTask(kortex_driver::DeleteSequenceTask::Request &req, kortex_driver::DeleteSequenceTask::Response &res) override; - std::function DeleteAllSequenceTasksHandler = nullptr; + std::function DeleteAllSequenceTasksHandler = nullptr; virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) override; - std::function TakeSnapshotHandler = nullptr; + std::function TakeSnapshotHandler = nullptr; virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) override; - std::function GetFirmwareBundleVersionsHandler = nullptr; + std::function GetFirmwareBundleVersionsHandler = nullptr; virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) override; - std::function MoveSequenceTaskHandler = nullptr; + std::function MoveSequenceTaskHandler = nullptr; virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) override; - std::function DuplicateMappingHandler = nullptr; + std::function DuplicateMappingHandler = nullptr; virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) override; - std::function DuplicateMapHandler = nullptr; + std::function DuplicateMapHandler = nullptr; virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) override; - std::function SetControllerConfigurationHandler = nullptr; + std::function SetControllerConfigurationHandler = nullptr; virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) override; - std::function GetControllerConfigurationHandler = nullptr; + std::function GetControllerConfigurationHandler = nullptr; virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) override; - std::function GetAllControllerConfigurationsHandler = nullptr; + std::function GetAllControllerConfigurationsHandler = nullptr; virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h index 8e6ce705..30a8738a 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/controlconfig_services.h @@ -28,64 +28,64 @@ class ControlConfigSimulationServices : public IControlConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function SetGravityVectorHandler = nullptr; + std::function SetGravityVectorHandler = nullptr; virtual bool SetGravityVector(kortex_driver::SetGravityVector::Request &req, kortex_driver::SetGravityVector::Response &res) override; - std::function GetGravityVectorHandler = nullptr; + std::function GetGravityVectorHandler = nullptr; virtual bool GetGravityVector(kortex_driver::GetGravityVector::Request &req, kortex_driver::GetGravityVector::Response &res) override; - std::function SetPayloadInformationHandler = nullptr; + std::function SetPayloadInformationHandler = nullptr; virtual bool SetPayloadInformation(kortex_driver::SetPayloadInformation::Request &req, kortex_driver::SetPayloadInformation::Response &res) override; - std::function GetPayloadInformationHandler = nullptr; + std::function GetPayloadInformationHandler = nullptr; virtual bool GetPayloadInformation(kortex_driver::GetPayloadInformation::Request &req, kortex_driver::GetPayloadInformation::Response &res) override; - std::function SetToolConfigurationHandler = nullptr; + std::function SetToolConfigurationHandler = nullptr; virtual bool SetToolConfiguration(kortex_driver::SetToolConfiguration::Request &req, kortex_driver::SetToolConfiguration::Response &res) override; - std::function GetToolConfigurationHandler = nullptr; + std::function GetToolConfigurationHandler = nullptr; virtual bool GetToolConfiguration(kortex_driver::GetToolConfiguration::Request &req, kortex_driver::GetToolConfiguration::Response &res) override; - std::function OnNotificationControlConfigurationTopicHandler = nullptr; + std::function OnNotificationControlConfigurationTopicHandler = nullptr; virtual bool OnNotificationControlConfigurationTopic(kortex_driver::OnNotificationControlConfigurationTopic::Request &req, kortex_driver::OnNotificationControlConfigurationTopic::Response &res) override; virtual void cb_ControlConfigurationTopic(Kinova::Api::ControlConfig::ControlConfigurationNotification notif) override; - std::function ControlConfig_UnsubscribeHandler = nullptr; + std::function ControlConfig_UnsubscribeHandler = nullptr; virtual bool ControlConfig_Unsubscribe(kortex_driver::ControlConfig_Unsubscribe::Request &req, kortex_driver::ControlConfig_Unsubscribe::Response &res) override; - std::function SetCartesianReferenceFrameHandler = nullptr; + std::function SetCartesianReferenceFrameHandler = nullptr; virtual bool SetCartesianReferenceFrame(kortex_driver::SetCartesianReferenceFrame::Request &req, kortex_driver::SetCartesianReferenceFrame::Response &res) override; - std::function GetCartesianReferenceFrameHandler = nullptr; + std::function GetCartesianReferenceFrameHandler = nullptr; virtual bool GetCartesianReferenceFrame(kortex_driver::GetCartesianReferenceFrame::Request &req, kortex_driver::GetCartesianReferenceFrame::Response &res) override; - std::function ControlConfig_GetControlModeHandler = nullptr; + std::function ControlConfig_GetControlModeHandler = nullptr; virtual bool ControlConfig_GetControlMode(kortex_driver::ControlConfig_GetControlMode::Request &req, kortex_driver::ControlConfig_GetControlMode::Response &res) override; - std::function SetJointSpeedSoftLimitsHandler = nullptr; + std::function SetJointSpeedSoftLimitsHandler = nullptr; virtual bool SetJointSpeedSoftLimits(kortex_driver::SetJointSpeedSoftLimits::Request &req, kortex_driver::SetJointSpeedSoftLimits::Response &res) override; - std::function SetTwistLinearSoftLimitHandler = nullptr; + std::function SetTwistLinearSoftLimitHandler = nullptr; virtual bool SetTwistLinearSoftLimit(kortex_driver::SetTwistLinearSoftLimit::Request &req, kortex_driver::SetTwistLinearSoftLimit::Response &res) override; - std::function SetTwistAngularSoftLimitHandler = nullptr; + std::function SetTwistAngularSoftLimitHandler = nullptr; virtual bool SetTwistAngularSoftLimit(kortex_driver::SetTwistAngularSoftLimit::Request &req, kortex_driver::SetTwistAngularSoftLimit::Response &res) override; - std::function SetJointAccelerationSoftLimitsHandler = nullptr; + std::function SetJointAccelerationSoftLimitsHandler = nullptr; virtual bool SetJointAccelerationSoftLimits(kortex_driver::SetJointAccelerationSoftLimits::Request &req, kortex_driver::SetJointAccelerationSoftLimits::Response &res) override; - std::function GetKinematicHardLimitsHandler = nullptr; + std::function GetKinematicHardLimitsHandler = nullptr; virtual bool GetKinematicHardLimits(kortex_driver::GetKinematicHardLimits::Request &req, kortex_driver::GetKinematicHardLimits::Response &res) override; - std::function GetKinematicSoftLimitsHandler = nullptr; + std::function GetKinematicSoftLimitsHandler = nullptr; virtual bool GetKinematicSoftLimits(kortex_driver::GetKinematicSoftLimits::Request &req, kortex_driver::GetKinematicSoftLimits::Response &res) override; - std::function GetAllKinematicSoftLimitsHandler = nullptr; + std::function GetAllKinematicSoftLimitsHandler = nullptr; virtual bool GetAllKinematicSoftLimits(kortex_driver::GetAllKinematicSoftLimits::Request &req, kortex_driver::GetAllKinematicSoftLimits::Response &res) override; - std::function SetDesiredLinearTwistHandler = nullptr; + std::function SetDesiredLinearTwistHandler = nullptr; virtual bool SetDesiredLinearTwist(kortex_driver::SetDesiredLinearTwist::Request &req, kortex_driver::SetDesiredLinearTwist::Response &res) override; - std::function SetDesiredAngularTwistHandler = nullptr; + std::function SetDesiredAngularTwistHandler = nullptr; virtual bool SetDesiredAngularTwist(kortex_driver::SetDesiredAngularTwist::Request &req, kortex_driver::SetDesiredAngularTwist::Response &res) override; - std::function SetDesiredJointSpeedsHandler = nullptr; + std::function SetDesiredJointSpeedsHandler = nullptr; virtual bool SetDesiredJointSpeeds(kortex_driver::SetDesiredJointSpeeds::Request &req, kortex_driver::SetDesiredJointSpeeds::Response &res) override; - std::function GetDesiredSpeedsHandler = nullptr; + std::function GetDesiredSpeedsHandler = nullptr; virtual bool GetDesiredSpeeds(kortex_driver::GetDesiredSpeeds::Request &req, kortex_driver::GetDesiredSpeeds::Response &res) override; - std::function ResetGravityVectorHandler = nullptr; + std::function ResetGravityVectorHandler = nullptr; virtual bool ResetGravityVector(kortex_driver::ResetGravityVector::Request &req, kortex_driver::ResetGravityVector::Response &res) override; - std::function ResetPayloadInformationHandler = nullptr; + std::function ResetPayloadInformationHandler = nullptr; virtual bool ResetPayloadInformation(kortex_driver::ResetPayloadInformation::Request &req, kortex_driver::ResetPayloadInformation::Response &res) override; - std::function ResetToolConfigurationHandler = nullptr; + std::function ResetToolConfigurationHandler = nullptr; virtual bool ResetToolConfiguration(kortex_driver::ResetToolConfiguration::Request &req, kortex_driver::ResetToolConfiguration::Response &res) override; - std::function ResetJointSpeedSoftLimitsHandler = nullptr; + std::function ResetJointSpeedSoftLimitsHandler = nullptr; virtual bool ResetJointSpeedSoftLimits(kortex_driver::ResetJointSpeedSoftLimits::Request &req, kortex_driver::ResetJointSpeedSoftLimits::Response &res) override; - std::function ResetTwistLinearSoftLimitHandler = nullptr; + std::function ResetTwistLinearSoftLimitHandler = nullptr; virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) override; - std::function ResetTwistAngularSoftLimitHandler = nullptr; + std::function ResetTwistAngularSoftLimitHandler = nullptr; virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) override; - std::function ResetJointAccelerationSoftLimitsHandler = nullptr; + std::function ResetJointAccelerationSoftLimitsHandler = nullptr; virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h index cd3429ec..d047b456 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/deviceconfig_services.h @@ -28,70 +28,70 @@ class DeviceConfigSimulationServices : public IDeviceConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function GetRunModeHandler = nullptr; + std::function GetRunModeHandler = nullptr; virtual bool GetRunMode(kortex_driver::GetRunMode::Request &req, kortex_driver::GetRunMode::Response &res) override; - std::function SetRunModeHandler = nullptr; + std::function SetRunModeHandler = nullptr; virtual bool SetRunMode(kortex_driver::SetRunMode::Request &req, kortex_driver::SetRunMode::Response &res) override; - std::function GetDeviceTypeHandler = nullptr; + std::function GetDeviceTypeHandler = nullptr; virtual bool GetDeviceType(kortex_driver::GetDeviceType::Request &req, kortex_driver::GetDeviceType::Response &res) override; - std::function GetFirmwareVersionHandler = nullptr; + std::function GetFirmwareVersionHandler = nullptr; virtual bool GetFirmwareVersion(kortex_driver::GetFirmwareVersion::Request &req, kortex_driver::GetFirmwareVersion::Response &res) override; - std::function GetBootloaderVersionHandler = nullptr; + std::function GetBootloaderVersionHandler = nullptr; virtual bool GetBootloaderVersion(kortex_driver::GetBootloaderVersion::Request &req, kortex_driver::GetBootloaderVersion::Response &res) override; - std::function GetModelNumberHandler = nullptr; + std::function GetModelNumberHandler = nullptr; virtual bool GetModelNumber(kortex_driver::GetModelNumber::Request &req, kortex_driver::GetModelNumber::Response &res) override; - std::function GetPartNumberHandler = nullptr; + std::function GetPartNumberHandler = nullptr; virtual bool GetPartNumber(kortex_driver::GetPartNumber::Request &req, kortex_driver::GetPartNumber::Response &res) override; - std::function GetSerialNumberHandler = nullptr; + std::function GetSerialNumberHandler = nullptr; virtual bool GetSerialNumber(kortex_driver::GetSerialNumber::Request &req, kortex_driver::GetSerialNumber::Response &res) override; - std::function GetMACAddressHandler = nullptr; + std::function GetMACAddressHandler = nullptr; virtual bool GetMACAddress(kortex_driver::GetMACAddress::Request &req, kortex_driver::GetMACAddress::Response &res) override; - std::function GetIPv4SettingsHandler = nullptr; + std::function GetIPv4SettingsHandler = nullptr; virtual bool GetIPv4Settings(kortex_driver::GetIPv4Settings::Request &req, kortex_driver::GetIPv4Settings::Response &res) override; - std::function SetIPv4SettingsHandler = nullptr; + std::function SetIPv4SettingsHandler = nullptr; virtual bool SetIPv4Settings(kortex_driver::SetIPv4Settings::Request &req, kortex_driver::SetIPv4Settings::Response &res) override; - std::function GetPartNumberRevisionHandler = nullptr; + std::function GetPartNumberRevisionHandler = nullptr; virtual bool GetPartNumberRevision(kortex_driver::GetPartNumberRevision::Request &req, kortex_driver::GetPartNumberRevision::Response &res) override; - std::function RebootRequestHandler = nullptr; + std::function RebootRequestHandler = nullptr; virtual bool RebootRequest(kortex_driver::RebootRequest::Request &req, kortex_driver::RebootRequest::Response &res) override; - std::function SetSafetyEnableHandler = nullptr; + std::function SetSafetyEnableHandler = nullptr; virtual bool SetSafetyEnable(kortex_driver::SetSafetyEnable::Request &req, kortex_driver::SetSafetyEnable::Response &res) override; - std::function SetSafetyErrorThresholdHandler = nullptr; + std::function SetSafetyErrorThresholdHandler = nullptr; virtual bool SetSafetyErrorThreshold(kortex_driver::SetSafetyErrorThreshold::Request &req, kortex_driver::SetSafetyErrorThreshold::Response &res) override; - std::function SetSafetyWarningThresholdHandler = nullptr; + std::function SetSafetyWarningThresholdHandler = nullptr; virtual bool SetSafetyWarningThreshold(kortex_driver::SetSafetyWarningThreshold::Request &req, kortex_driver::SetSafetyWarningThreshold::Response &res) override; - std::function SetSafetyConfigurationHandler = nullptr; + std::function SetSafetyConfigurationHandler = nullptr; virtual bool SetSafetyConfiguration(kortex_driver::SetSafetyConfiguration::Request &req, kortex_driver::SetSafetyConfiguration::Response &res) override; - std::function GetSafetyConfigurationHandler = nullptr; + std::function GetSafetyConfigurationHandler = nullptr; virtual bool GetSafetyConfiguration(kortex_driver::GetSafetyConfiguration::Request &req, kortex_driver::GetSafetyConfiguration::Response &res) override; - std::function GetSafetyInformationHandler = nullptr; + std::function GetSafetyInformationHandler = nullptr; virtual bool GetSafetyInformation(kortex_driver::GetSafetyInformation::Request &req, kortex_driver::GetSafetyInformation::Response &res) override; - std::function GetSafetyEnableHandler = nullptr; + std::function GetSafetyEnableHandler = nullptr; virtual bool GetSafetyEnable(kortex_driver::GetSafetyEnable::Request &req, kortex_driver::GetSafetyEnable::Response &res) override; - std::function GetSafetyStatusHandler = nullptr; + std::function GetSafetyStatusHandler = nullptr; virtual bool GetSafetyStatus(kortex_driver::GetSafetyStatus::Request &req, kortex_driver::GetSafetyStatus::Response &res) override; - std::function ClearAllSafetyStatusHandler = nullptr; + std::function ClearAllSafetyStatusHandler = nullptr; virtual bool ClearAllSafetyStatus(kortex_driver::ClearAllSafetyStatus::Request &req, kortex_driver::ClearAllSafetyStatus::Response &res) override; - std::function ClearSafetyStatusHandler = nullptr; + std::function ClearSafetyStatusHandler = nullptr; virtual bool ClearSafetyStatus(kortex_driver::ClearSafetyStatus::Request &req, kortex_driver::ClearSafetyStatus::Response &res) override; - std::function GetAllSafetyConfigurationHandler = nullptr; + std::function GetAllSafetyConfigurationHandler = nullptr; virtual bool GetAllSafetyConfiguration(kortex_driver::GetAllSafetyConfiguration::Request &req, kortex_driver::GetAllSafetyConfiguration::Response &res) override; - std::function GetAllSafetyInformationHandler = nullptr; + std::function GetAllSafetyInformationHandler = nullptr; virtual bool GetAllSafetyInformation(kortex_driver::GetAllSafetyInformation::Request &req, kortex_driver::GetAllSafetyInformation::Response &res) override; - std::function ResetSafetyDefaultsHandler = nullptr; + std::function ResetSafetyDefaultsHandler = nullptr; virtual bool ResetSafetyDefaults(kortex_driver::ResetSafetyDefaults::Request &req, kortex_driver::ResetSafetyDefaults::Response &res) override; - std::function OnNotificationSafetyTopicHandler = nullptr; + std::function OnNotificationSafetyTopicHandler = nullptr; virtual bool OnNotificationSafetyTopic(kortex_driver::OnNotificationSafetyTopic::Request &req, kortex_driver::OnNotificationSafetyTopic::Response &res) override; virtual void cb_SafetyTopic(Kinova::Api::Common::SafetyNotification notif) override; - std::function ExecuteCalibrationHandler = nullptr; + std::function ExecuteCalibrationHandler = nullptr; virtual bool ExecuteCalibration(kortex_driver::ExecuteCalibration::Request &req, kortex_driver::ExecuteCalibration::Response &res) override; - std::function GetCalibrationResultHandler = nullptr; + std::function GetCalibrationResultHandler = nullptr; virtual bool GetCalibrationResult(kortex_driver::GetCalibrationResult::Request &req, kortex_driver::GetCalibrationResult::Response &res) override; - std::function StopCalibrationHandler = nullptr; + std::function StopCalibrationHandler = nullptr; virtual bool StopCalibration(kortex_driver::StopCalibration::Request &req, kortex_driver::StopCalibration::Response &res) override; - std::function DeviceConfig_SetCapSenseConfigHandler = nullptr; + std::function DeviceConfig_SetCapSenseConfigHandler = nullptr; virtual bool DeviceConfig_SetCapSenseConfig(kortex_driver::DeviceConfig_SetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_SetCapSenseConfig::Response &res) override; - std::function DeviceConfig_GetCapSenseConfigHandler = nullptr; + std::function DeviceConfig_GetCapSenseConfigHandler = nullptr; virtual bool DeviceConfig_GetCapSenseConfig(kortex_driver::DeviceConfig_GetCapSenseConfig::Request &req, kortex_driver::DeviceConfig_GetCapSenseConfig::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h index c3305cad..0c45ae75 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/devicemanager_services.h @@ -28,7 +28,7 @@ class DeviceManagerSimulationServices : public IDeviceManagerServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function ReadAllDevicesHandler = nullptr; + std::function ReadAllDevicesHandler = nullptr; virtual bool ReadAllDevices(kortex_driver::ReadAllDevices::Request &req, kortex_driver::ReadAllDevices::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h index a5444818..1053941f 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/interconnectconfig_services.h @@ -28,33 +28,33 @@ class InterconnectConfigSimulationServices : public IInterconnectConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function GetUARTConfigurationHandler = nullptr; + std::function GetUARTConfigurationHandler = nullptr; virtual bool GetUARTConfiguration(kortex_driver::GetUARTConfiguration::Request &req, kortex_driver::GetUARTConfiguration::Response &res) override; - std::function SetUARTConfigurationHandler = nullptr; + std::function SetUARTConfigurationHandler = nullptr; virtual bool SetUARTConfiguration(kortex_driver::SetUARTConfiguration::Request &req, kortex_driver::SetUARTConfiguration::Response &res) override; - std::function GetEthernetConfigurationHandler = nullptr; + std::function GetEthernetConfigurationHandler = nullptr; virtual bool GetEthernetConfiguration(kortex_driver::GetEthernetConfiguration::Request &req, kortex_driver::GetEthernetConfiguration::Response &res) override; - std::function SetEthernetConfigurationHandler = nullptr; + std::function SetEthernetConfigurationHandler = nullptr; virtual bool SetEthernetConfiguration(kortex_driver::SetEthernetConfiguration::Request &req, kortex_driver::SetEthernetConfiguration::Response &res) override; - std::function GetGPIOConfigurationHandler = nullptr; + std::function GetGPIOConfigurationHandler = nullptr; virtual bool GetGPIOConfiguration(kortex_driver::GetGPIOConfiguration::Request &req, kortex_driver::GetGPIOConfiguration::Response &res) override; - std::function SetGPIOConfigurationHandler = nullptr; + std::function SetGPIOConfigurationHandler = nullptr; virtual bool SetGPIOConfiguration(kortex_driver::SetGPIOConfiguration::Request &req, kortex_driver::SetGPIOConfiguration::Response &res) override; - std::function GetGPIOStateHandler = nullptr; + std::function GetGPIOStateHandler = nullptr; virtual bool GetGPIOState(kortex_driver::GetGPIOState::Request &req, kortex_driver::GetGPIOState::Response &res) override; - std::function SetGPIOStateHandler = nullptr; + std::function SetGPIOStateHandler = nullptr; virtual bool SetGPIOState(kortex_driver::SetGPIOState::Request &req, kortex_driver::SetGPIOState::Response &res) override; - std::function GetI2CConfigurationHandler = nullptr; + std::function GetI2CConfigurationHandler = nullptr; virtual bool GetI2CConfiguration(kortex_driver::GetI2CConfiguration::Request &req, kortex_driver::GetI2CConfiguration::Response &res) override; - std::function SetI2CConfigurationHandler = nullptr; + std::function SetI2CConfigurationHandler = nullptr; virtual bool SetI2CConfiguration(kortex_driver::SetI2CConfiguration::Request &req, kortex_driver::SetI2CConfiguration::Response &res) override; - std::function I2CReadHandler = nullptr; + std::function I2CReadHandler = nullptr; virtual bool I2CRead(kortex_driver::I2CRead::Request &req, kortex_driver::I2CRead::Response &res) override; - std::function I2CReadRegisterHandler = nullptr; + std::function I2CReadRegisterHandler = nullptr; virtual bool I2CReadRegister(kortex_driver::I2CReadRegister::Request &req, kortex_driver::I2CReadRegister::Response &res) override; - std::function I2CWriteHandler = nullptr; + std::function I2CWriteHandler = nullptr; virtual bool I2CWrite(kortex_driver::I2CWrite::Request &req, kortex_driver::I2CWrite::Response &res) override; - std::function I2CWriteRegisterHandler = nullptr; + std::function I2CWriteRegisterHandler = nullptr; virtual bool I2CWriteRegister(kortex_driver::I2CWriteRegister::Request &req, kortex_driver::I2CWriteRegister::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h index 57ea508b..c98151ff 100644 --- a/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h +++ b/kortex_driver/include/kortex_driver/generated/simulation/visionconfig_services.h @@ -28,30 +28,30 @@ class VisionConfigSimulationServices : public IVisionConfigServices virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; - std::function SetSensorSettingsHandler = nullptr; + std::function SetSensorSettingsHandler = nullptr; virtual bool SetSensorSettings(kortex_driver::SetSensorSettings::Request &req, kortex_driver::SetSensorSettings::Response &res) override; - std::function GetSensorSettingsHandler = nullptr; + std::function GetSensorSettingsHandler = nullptr; virtual bool GetSensorSettings(kortex_driver::GetSensorSettings::Request &req, kortex_driver::GetSensorSettings::Response &res) override; - std::function GetOptionValueHandler = nullptr; + std::function GetOptionValueHandler = nullptr; virtual bool GetOptionValue(kortex_driver::GetOptionValue::Request &req, kortex_driver::GetOptionValue::Response &res) override; - std::function SetOptionValueHandler = nullptr; + std::function SetOptionValueHandler = nullptr; virtual bool SetOptionValue(kortex_driver::SetOptionValue::Request &req, kortex_driver::SetOptionValue::Response &res) override; - std::function GetOptionInformationHandler = nullptr; + std::function GetOptionInformationHandler = nullptr; virtual bool GetOptionInformation(kortex_driver::GetOptionInformation::Request &req, kortex_driver::GetOptionInformation::Response &res) override; - std::function OnNotificationVisionTopicHandler = nullptr; + std::function OnNotificationVisionTopicHandler = nullptr; virtual bool OnNotificationVisionTopic(kortex_driver::OnNotificationVisionTopic::Request &req, kortex_driver::OnNotificationVisionTopic::Response &res) override; virtual void cb_VisionTopic(Kinova::Api::VisionConfig::VisionNotification notif) override; - std::function DoSensorFocusActionHandler = nullptr; + std::function DoSensorFocusActionHandler = nullptr; virtual bool DoSensorFocusAction(kortex_driver::DoSensorFocusAction::Request &req, kortex_driver::DoSensorFocusAction::Response &res) override; - std::function GetIntrinsicParametersHandler = nullptr; + std::function GetIntrinsicParametersHandler = nullptr; virtual bool GetIntrinsicParameters(kortex_driver::GetIntrinsicParameters::Request &req, kortex_driver::GetIntrinsicParameters::Response &res) override; - std::function GetIntrinsicParametersProfileHandler = nullptr; + std::function GetIntrinsicParametersProfileHandler = nullptr; virtual bool GetIntrinsicParametersProfile(kortex_driver::GetIntrinsicParametersProfile::Request &req, kortex_driver::GetIntrinsicParametersProfile::Response &res) override; - std::function SetIntrinsicParametersHandler = nullptr; + std::function SetIntrinsicParametersHandler = nullptr; virtual bool SetIntrinsicParameters(kortex_driver::SetIntrinsicParameters::Request &req, kortex_driver::SetIntrinsicParameters::Response &res) override; - std::function GetExtrinsicParametersHandler = nullptr; + std::function GetExtrinsicParametersHandler = nullptr; virtual bool GetExtrinsicParameters(kortex_driver::GetExtrinsicParameters::Request &req, kortex_driver::GetExtrinsicParameters::Response &res) override; - std::function SetExtrinsicParametersHandler = nullptr; + std::function SetExtrinsicParametersHandler = nullptr; virtual bool SetExtrinsicParameters(kortex_driver::SetExtrinsicParameters::Request &req, kortex_driver::SetExtrinsicParameters::Response &res) override; }; diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 5249fe33..63126621 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -56,6 +56,7 @@ #include "kortex_driver/non-generated/pre_computed_joint_trajectory_action_server.h" #include "kortex_driver/non-generated/robotiq_gripper_command_action_server.h" #include "kortex_driver/non-generated/kortex_subscribers.h" +#include "kortex_driver/non-generated/kortex_arm_simulation.h" #define TCP_PORT 10000 #define UDP_PORT 10001 @@ -85,6 +86,7 @@ class KortexArmDriver // False if in simulation bool m_is_real_robot; + std::unique_ptr m_simulator; // Api options std::string m_ip_address; @@ -161,6 +163,7 @@ class KortexArmDriver bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); void publishRobotFeedback(); + void registerSimulationHandlers(); }; #endif diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h new file mode 100644 index 00000000..208a5b48 --- /dev/null +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_simulation.h @@ -0,0 +1,126 @@ +#ifndef _KORTEX_ARM_SIMULATION_H_ +#define _KORTEX_ARM_SIMULATION_H_ + +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2020 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +#include + +#include +#include + +#include "kortex_driver/non-generated/kortex_math_util.h" + +#include "kortex_driver/ActionType.h" +#include "kortex_driver/KortexError.h" + +#include "kortex_driver/CreateAction.h" +#include "kortex_driver/ReadAction.h" +#include "kortex_driver/ReadAllActions.h" +#include "kortex_driver/DeleteAction.h" +#include "kortex_driver/UpdateAction.h" +#include "kortex_driver/ExecuteActionFromReference.h" +#include "kortex_driver/ExecuteAction.h" +#include "kortex_driver/PauseAction.h" +#include "kortex_driver/StopAction.h" +#include "kortex_driver/ResumeAction.h" + +#include "kortex_driver/PlayCartesianTrajectory.h" +#include "kortex_driver/Stop.h" +#include "kortex_driver/GetMeasuredCartesianPose.h" +#include "kortex_driver/SendTwistCommand.h" +#include "kortex_driver/PlayJointTrajectory.h" +#include "kortex_driver/SendJointSpeedsCommand.h" +#include "kortex_driver/SendGripperCommand.h" +#include "kortex_driver/ApplyEmergencyStop.h" + +#include + +class KortexArmSimulation +{ + public: + KortexArmSimulation() = delete; + KortexArmSimulation(ros::NodeHandle& nh); + ~KortexArmSimulation(); + std::unordered_map GetActionsMap() const; + int GetDOF() const {return m_degrees_of_freedom;} + + // Handlers for simulated Kortex API functions + // Actions API + kortex_driver::CreateAction::Response CreateAction(const kortex_driver::CreateAction::Request& req); + kortex_driver::ReadAction::Response ReadAction(const kortex_driver::ReadAction::Request& req); + kortex_driver::ReadAllActions::Response ReadAllActions(const kortex_driver::ReadAllActions::Request& req); + kortex_driver::DeleteAction::Response DeleteAction(const kortex_driver::DeleteAction::Request& req); + kortex_driver::UpdateAction::Response UpdateAction(const kortex_driver::UpdateAction::Request& req); + kortex_driver::ExecuteActionFromReference::Response ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req); + kortex_driver::ExecuteAction::Response ExecuteAction(const kortex_driver::ExecuteAction::Request& req); + kortex_driver::StopAction::Response StopAction(const kortex_driver::StopAction::Request& req); + // Other RPCs + kortex_driver::PlayCartesianTrajectory::Response PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req); + kortex_driver::SendTwistCommand::Response SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req); + kortex_driver::PlayJointTrajectory::Response PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req); + kortex_driver::SendJointSpeedsCommand::Response SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req); + kortex_driver::SendGripperCommand::Response SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req); + kortex_driver::Stop::Response Stop(const kortex_driver::Stop::Request& req); + kortex_driver::ApplyEmergencyStop::Response ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req); + + private: + + ros::NodeHandle m_node_handle; + + // Publishers + ros::Publisher m_pub_action_topic; + + // Namespacing and prefixing information + std::string m_prefix; + std::string m_robot_name; + + // Arm and gripper information + std::string m_arm_name; + std::vector m_arm_joint_names; + std::string m_gripper_name; + std::vector m_gripper_joint_names; + std::vector m_gripper_joint_limits_max; + std::vector m_gripper_joint_limits_min; + int m_degrees_of_freedom; + + // Action-related + std::unordered_map m_map_actions; + + // Math utility + KortexMathUtil m_math_util; + + // Threading + std::atomic m_is_action_being_executed; + std::atomic m_action_preempted; + std::thread m_action_executor_thread; + + // MoveIt-related + std::unique_ptr m_moveit_arm_interface; + std::unique_ptr m_moveit_gripper_interface; + + // Helper functions + bool IsGripperPresent() const {return !m_gripper_name.empty();} + void CreateDefaultActions(); + + // Executors + void CancelAction(); + void PlayAction(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteReachJointAngles(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteReachPose(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendJointSpeeds(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendTwist(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteSendGripperCommand(const kortex_driver::Action& action); + kortex_driver::KortexError ExecuteTimeDelay(const kortex_driver::Action& action); +}; + +#endif //_KORTEX_ARM_SIMULATION_H_ diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 79123220..ce95d368 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -9,7 +9,9 @@ - + + + @@ -70,6 +72,7 @@ + diff --git a/kortex_driver/launch/kortex_dual_driver.launch b/kortex_driver/launch/kortex_dual_driver.launch index 4cee2ba6..57b7260c 100644 --- a/kortex_driver/launch/kortex_dual_driver.launch +++ b/kortex_driver/launch/kortex_dual_driver.launch @@ -90,6 +90,7 @@ + @@ -109,6 +110,7 @@ + diff --git a/kortex_driver/launch/test_simulator.launch b/kortex_driver/launch/test_simulator.launch new file mode 100644 index 00000000..b425760b --- /dev/null +++ b/kortex_driver/launch/test_simulator.launch @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [base_feedback/joint_state] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/kortex_driver/package.xml b/kortex_driver/package.xml index 3654d29d..02f59c73 100644 --- a/kortex_driver/package.xml +++ b/kortex_driver/package.xml @@ -14,6 +14,7 @@ std_msgs control_msgs actionlib + moveit_ros_planning_interface roscpp rospy std_msgs diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 1ead99b9..c4801fa7 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -16,7 +16,8 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), m_node_is_running(true), m_consecutive_base_cyclic_timeouts(0), m_is_interconnect_module_present(false), - m_is_vision_module_present(false) + m_is_vision_module_present(false), + m_simulator{} { // Parameter to let the other nodes know this node is up ros::param::set("is_initialized", false); @@ -30,11 +31,18 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), verifyProductConfiguration(); initSubscribers(); startActionServers(); - } + } // ROS Services are always started initRosServices(); + // Enable ROS Service simulation if not with a real robot + if (!m_is_real_robot) + { + m_simulator.reset(new KortexArmSimulation(nh)); + registerSimulationHandlers(); + } + // Start the thread to publish the feedback and joint states m_pub_base_feedback = m_node_handle.advertise("base_feedback", 1000); m_pub_joint_state = m_node_handle.advertise("base_feedback/joint_state", 1000); @@ -217,6 +225,13 @@ void KortexArmDriver::parseRosArguments() ROS_ERROR("%s", error_string.c_str()); throw new std::runtime_error(error_string); } + std::string robot_name; + if (!ros::param::get("~robot_name", robot_name)) + { + std::string error_string = "Robot name was not specified in the launch file, shutting down the node..."; + ROS_ERROR("%s", error_string.c_str()); + throw new std::runtime_error(error_string); + } if (!ros::param::get("~prefix", m_prefix)) { std::string error_string = "Prefix name was not specified in the launch file, shutting down the node..."; @@ -623,3 +638,32 @@ void KortexArmDriver::publishRobotFeedback() rate.sleep(); } } + +void KortexArmDriver::registerSimulationHandlers() +{ + BaseSimulationServices* base_services_simulation = dynamic_cast(m_base_ros_services); + // Link the m_simulator handlers to the ROS services callbacks + // Action services + base_services_simulation->CreateActionHandler = std::bind(&KortexArmSimulation::CreateAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ReadActionHandler = std::bind(&KortexArmSimulation::ReadAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ReadAllActionsHandler = std::bind(&KortexArmSimulation::ReadAllActions, m_simulator.get(), std::placeholders::_1); + base_services_simulation->DeleteActionHandler = std::bind(&KortexArmSimulation::DeleteAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->UpdateActionHandler = std::bind(&KortexArmSimulation::UpdateAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ExecuteActionFromReferenceHandler = std::bind(&KortexArmSimulation::ExecuteActionFromReference, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ExecuteActionHandler = std::bind(&KortexArmSimulation::ExecuteAction, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopActionHandler = std::bind(&KortexArmSimulation::StopAction, m_simulator.get(), std::placeholders::_1); + // Other services + base_services_simulation->PlayCartesianTrajectoryHandler = std::bind(&KortexArmSimulation::PlayCartesianTrajectory, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendTwistCommandHandler = std::bind(&KortexArmSimulation::SendTwistCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->PlayJointTrajectoryHandler = std::bind(&KortexArmSimulation::PlayJointTrajectory, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendJointSpeedsCommandHandler = std::bind(&KortexArmSimulation::SendJointSpeedsCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->SendGripperCommandHandler = std::bind(&KortexArmSimulation::SendGripperCommand, m_simulator.get(), std::placeholders::_1); + base_services_simulation->StopHandler = std::bind(&KortexArmSimulation::Stop, m_simulator.get(), std::placeholders::_1); + base_services_simulation->ApplyEmergencyStopHandler = std::bind(&KortexArmSimulation::ApplyEmergencyStop, m_simulator.get(), std::placeholders::_1); + + // Prospects + //SendSelectedJointSpeedCommand + //PlaySelectedJointTrajectory + //PlayCartesianTrajectoryPosition + //PlayCartesianTrajectoryOrientation +} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp new file mode 100644 index 00000000..f0c6d47b --- /dev/null +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -0,0 +1,657 @@ +/* +* KINOVA (R) KORTEX (TM) +* +* Copyright (c) 2020 Kinova inc. All rights reserved. +* +* This software may be modified and distributed under the +* terms of the BSD 3-Clause license. +* +* Refer to the LICENSE file for details. +* +*/ + +#include "kortex_driver/non-generated/kortex_arm_simulation.h" +#include "kortex_driver/ErrorCodes.h" +#include "kortex_driver/SubErrorCodes.h" +#include "kortex_driver/ActionNotification.h" +#include "kortex_driver/ActionEvent.h" + +#include +#include + +namespace +{ + static const std::string ARM_PLANNING_GROUP = "arm"; + static const std::string GRIPPER_PLANNING_GROUP = "gripper"; + static constexpr unsigned int FIRST_CREATED_ACTION_ID = 10000; + static const std::set DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; +} + +KortexArmSimulation::KortexArmSimulation(ros::NodeHandle& node_handle): m_node_handle(node_handle), + m_map_actions{}, + m_is_action_being_executed{false}, + m_action_preempted{false} +{ + // Namespacing and prefixing information + ros::param::get("~robot_name", m_robot_name); + ros::param::get("~prefix", m_prefix); + + // Arm and gripper information + ros::param::get("~dof", m_degrees_of_freedom); + ros::param::get("~arm", m_arm_name); + ros::param::get("~joint_names", m_arm_joint_names); + for (auto s : m_arm_joint_names) + { + s.insert(0, m_prefix); + } + ros::param::get("~gripper", m_gripper_name); + if (IsGripperPresent()) + { + ros::param::get("~gripper_joint_names", m_gripper_joint_names); + for (auto s : m_gripper_joint_names) + { + s.insert(0, m_prefix); + } + ros::param::get("~gripper_joint_limits_max", m_gripper_joint_limits_max); + ros::param::get("~gripper_joint_limits_min", m_gripper_joint_limits_min); + } + + // Print out simulation configuration + ROS_INFO("Simulating arm with following characteristics:"); + ROS_INFO("Arm type : %s", m_arm_name.c_str()); + ROS_INFO("Gripper type : %s", m_gripper_name.empty() ? "None" : m_gripper_name.c_str()); + ROS_INFO("Arm namespace : %s", m_robot_name.c_str()); + ROS_INFO("URDF prefix : %s", m_prefix.c_str()); + + // Start MoveIt client + m_moveit_arm_interface.reset(new moveit::planning_interface::MoveGroupInterface(ARM_PLANNING_GROUP)); + if (IsGripperPresent()) + { + m_moveit_gripper_interface.reset(new moveit::planning_interface::MoveGroupInterface(GRIPPER_PLANNING_GROUP)); + } + + // Create default actions + CreateDefaultActions(); + + // Create publishers + m_pub_action_topic = m_node_handle.advertise("action_topic", 1000); +} + +KortexArmSimulation::~KortexArmSimulation() +{ + CancelAction(); +} + +std::unordered_map KortexArmSimulation::GetActionsMap() const +{ + return m_map_actions; +} + +kortex_driver::CreateAction::Response KortexArmSimulation::CreateAction(const kortex_driver::CreateAction::Request& req) +{ + auto new_action = req.input; + unsigned int identifier = FIRST_CREATED_ACTION_ID; + bool identifier_taken = true; + // Find unique identifier for new action + while (identifier_taken) + { + identifier_taken = m_map_actions.count(identifier) == 1; + if (identifier_taken) + { + ++identifier; + } + } + // Add Action to map if type is supported + switch (new_action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + case kortex_driver::ActionType::REACH_POSE: + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + case kortex_driver::ActionType::TIME_DELAY: + new_action.handle.identifier = identifier; + new_action.handle.permission = 7; + m_map_actions.emplace(std::make_pair(identifier, new_action)); + break; + default: + ROS_ERROR("Unsupported action type %d : could not create simulated action.", new_action.handle.action_type); + break; + } + // Return ActionHandle for added action + kortex_driver::CreateAction::Response response; + response.output = new_action.handle; + return response; +} + +kortex_driver::ReadAction::Response KortexArmSimulation::ReadAction(const kortex_driver::ReadAction::Request& req) +{ + auto input = req.input; + kortex_driver::ReadAction::Response response; + auto it = m_map_actions.find(input.identifier); + if (it != m_map_actions.end()) + { + response.output = it->second; + } + return response; +} + +kortex_driver::ReadAllActions::Response KortexArmSimulation::ReadAllActions(const kortex_driver::ReadAllActions::Request& req) +{ + auto input = req.input; + kortex_driver::ReadAllActions::Response response; + kortex_driver::ActionList action_list; + for (auto a : m_map_actions) + { + // If requested action type is specified and matches iterated action's type, add it to the list + if (input.action_type == 0 || input.action_type == a.second.handle.action_type) + { + action_list.action_list.push_back(a.second); + } + + } + response.output = action_list; + return response; +} + +kortex_driver::DeleteAction::Response KortexArmSimulation::DeleteAction(const kortex_driver::DeleteAction::Request& req) +{ + auto handle = req.input; + // If the action is not a default action + if (DEFAULT_ACTIONS_IDENTIFIERS.find(handle.identifier) == DEFAULT_ACTIONS_IDENTIFIERS.end()) + { + auto it = m_map_actions.find(handle.identifier); + if (it != m_map_actions.end()) + { + m_map_actions.erase(it); + ROS_INFO("Simulated action #%u properly deleted.", handle.identifier); + } + else + { + ROS_WARN("Could not find simulated action #%u to delete in actions map.", handle.identifier); + } + } + else + { + ROS_ERROR("Cannot delete default simulated actions."); + } + + return kortex_driver::DeleteAction::Response(); +} + +kortex_driver::UpdateAction::Response KortexArmSimulation::UpdateAction(const kortex_driver::UpdateAction::Request& req) +{ + auto action = req.input; + // If the action is not a default action + if (DEFAULT_ACTIONS_IDENTIFIERS.find(action.handle.identifier) == DEFAULT_ACTIONS_IDENTIFIERS.end()) + { + auto it = m_map_actions.find(action.handle.identifier); + if (it != m_map_actions.end()) + { + if (it->second.handle.action_type == action.handle.action_type) + { + it->second = action; + ROS_INFO("Simulated action #%u properly updated.", action.handle.identifier); + } + else + { + ROS_ERROR("Cannot update action with different type."); + } + } + else + { + ROS_ERROR("Could not find simulated action #%u to update in actions map.", action.handle.identifier); + } + } + else + { + ROS_ERROR("Cannot update default simulated actions."); + } + + return kortex_driver::UpdateAction::Response(); +} + +kortex_driver::ExecuteActionFromReference::Response KortexArmSimulation::ExecuteActionFromReference(const kortex_driver::ExecuteActionFromReference::Request& req) +{ + auto handle = req.input; + auto it = m_map_actions.find(handle.identifier); + if (it != m_map_actions.end()) + { + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, it->second); + } + else + { + ROS_ERROR("Could not find action with given identifier %d", handle.identifier); + } + + return kortex_driver::ExecuteActionFromReference::Response(); +} + +kortex_driver::ExecuteAction::Response KortexArmSimulation::ExecuteAction(const kortex_driver::ExecuteAction::Request& req) +{ + auto action = req.input; + + // Add Action to map if type is supported + switch (action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + case kortex_driver::ActionType::REACH_POSE: + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + case kortex_driver::ActionType::TIME_DELAY: + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + break; + default: + ROS_ERROR("Unsupported action type %d : could not execute simulated action.", action.handle.action_type); + break; + } + + return kortex_driver::ExecuteAction::Response(); +} + +kortex_driver::StopAction::Response KortexArmSimulation::StopAction(const kortex_driver::StopAction::Request& req) +{ + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + + return kortex_driver::StopAction::Response(); +} + +kortex_driver::PlayCartesianTrajectory::Response KortexArmSimulation::PlayCartesianTrajectory(const kortex_driver::PlayCartesianTrajectory::Request& req) +{ + auto constrained_pose = req.input; + kortex_driver::Action action; + action.name = "PlayCartesianTrajectory"; + action.handle.action_type = kortex_driver::ActionType::REACH_POSE; + action.oneof_action_parameters.reach_pose.push_back(constrained_pose); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::PlayCartesianTrajectory::Response(); +} + +kortex_driver::SendTwistCommand::Response KortexArmSimulation::SendTwistCommand(const kortex_driver::SendTwistCommand::Request& req) +{ + auto twist_command = req.input; + kortex_driver::Action action; + action.name = "SendTwistCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_TWIST_COMMAND; + action.oneof_action_parameters.send_twist_command.push_back(twist_command); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendTwistCommand::Response(); +} + +kortex_driver::PlayJointTrajectory::Response KortexArmSimulation::PlayJointTrajectory(const kortex_driver::PlayJointTrajectory::Request& req) +{ + auto constrained_joint_angles = req.input; + kortex_driver::Action action; + action.name = "PlayJointTrajectory"; + action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + action.oneof_action_parameters.reach_joint_angles.push_back(constrained_joint_angles); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::PlayJointTrajectory::Response(); +} + +kortex_driver::SendJointSpeedsCommand::Response KortexArmSimulation::SendJointSpeedsCommand(const kortex_driver::SendJointSpeedsCommand::Request& req) +{ + auto joint_speeds = req.input; + kortex_driver::Action action; + action.name = "SendJointSpeedsCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + action.oneof_action_parameters.send_joint_speeds.push_back(joint_speeds); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendJointSpeedsCommand::Response(); +} + +kortex_driver::SendGripperCommand::Response KortexArmSimulation::SendGripperCommand(const kortex_driver::SendGripperCommand::Request& req) +{ + auto gripper_command = req.input; + kortex_driver::Action action; + action.name = "GripperCommand"; + action.handle.action_type = kortex_driver::ActionType::SEND_GRIPPER_COMMAND; + action.oneof_action_parameters.send_gripper_command.push_back(gripper_command); + + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + m_action_executor_thread = std::thread(&KortexArmSimulation::PlayAction, this, action); + + return kortex_driver::SendGripperCommand::Response(); +} + +kortex_driver::Stop::Response KortexArmSimulation::Stop(const kortex_driver::Stop::Request& req) +{ + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + return kortex_driver::Stop::Response(); +} + +kortex_driver::ApplyEmergencyStop::Response KortexArmSimulation::ApplyEmergencyStop(const kortex_driver::ApplyEmergencyStop::Request& req) +{ + // If an action is ongoing, cancel it first + if (m_is_action_being_executed.load()) + { + CancelAction(); // this will block until the thread is joined and current action finished + } + return kortex_driver::ApplyEmergencyStop::Response(); +} + +void KortexArmSimulation::CreateDefaultActions() +{ + kortex_driver::Action retract, home, zero; + kortex_driver::ConstrainedJointAngles retract_angles, home_angles, zero_angles; + // Retract + retract.handle.identifier = 1; + retract.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + retract.handle.permission = 7; + retract.name = "Retract"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("retract"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + retract_angles.joint_angles.joint_angles.push_back(a); + } + retract.oneof_action_parameters.reach_joint_angles.push_back(retract_angles); + // Home + home.handle.identifier = 2; + home.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + home.handle.permission = 7; + home.name = "Home"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("home"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + home_angles.joint_angles.joint_angles.push_back(a); + } + home.oneof_action_parameters.reach_joint_angles.push_back(home_angles); + // Zero + zero.handle.identifier = 3; + zero.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + zero.handle.permission = 7; + zero.name = "Zero"; + for (int i = 0; i < m_degrees_of_freedom; i++) + { + kortex_driver::JointAngle a; + a.joint_identifier = i; + auto named_target = m_moveit_arm_interface->getNamedTargetValues("vertical"); + double moveit_angle = named_target["joint_"+std::to_string(i+1)]; // rad + a.value = m_math_util.wrapDegreesFromZeroTo360(m_math_util.toDeg(moveit_angle)); + zero_angles.joint_angles.joint_angles.push_back(a); + } + zero.oneof_action_parameters.reach_joint_angles.push_back(zero_angles); + // Add actions + m_map_actions.emplace(std::make_pair(retract.handle.identifier, retract)); + m_map_actions.emplace(std::make_pair(home.handle.identifier, home)); + m_map_actions.emplace(std::make_pair(zero.handle.identifier, zero)); +} + +void KortexArmSimulation::CancelAction() +{ + m_action_preempted = true; + if (m_action_executor_thread.joinable()) + { + m_action_executor_thread.join(); + } + m_action_preempted = false; +} + +void KortexArmSimulation::PlayAction(const kortex_driver::Action& action) +{ + kortex_driver::KortexError action_result; + + // Notify action started + kortex_driver::ActionNotification start_notif; + start_notif.handle = action.handle; + start_notif.action_event = kortex_driver::ActionEvent::ACTION_START; + m_pub_action_topic.publish(start_notif); + m_is_action_being_executed = true; + + // Switch executor on the action type + switch (action.handle.action_type) + { + case kortex_driver::ActionType::REACH_JOINT_ANGLES: + action_result = ExecuteReachJointAngles(action); + break; + case kortex_driver::ActionType::REACH_POSE: + action_result = ExecuteReachPose(action); + break; + case kortex_driver::ActionType::SEND_JOINT_SPEEDS: + action_result = ExecuteSendJointSpeeds(action); + break; + case kortex_driver::ActionType::SEND_TWIST_COMMAND: + action_result = ExecuteSendTwist(action); + break; + case kortex_driver::ActionType::SEND_GRIPPER_COMMAND: + action_result = ExecuteSendGripperCommand(action); + break; + case kortex_driver::ActionType::TIME_DELAY: + action_result = ExecuteTimeDelay(action); + break; + default: + action_result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + action_result.subCode = kortex_driver::SubErrorCodes::UNSUPPORTED_ACTION; + break; + } + + kortex_driver::ActionNotification end_notif; + end_notif.handle = action.handle; + // Action was cancelled by user + if (m_action_preempted.load()) + { + // Notify ACTION_ABORT + end_notif.action_event = kortex_driver::ActionEvent::ACTION_ABORT; + ROS_WARN("Action was aborted by user."); + } + // Action ended on its own + else + { + if (action_result.code != kortex_driver::ErrorCodes::ERROR_NONE) + { + // Notify ACTION_ABORT + end_notif.action_event = kortex_driver::ActionEvent::ACTION_ABORT; + end_notif.abort_details = action_result.subCode; + ROS_WARN("Action was failed : \nError code is %d\nSub-error code is %d\nError description is : %s", + action_result.code, + action_result.subCode, + action_result.description.c_str()); + } + else + { + // Notify ACTION_END + end_notif.action_event = kortex_driver::ActionEvent::ACTION_END; + } + } + m_pub_action_topic.publish(end_notif); + + m_is_action_being_executed = false; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.reach_joint_angles.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joint angles action : action is malformed."; + return result; + } + auto constrained_joint_angles = action.oneof_action_parameters.reach_joint_angles[0]; + if (constrained_joint_angles.joint_angles.joint_angles.size() != GetDOF()) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joint angles action : action contains " + std::to_string(constrained_joint_angles.joint_angles.joint_angles.size()) + " joint angles but arm has " + std::to_string(GetDOF()); + return result; + } + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm to angular position + + return result; +} + +// TODO Fill implementation +kortex_driver::KortexError KortexArmSimulation::ExecuteReachPose(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.reach_pose.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing pose action : action is malformed."; + return result; + } + auto constrained_pose = action.oneof_action_parameters.reach_pose[0]; + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm to Cartesian pose + + return result; +} + +// TODO Fill implementation +kortex_driver::KortexError KortexArmSimulation::ExecuteSendJointSpeeds(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_joint_speeds.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joints speeds : action is malformed."; + return result; + } + auto joint_speeds = action.oneof_action_parameters.send_joint_speeds[0]; + if (joint_speeds.joint_speeds.size() != GetDOF()) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing joint speeds action : action contains " + std::to_string(joint_speeds.joint_speeds.size()) + " joint speeds but arm has " + std::to_string(GetDOF()); + return result; + } + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm at angular speeds + + return result; +} + +// TODO Fill implementation +kortex_driver::KortexError KortexArmSimulation::ExecuteSendTwist(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_twist_command.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing twist action : action is malformed."; + return result; + } + auto twist = action.oneof_action_parameters.send_twist_command[0]; + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Fill implementation to move simulated arm at Cartesian twist + + return result; +} + +// TODO Fill implementation +kortex_driver::KortexError KortexArmSimulation::ExecuteSendGripperCommand(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (action.oneof_action_parameters.send_gripper_command.size() != 1) + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing gripper command action : action is malformed."; + return result; + } + auto gripper_command = action.oneof_action_parameters.send_gripper_command[0]; + + // TODO Handle constraints and warn if some cannot be applied in simulation + // TODO Handle velocity mode too? + // TODO Fill implementation to move simulated gripper to given position + + return result; +} + +kortex_driver::KortexError KortexArmSimulation::ExecuteTimeDelay(const kortex_driver::Action& action) +{ + kortex_driver::KortexError result; + result.code = kortex_driver::ErrorCodes::ERROR_NONE; + result.subCode = kortex_driver::SubErrorCodes::SUB_ERROR_NONE; + if (!action.oneof_action_parameters.delay.empty()) + { + auto start = std::chrono::system_clock::now(); + uint32_t delay_seconds = action.oneof_action_parameters.delay[0].duration; + // While not preempted and duration not elapsed + while (!m_action_preempted.load() && (std::chrono::system_clock::now() - start) < std::chrono::seconds(delay_seconds)) + { + // sleep a bit + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + else + { + result.code = kortex_driver::ErrorCodes::ERROR_DEVICE; + result.subCode = kortex_driver::SubErrorCodes::INVALID_PARAM; + result.description = "Error playing time delay action : action is malformed."; + } + return result; +} diff --git a/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc b/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc index 42ac5233..60ec3c50 100644 --- a/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc +++ b/kortex_driver/src/non-generated/tests/kortex_arm_driver_func_tests.cc @@ -54,15 +54,3 @@ TEST_F(KortexDriverTest, parseURDF) urdf::Model model; ASSERT_TRUE(model.initParam("robot_description")); } - -int main(int argc, char** argv){ - ros::init(argc, argv, "KortexArmDriverInitTestsNode"); - testing::InitGoogleTest(&argc, argv); - - std::thread t([]{while(ros::ok()) ros::spin();}); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - return res; -} \ No newline at end of file diff --git a/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc new file mode 100644 index 00000000..9b05c1fb --- /dev/null +++ b/kortex_driver/src/non-generated/tests/kortex_simulator_unit_tests.cc @@ -0,0 +1,508 @@ +#include +#include +#include +#include +#include +#include "kortex_driver/ActionEvent.h" +#include "kortex_driver/CartesianReferenceFrame.h" +#include "kortex_driver/GripperMode.h" + +class KortexSimulatorTest : public ::testing::Test { + protected: + + void SetUp() override + { + // Create Simulator + m_simulator.reset(new KortexArmSimulation(n)); + + // Create dummy action + dummy_action.name = "MyDummyAction"; + dummy_action.handle.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + dummy_action.handle.permission = 7; + kortex_driver::ConstrainedJointAngles angles; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = 10.0f*i; + angles.joint_angles.joint_angles.push_back(angle); + } + dummy_action.oneof_action_parameters.reach_joint_angles.push_back(angles); + + // Create action topic subscriber + m_action_topic_sub = n.subscribe("action_topic", 5, &KortexSimulatorTest::ActionTopicHandler, this); + m_received_notifications.clear(); + } + + void TearDown() override + { + } + + void CompareReachJointAnglesActions(const kortex_driver::Action& a1, const kortex_driver::Action& a2, bool same) + { + ASSERT_EQ(a1.oneof_action_parameters.reach_joint_angles.size(), a2.oneof_action_parameters.reach_joint_angles.size()); + auto angles1 = a1.oneof_action_parameters.reach_joint_angles[0]; + auto angles2 = a2.oneof_action_parameters.reach_joint_angles[0]; + ASSERT_EQ(angles1.joint_angles.joint_angles.size(), angles2.joint_angles.joint_angles.size()); + for (int i = 0; i < angles1.joint_angles.joint_angles.size() && same; i++) + { + ASSERT_EQ(same, angles1.joint_angles.joint_angles.at(i) == angles2.joint_angles.joint_angles.at(i)); + } + } + + void ActionTopicHandler(const kortex_driver::ActionNotification& notif) + { + m_received_notifications.push_back(notif); + } + + kortex_driver::Action dummy_action; + ros::NodeHandle n; + std::unique_ptr m_simulator; + ros::Subscriber m_action_topic_sub; + std::vector m_received_notifications; +}; + +// Make sure after initialisation the default actions are +// Tests ReadAllActions at the same time +TEST_F(KortexSimulatorTest, DefaultActions) +{ + auto actions = m_simulator->GetActionsMap(); + bool retract, home, zero, other = false; + for (auto a : actions) + { + if (a.second.name == "Retract") retract = true; + else if (a.second.name == "Home") home = true; + else if (a.second.name == "Zero") zero = true; + else other = true; + } + ASSERT_TRUE(retract); + ASSERT_TRUE(home); + ASSERT_TRUE(zero); + ASSERT_FALSE(other); +} + +// Tests DeleteAction so default actions are not deleted +TEST_F(KortexSimulatorTest, DeleteDefaultActions) +{ + static const std::vector DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + // Make sure the action can be deleted properly + kortex_driver::DeleteAction::Request req; + kortex_driver::ActionHandle handle; + for (unsigned int i : DEFAULT_ACTIONS_IDENTIFIERS) + { + handle.identifier = i; + req.input = handle; + m_simulator->DeleteAction(req); + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(i), 1); + } +} + +// Tests UpdateAction so default actions are not updated +TEST_F(KortexSimulatorTest, UpdateDefaultActions) +{ + static const std::vector DEFAULT_ACTIONS_IDENTIFIERS{1,2,3}; + // Make sure the action cannot be updated + kortex_driver::UpdateAction::Request req; + for (unsigned int i : DEFAULT_ACTIONS_IDENTIFIERS) + { + dummy_action.handle.identifier = i; + req.input = dummy_action; + m_simulator->UpdateAction(req); + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(i), 1); + CompareReachJointAnglesActions(actions_map[i], dummy_action, false); + } +} + +// Tests ReadAllActions +TEST_F(KortexSimulatorTest, ReadAllActions) +{ + // Test for 3 known actions (default) of this type in the map + kortex_driver::ReadAllActions::Request req; + kortex_driver::RequestedActionType type; + type.action_type = kortex_driver::ActionType::REACH_JOINT_ANGLES; + req.input = type; + auto res = m_simulator->ReadAllActions(req); + auto action_list = res.output; + ASSERT_EQ(action_list.action_list.size(), 3); // Number of default actions + + // Test for 0 known actions of this type in the map + type.action_type = kortex_driver::ActionType::REACH_POSE; + req.input = type; + res = m_simulator->ReadAllActions(req); + action_list = res.output; + ASSERT_TRUE(action_list.action_list.empty()); +} + +// Tests CreateAction handler for a supported Action, and DeleteAction +TEST_F(KortexSimulatorTest, CreateSupportedAction) +{ + static const std::string name = "MyNewAction"; + + kortex_driver::CreateAction::Request req; + dummy_action.name = name; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(actions_map[handle.identifier].name, name); + ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); + + // Make sure the action can be deleted properly + kortex_driver::DeleteAction::Request del_req; + del_req.input = handle; + m_simulator->DeleteAction(del_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 0); +} + +// Tests CreateAction handler for an unsupported Action +TEST_F(KortexSimulatorTest, CreateUnsupportedAction) +{ + kortex_driver::CreateAction::Request req; + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + kortex_driver::Base_JointSpeeds speeds; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointSpeed speed; + speed.joint_identifier = i; + speed.value = 10.0f*i; + speeds.joint_speeds.push_back(speed); + } + dummy_action.oneof_action_parameters.send_joint_speeds.push_back(speeds); + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 0); +} + +// Tests UpdateAction on existing and non-existing actions +TEST_F(KortexSimulatorTest, UpdateAction) +{ + // Create Action at first + kortex_driver::CreateAction::Request req; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + auto handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + + // Make sure the action was added to the map + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(actions_map[handle.identifier].handle.action_type, kortex_driver::ActionType::REACH_JOINT_ANGLES); + + // Modify and update the Action + dummy_action.name = "MyUpdatedName"; + dummy_action.handle.identifier = handle.identifier; + dummy_action.oneof_action_parameters.reach_joint_angles[0].joint_angles.joint_angles[3].value = 0.0f; + kortex_driver::UpdateAction::Request update_req; + update_req.input = dummy_action; + m_simulator->UpdateAction(update_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); + CompareReachJointAnglesActions(dummy_action, actions_map[handle.identifier], true); + + // Modify and update the Action with a different type + kortex_driver::Action wrong_type_action; + wrong_type_action.name = "WrongType"; + wrong_type_action.handle.identifier = handle.identifier; + wrong_type_action.handle.action_type = kortex_driver::ActionType::SEND_JOINT_SPEEDS; + wrong_type_action.oneof_action_parameters.reach_joint_angles.clear(); + wrong_type_action.oneof_action_parameters.send_joint_speeds.push_back(kortex_driver::Base_JointSpeeds()); + update_req.input = wrong_type_action; + m_simulator->UpdateAction(update_req); + actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(handle.identifier), 1); + ASSERT_EQ(dummy_action.name, actions_map[handle.identifier].name); + ASSERT_EQ(kortex_driver::ActionType::REACH_JOINT_ANGLES, actions_map[handle.identifier].handle.action_type); +} + +// This uses a TIME_DELAY action to test execution +TEST_F(KortexSimulatorTest, ExecuteAction) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Make sure after a couple seconds we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + std::this_thread::sleep_for(std::chrono::seconds(3)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// This uses a TIME_DELAY action to test execution from reference +TEST_F(KortexSimulatorTest, ExecuteActionFromReference) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create the Delay action + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::CreateAction::Request req; + req.input = dummy_action; + auto res = m_simulator->CreateAction(req); + dummy_action.handle = res.output; + auto actions_map = m_simulator->GetActionsMap(); + ASSERT_EQ(actions_map.count(dummy_action.handle.identifier), 1); + + // Execute the Delay action by reference + ASSERT_EQ(m_received_notifications.size(), 0); + kortex_driver::ExecuteActionFromReference::Request execute_req; + execute_req.input = dummy_action.handle; + m_simulator->ExecuteActionFromReference(execute_req); + std::this_thread::sleep_for(std::chrono::seconds(2)); + // Make sure after a couple seconds we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + std::this_thread::sleep_for(std::chrono::seconds(3)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// This uses a TIME_DELAY action to test aborting +TEST_F(KortexSimulatorTest, StopAction) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare StopAction request + kortex_driver::StopAction::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->StopAction(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} + +TEST_F(KortexSimulatorTest, PlayCartesianTrajectory) +{ + kortex_driver::PlayCartesianTrajectory::Request req; + kortex_driver::ConstrainedPose pose; + pose.target_pose.x = 0.1; + pose.target_pose.y = 0.1; + pose.target_pose.z = 0.1; + pose.target_pose.theta_x = 0.1; + pose.target_pose.theta_y = 0.1; + pose.target_pose.theta_z = 0.1; + req.input = pose; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->PlayCartesianTrajectory(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendTwistCommand) +{ + kortex_driver::SendTwistCommand::Request req; + kortex_driver::TwistCommand twist_command; + twist_command.reference_frame = kortex_driver::CartesianReferenceFrame::CARTESIAN_REFERENCE_FRAME_BASE; + twist_command.twist.linear_x = 0.1; + twist_command.twist.linear_y = 0.1; + twist_command.twist.linear_z = 0.1; + twist_command.twist.angular_x = 0.1; + twist_command.twist.angular_y = 0.1; + twist_command.twist.angular_z = 0.1; + req.input = twist_command; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendTwistCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, PlayJointTrajectory) +{ + kortex_driver::PlayJointTrajectory::Request req; + kortex_driver::ConstrainedJointAngles constrained_joint_angles; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointAngle angle; + angle.joint_identifier = i; + angle.value = i*10.0f; + constrained_joint_angles.joint_angles.joint_angles.push_back(angle); + } + req.input = constrained_joint_angles; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->PlayJointTrajectory(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendJointSpeedsCommand) +{ + kortex_driver::SendJointSpeedsCommand::Request req; + kortex_driver::Base_JointSpeeds joint_speeds; + for (int i = 0; i < m_simulator->GetDOF(); i++) + { + kortex_driver::JointSpeed speed; + speed.joint_identifier = i; + speed.value = i*10.0f; + joint_speeds.joint_speeds.push_back(speed); + } + req.input = joint_speeds; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendJointSpeedsCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +TEST_F(KortexSimulatorTest, SendGripperCommand) +{ + kortex_driver::SendGripperCommand::Request req; + kortex_driver::GripperCommand gripper_command; + gripper_command.mode = kortex_driver::GripperMode::GRIPPER_POSITION; + kortex_driver::Finger finger; + finger.finger_identifier = 0; + finger.value = 10.0f; + gripper_command.gripper.finger.push_back(finger); + req.input = gripper_command; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->SendGripperCommand(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // TODO Change when implementation is filled + // Make sure after one second we received the ACTION_START and ACTION_END + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_END); +} + +// Use a TIME_DELAY action to test the Stop RPC +TEST_F(KortexSimulatorTest, Stop) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare Stop request + kortex_driver::Stop::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->Stop(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} + +TEST_F(KortexSimulatorTest, ApplyEmergencyStop) +{ + static constexpr uint32_t SLEEP_DURATION_SECONDS = 4; + + // Create delay action object + dummy_action.oneof_action_parameters.reach_joint_angles.clear(); + dummy_action.handle.action_type = kortex_driver::ActionType::TIME_DELAY; + kortex_driver::Delay delay; + delay.duration = SLEEP_DURATION_SECONDS; + dummy_action.oneof_action_parameters.delay.push_back(delay); + kortex_driver::ExecuteAction::Request req; + req.input = dummy_action; + + // Prepare Stop request + kortex_driver::ApplyEmergencyStop::Request stop_req; + + // Execute the action + ASSERT_EQ(m_received_notifications.size(), 0); + m_simulator->ExecuteAction(req); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Make sure after one second we received the ACTION_START + ASSERT_EQ(m_received_notifications.size(), 1); + ASSERT_EQ(m_received_notifications[0].action_event, kortex_driver::ActionEvent::ACTION_START); + + // Abort the action now + m_simulator->ApplyEmergencyStop(stop_req); + + // Wait a biut and make sure we received the ACTION_ABORT + std::this_thread::sleep_for(std::chrono::seconds(1)); + ASSERT_EQ(m_received_notifications.size(), 2); + ASSERT_EQ(m_received_notifications[1].action_event, kortex_driver::ActionEvent::ACTION_ABORT); +} diff --git a/kortex_driver/src/non-generated/tests/main.cc b/kortex_driver/src/non-generated/tests/main.cc new file mode 100644 index 00000000..d330136e --- /dev/null +++ b/kortex_driver/src/non-generated/tests/main.cc @@ -0,0 +1,15 @@ +#include +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "TestNode"); + testing::InitGoogleTest(&argc, argv); + + std::thread t([]{while(ros::ok()) ros::spin();}); + + auto res = RUN_ALL_TESTS(); + + ros::shutdown(); + return res; +} \ No newline at end of file diff --git a/kortex_driver/templates/services_simulation.h.jinja2 b/kortex_driver/templates/services_simulation.h.jinja2 index 9a9e49a5..c2ccd970 100644 --- a/kortex_driver/templates/services_simulation.h.jinja2 +++ b/kortex_driver/templates/services_simulation.h.jinja2 @@ -29,7 +29,7 @@ class {{package.short_name}}SimulationServices : public I{{package.short_name}}S virtual bool SetDeviceID(kortex_driver::SetDeviceID::Request &req, kortex_driver::SetDeviceID::Response &res) override; virtual bool SetApiOptions(kortex_driver::SetApiOptions::Request &req, kortex_driver::SetApiOptions::Response &res) override; {%- for method in package.methods %} - std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; + std::function {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}Handler = nullptr; virtual bool {{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}(kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Request &req, kortex_driver::{{method.prepend_rpc_package_name}}{{method.prepend_on_notification}}{{method.name}}::Response &res) override; {%- if method.is_notification_rpc %} virtual void cb_{{method.name}}({{method.notification_message_cpp_namespace}}::{{method.prepend_rpc_package_name}}{{method.name|replace("Topic", "")}}Notification notif) override; diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 836674bc..44f4bf5b 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -11,12 +11,16 @@ - + - + + + + + @@ -134,6 +138,7 @@ + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml index ff37a3ce..3b0aed09 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,11 @@ + + - + diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch index 990954d1..222a7a55 100644 --- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/launch/planning_context.launch @@ -12,7 +12,12 @@ - + + + + diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro index 5930cc1e..beb8c857 100644 --- a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro +++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf.xacro @@ -4,6 +4,7 @@ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml index d2f4eaf8..482742e6 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,11 +1,12 @@ + - + diff --git a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch index 4548bcdc..21884a9f 100644 --- a/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_move_it_config/launch/planning_context.launch @@ -18,7 +18,12 @@ - + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml index f3a2be33..6ff1ed9f 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -2,11 +2,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch index 73d7424e..7b26b18d 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/launch/planning_context.launch @@ -18,7 +18,12 @@ - + + + + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml index 65ad9d3f..d9a09845 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/fake_moveit_controller_manager.launch.xml @@ -2,11 +2,12 @@ + - + diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch index 72420090..5483e95a 100644 --- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch +++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/launch/planning_context.launch @@ -18,7 +18,12 @@ - + + + +