Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merge release 2.3 to kinetic-devel #154

Merged
merged 4 commits into from
Jun 4, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions kortex_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -54,11 +54,13 @@ foreach(sub_dir ${srv_generated_dir_list})
add_service_files(DIRECTORY srv/generated/${sub_dir})
endforeach()

add_action_files(DIRECTORY action/non_generated)

## generate added messages and services
generate_messages(DEPENDENCIES std_msgs)
generate_messages(DEPENDENCIES std_msgs actionlib_msgs)

## declare a catkin package
catkin_package()
catkin_package(CATKIN_DEPENDS actionlib_msgs)

if(USE_CONAN)
# Include conan.cmake module and download Kortex API from artifactory
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
#The trajectory to follow
CartesianWaypoint[] trajectory
duration goal_time_tolerance
bool use_optimal_blending
---
#result definition
string error_string
int32 error_code
int32 SUCCESSFUL = 0
int32 INVALID_GOAL = -1
int32 PATH_TOLERANCE_VIOLATED = -2

---
#feedback
CartesianWaypoint actual
2 changes: 1 addition & 1 deletion kortex_driver/conanfile.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,4 +6,4 @@
class ROSKortexConan(ConanFile):

def requirements(self):
self.requires("kortex_api_cpp/2.2.0-r.31@kortex/stable")
self.requires("kortex_api_cpp/2.3.0-r.34@kortex/stable")
Original file line number Diff line number Diff line change
Expand Up @@ -84,8 +84,8 @@
#include "kortex_driver/ConfigurationChangeNotification.h"
#include "kortex_driver/OnNotificationMappingInfoTopic.h"
#include "kortex_driver/MappingInfoNotification.h"
#include "kortex_driver/OnNotificationControlModeTopic.h"
#include "kortex_driver/ControlModeNotification.h"
#include "kortex_driver/Base_OnNotificationControlModeTopic.h"
#include "kortex_driver/Base_ControlModeNotification.h"
#include "kortex_driver/OnNotificationOperatingModeTopic.h"
#include "kortex_driver/OperatingModeNotification.h"
#include "kortex_driver/OnNotificationSequenceInfoTopic.h"
Expand Down Expand Up @@ -176,12 +176,16 @@
#include "kortex_driver/DeleteAllSequenceTasks.h"
#include "kortex_driver/TakeSnapshot.h"
#include "kortex_driver/GetFirmwareBundleVersions.h"
#include "kortex_driver/ExecuteWaypointTrajectory.h"
#include "kortex_driver/MoveSequenceTask.h"
#include "kortex_driver/DuplicateMapping.h"
#include "kortex_driver/DuplicateMap.h"
#include "kortex_driver/SetControllerConfiguration.h"
#include "kortex_driver/GetControllerConfiguration.h"
#include "kortex_driver/GetAllControllerConfigurations.h"
#include "kortex_driver/ComputeForwardKinematics.h"
#include "kortex_driver/ComputeInverseKinematics.h"
#include "kortex_driver/ValidateWaypointList.h"

#include "kortex_driver/KortexError.h"
#include "kortex_driver/SetDeviceID.h"
Expand Down Expand Up @@ -257,7 +261,7 @@ class IBaseServices
virtual void cb_ConfigurationChangeTopic(Kinova::Api::Base::ConfigurationChangeNotification notif) = 0;
virtual bool OnNotificationMappingInfoTopic(kortex_driver::OnNotificationMappingInfoTopic::Request &req, kortex_driver::OnNotificationMappingInfoTopic::Response &res) = 0;
virtual void cb_MappingInfoTopic(Kinova::Api::Base::MappingInfoNotification notif) = 0;
virtual bool OnNotificationControlModeTopic(kortex_driver::OnNotificationControlModeTopic::Request &req, kortex_driver::OnNotificationControlModeTopic::Response &res) = 0;
virtual bool Base_OnNotificationControlModeTopic(kortex_driver::Base_OnNotificationControlModeTopic::Request &req, kortex_driver::Base_OnNotificationControlModeTopic::Response &res) = 0;
virtual void cb_ControlModeTopic(Kinova::Api::Base::ControlModeNotification notif) = 0;
virtual bool OnNotificationOperatingModeTopic(kortex_driver::OnNotificationOperatingModeTopic::Request &req, kortex_driver::OnNotificationOperatingModeTopic::Response &res) = 0;
virtual void cb_OperatingModeTopic(Kinova::Api::Base::OperatingModeNotification notif) = 0;
Expand Down Expand Up @@ -349,12 +353,16 @@ class IBaseServices
virtual bool DeleteAllSequenceTasks(kortex_driver::DeleteAllSequenceTasks::Request &req, kortex_driver::DeleteAllSequenceTasks::Response &res) = 0;
virtual bool TakeSnapshot(kortex_driver::TakeSnapshot::Request &req, kortex_driver::TakeSnapshot::Response &res) = 0;
virtual bool GetFirmwareBundleVersions(kortex_driver::GetFirmwareBundleVersions::Request &req, kortex_driver::GetFirmwareBundleVersions::Response &res) = 0;
virtual bool ExecuteWaypointTrajectory(kortex_driver::ExecuteWaypointTrajectory::Request &req, kortex_driver::ExecuteWaypointTrajectory::Response &res) = 0;
virtual bool MoveSequenceTask(kortex_driver::MoveSequenceTask::Request &req, kortex_driver::MoveSequenceTask::Response &res) = 0;
virtual bool DuplicateMapping(kortex_driver::DuplicateMapping::Request &req, kortex_driver::DuplicateMapping::Response &res) = 0;
virtual bool DuplicateMap(kortex_driver::DuplicateMap::Request &req, kortex_driver::DuplicateMap::Response &res) = 0;
virtual bool SetControllerConfiguration(kortex_driver::SetControllerConfiguration::Request &req, kortex_driver::SetControllerConfiguration::Response &res) = 0;
virtual bool GetControllerConfiguration(kortex_driver::GetControllerConfiguration::Request &req, kortex_driver::GetControllerConfiguration::Response &res) = 0;
virtual bool GetAllControllerConfigurations(kortex_driver::GetAllControllerConfigurations::Request &req, kortex_driver::GetAllControllerConfigurations::Response &res) = 0;
virtual bool ComputeForwardKinematics(kortex_driver::ComputeForwardKinematics::Request &req, kortex_driver::ComputeForwardKinematics::Response &res) = 0;
virtual bool ComputeInverseKinematics(kortex_driver::ComputeInverseKinematics::Request &req, kortex_driver::ComputeInverseKinematics::Response &res) = 0;
virtual bool ValidateWaypointList(kortex_driver::ValidateWaypointList::Request &req, kortex_driver::ValidateWaypointList::Response &res) = 0;

protected:
ros::NodeHandle m_node_handle;
Expand Down Expand Up @@ -449,7 +457,7 @@ class IBaseServices
ros::ServiceServer m_serviceBase_Unsubscribe;
ros::ServiceServer m_serviceOnNotificationConfigurationChangeTopic;
ros::ServiceServer m_serviceOnNotificationMappingInfoTopic;
ros::ServiceServer m_serviceOnNotificationControlModeTopic;
ros::ServiceServer m_serviceBase_OnNotificationControlModeTopic;
ros::ServiceServer m_serviceOnNotificationOperatingModeTopic;
ros::ServiceServer m_serviceOnNotificationSequenceInfoTopic;
ros::ServiceServer m_serviceOnNotificationProtectionZoneTopic;
Expand Down Expand Up @@ -529,11 +537,15 @@ class IBaseServices
ros::ServiceServer m_serviceDeleteAllSequenceTasks;
ros::ServiceServer m_serviceTakeSnapshot;
ros::ServiceServer m_serviceGetFirmwareBundleVersions;
ros::ServiceServer m_serviceExecuteWaypointTrajectory;
ros::ServiceServer m_serviceMoveSequenceTask;
ros::ServiceServer m_serviceDuplicateMapping;
ros::ServiceServer m_serviceDuplicateMap;
ros::ServiceServer m_serviceSetControllerConfiguration;
ros::ServiceServer m_serviceGetControllerConfiguration;
ros::ServiceServer m_serviceGetAllControllerConfigurations;
ros::ServiceServer m_serviceComputeForwardKinematics;
ros::ServiceServer m_serviceComputeInverseKinematics;
ros::ServiceServer m_serviceValidateWaypointList;
};
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@
#include "kortex_driver/ResetTwistLinearSoftLimit.h"
#include "kortex_driver/ResetTwistAngularSoftLimit.h"
#include "kortex_driver/ResetJointAccelerationSoftLimits.h"
#include "kortex_driver/ControlConfig_OnNotificationControlModeTopic.h"
#include "kortex_driver/ControlConfig_ControlModeNotification.h"

#include "kortex_driver/KortexError.h"
#include "kortex_driver/SetDeviceID.h"
Expand Down Expand Up @@ -99,12 +101,16 @@ class IControlConfigServices
virtual bool ResetTwistLinearSoftLimit(kortex_driver::ResetTwistLinearSoftLimit::Request &req, kortex_driver::ResetTwistLinearSoftLimit::Response &res) = 0;
virtual bool ResetTwistAngularSoftLimit(kortex_driver::ResetTwistAngularSoftLimit::Request &req, kortex_driver::ResetTwistAngularSoftLimit::Response &res) = 0;
virtual bool ResetJointAccelerationSoftLimits(kortex_driver::ResetJointAccelerationSoftLimits::Request &req, kortex_driver::ResetJointAccelerationSoftLimits::Response &res) = 0;
virtual bool ControlConfig_OnNotificationControlModeTopic(kortex_driver::ControlConfig_OnNotificationControlModeTopic::Request &req, kortex_driver::ControlConfig_OnNotificationControlModeTopic::Response &res) = 0;
virtual void cb_ControlModeTopic(Kinova::Api::ControlConfig::ControlModeNotification notif) = 0;

protected:
ros::NodeHandle m_node_handle;
ros::Publisher m_pub_Error;
ros::Publisher m_pub_ControlConfigurationTopic;
bool m_is_activated_ControlConfigurationTopic;
ros::Publisher m_pub_ControlModeTopic;
bool m_is_activated_ControlModeTopic;

ros::ServiceServer m_serviceSetDeviceID;
ros::ServiceServer m_serviceSetApiOptions;
Expand Down Expand Up @@ -138,5 +144,6 @@ class IControlConfigServices
ros::ServiceServer m_serviceResetTwistLinearSoftLimit;
ros::ServiceServer m_serviceResetTwistAngularSoftLimit;
ros::ServiceServer m_serviceResetJointAccelerationSoftLimits;
ros::ServiceServer m_serviceControlConfig_OnNotificationControlModeTopic;
};
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,9 @@
#include "kortex_driver/generated/robot/visionconfig_proto_converter.h"


#include "kortex_driver/GpioConfigurationList.h"
#include "kortex_driver/Base_GpioConfiguration.h"
#include "kortex_driver/GpioPinConfiguration.h"
#include "kortex_driver/FullUserProfile.h"
#include "kortex_driver/UserProfile.h"
#include "kortex_driver/UserProfileList.h"
Expand Down Expand Up @@ -99,7 +102,7 @@
#include "kortex_driver/ConfigurationChangeNotification.h"
#include "kortex_driver/MappingInfoNotification.h"
#include "kortex_driver/Base_ControlModeInformation.h"
#include "kortex_driver/ControlModeNotification.h"
#include "kortex_driver/Base_ControlModeNotification.h"
#include "kortex_driver/ServoingModeInformation.h"
#include "kortex_driver/OperatingModeInformation.h"
#include "kortex_driver/OperatingModeNotification.h"
Expand All @@ -116,6 +119,7 @@
#include "kortex_driver/ControllerState.h"
#include "kortex_driver/ControllerElementState.h"
#include "kortex_driver/ActionNotification.h"
#include "kortex_driver/TrajectoryInfo.h"
#include "kortex_driver/ActionExecutionState.h"
#include "kortex_driver/RobotEventNotification.h"
#include "kortex_driver/FactoryNotification.h"
Expand Down Expand Up @@ -176,6 +180,7 @@
#include "kortex_driver/GripperRequest.h"
#include "kortex_driver/Gripper.h"
#include "kortex_driver/Finger.h"
#include "kortex_driver/GpioCommand.h"
#include "kortex_driver/SystemTime.h"
#include "kortex_driver/ControllerConfigurationMode.h"
#include "kortex_driver/ControllerConfiguration.h"
Expand All @@ -193,10 +198,20 @@
#include "kortex_driver/PreComputedJointTrajectoryElement.h"
#include "kortex_driver/TrajectoryErrorElement.h"
#include "kortex_driver/TrajectoryErrorReport.h"
#include "kortex_driver/WaypointValidationReport.h"
#include "kortex_driver/Waypoint.h"
#include "kortex_driver/AngularWaypoint.h"
#include "kortex_driver/CartesianWaypoint.h"
#include "kortex_driver/WaypointList.h"
#include "kortex_driver/KinematicTrajectoryConstraints.h"
#include "kortex_driver/FirmwareBundleVersions.h"
#include "kortex_driver/FirmwareComponentVersion.h"
#include "kortex_driver/IKData.h"


int ToProtoData(kortex_driver::GpioConfigurationList input, Kinova::Api::Base::GpioConfigurationList *output);
int ToProtoData(kortex_driver::Base_GpioConfiguration input, Kinova::Api::Base::GpioConfiguration *output);
int ToProtoData(kortex_driver::GpioPinConfiguration input, Kinova::Api::Base::GpioPinConfiguration *output);
int ToProtoData(kortex_driver::FullUserProfile input, Kinova::Api::Base::FullUserProfile *output);
int ToProtoData(kortex_driver::UserProfile input, Kinova::Api::Base::UserProfile *output);
int ToProtoData(kortex_driver::UserProfileList input, Kinova::Api::Base::UserProfileList *output);
Expand Down Expand Up @@ -255,7 +270,7 @@ int ToProtoData(kortex_driver::Query input, Kinova::Api::Base::Query *output);
int ToProtoData(kortex_driver::ConfigurationChangeNotification input, Kinova::Api::Base::ConfigurationChangeNotification *output);
int ToProtoData(kortex_driver::MappingInfoNotification input, Kinova::Api::Base::MappingInfoNotification *output);
int ToProtoData(kortex_driver::Base_ControlModeInformation input, Kinova::Api::Base::ControlModeInformation *output);
int ToProtoData(kortex_driver::ControlModeNotification input, Kinova::Api::Base::ControlModeNotification *output);
int ToProtoData(kortex_driver::Base_ControlModeNotification input, Kinova::Api::Base::ControlModeNotification *output);
int ToProtoData(kortex_driver::ServoingModeInformation input, Kinova::Api::Base::ServoingModeInformation *output);
int ToProtoData(kortex_driver::OperatingModeInformation input, Kinova::Api::Base::OperatingModeInformation *output);
int ToProtoData(kortex_driver::OperatingModeNotification input, Kinova::Api::Base::OperatingModeNotification *output);
Expand All @@ -272,6 +287,7 @@ int ToProtoData(kortex_driver::ControllerList input, Kinova::Api::Base::Controll
int ToProtoData(kortex_driver::ControllerState input, Kinova::Api::Base::ControllerState *output);
int ToProtoData(kortex_driver::ControllerElementState input, Kinova::Api::Base::ControllerElementState *output);
int ToProtoData(kortex_driver::ActionNotification input, Kinova::Api::Base::ActionNotification *output);
int ToProtoData(kortex_driver::TrajectoryInfo input, Kinova::Api::Base::TrajectoryInfo *output);
int ToProtoData(kortex_driver::ActionExecutionState input, Kinova::Api::Base::ActionExecutionState *output);
int ToProtoData(kortex_driver::RobotEventNotification input, Kinova::Api::Base::RobotEventNotification *output);
int ToProtoData(kortex_driver::FactoryNotification input, Kinova::Api::Base::FactoryNotification *output);
Expand Down Expand Up @@ -332,6 +348,7 @@ int ToProtoData(kortex_driver::GripperCommand input, Kinova::Api::Base::GripperC
int ToProtoData(kortex_driver::GripperRequest input, Kinova::Api::Base::GripperRequest *output);
int ToProtoData(kortex_driver::Gripper input, Kinova::Api::Base::Gripper *output);
int ToProtoData(kortex_driver::Finger input, Kinova::Api::Base::Finger *output);
int ToProtoData(kortex_driver::GpioCommand input, Kinova::Api::Base::GpioCommand *output);
int ToProtoData(kortex_driver::SystemTime input, Kinova::Api::Base::SystemTime *output);
int ToProtoData(kortex_driver::ControllerConfigurationMode input, Kinova::Api::Base::ControllerConfigurationMode *output);
int ToProtoData(kortex_driver::ControllerConfiguration input, Kinova::Api::Base::ControllerConfiguration *output);
Expand All @@ -349,7 +366,14 @@ int ToProtoData(kortex_driver::PreComputedJointTrajectory input, Kinova::Api::Ba
int ToProtoData(kortex_driver::PreComputedJointTrajectoryElement input, Kinova::Api::Base::PreComputedJointTrajectoryElement *output);
int ToProtoData(kortex_driver::TrajectoryErrorElement input, Kinova::Api::Base::TrajectoryErrorElement *output);
int ToProtoData(kortex_driver::TrajectoryErrorReport input, Kinova::Api::Base::TrajectoryErrorReport *output);
int ToProtoData(kortex_driver::WaypointValidationReport input, Kinova::Api::Base::WaypointValidationReport *output);
int ToProtoData(kortex_driver::Waypoint input, Kinova::Api::Base::Waypoint *output);
int ToProtoData(kortex_driver::AngularWaypoint input, Kinova::Api::Base::AngularWaypoint *output);
int ToProtoData(kortex_driver::CartesianWaypoint input, Kinova::Api::Base::CartesianWaypoint *output);
int ToProtoData(kortex_driver::WaypointList input, Kinova::Api::Base::WaypointList *output);
int ToProtoData(kortex_driver::KinematicTrajectoryConstraints input, Kinova::Api::Base::KinematicTrajectoryConstraints *output);
int ToProtoData(kortex_driver::FirmwareBundleVersions input, Kinova::Api::Base::FirmwareBundleVersions *output);
int ToProtoData(kortex_driver::FirmwareComponentVersion input, Kinova::Api::Base::FirmwareComponentVersion *output);
int ToProtoData(kortex_driver::IKData input, Kinova::Api::Base::IKData *output);

#endif
Loading