Skip to content

Commit

Permalink
Added simulation option to the driver so the gazebo package can launc…
Browse files Browse the repository at this point in the history 
…h the kortex_driver too, in a different mode
  • Loading branch information
@alexvannobel
alexvannobel committed Jun 10, 2020
1 parent 892513b commit 382909f
Show file tree
Hide file tree
Showing 6 changed files with 237 additions and 110 deletions.
5 changes: 4 additions & 1 deletion kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,9 @@ class KortexArmDriver

ros::NodeHandle m_node_handle;

// False if in simulation
bool m_is_real_robot;

// Api options
std::string m_ip_address;
std::string m_username;
Expand Down Expand Up @@ -157,7 +160,7 @@ class KortexArmDriver
// Private methods
bool isGripperPresent();
void setAngularTrajectorySoftLimitsToMax();
void publishFeedback();
void publishRobotFeedback();
};

#endif
1 change: 1 addition & 0 deletions kortex_driver/launch/kortex_driver.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@

<!-- Start the kortex_driver node -->
<node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
<param name="sim" value="false"/>
<param name="ip_address" value="$(arg ip_address)"/>
<param name="username" value="$(arg username)"/>
<param name="password" value="$(arg password)"/>
Expand Down
272 changes: 163 additions & 109 deletions kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,23 +12,36 @@

#include "kortex_driver/non-generated/kortex_arm_driver.h"

KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), m_node_is_running(true), m_consecutive_base_cyclic_timeouts(0)
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)
{
// Parameter to let the other nodes know this node is up
ros::param::set("is_initialized", false);

// Start the node in the different initialization functions
parseRosArguments();
initApi();
verifyProductConfiguration();
initSubscribers();

// If with a real robot, start the robot-specific stuff
if (m_is_real_robot)
{
initApi();
verifyProductConfiguration();
initSubscribers();
startActionServers();
}

// ROS Services are always started
initRosServices();
startActionServers();

// Start the thread to publish the feedback and joint states
m_pub_base_feedback = m_node_handle.advertise<kortex_driver::BaseCyclic_Feedback>("base_feedback", 1000);
m_pub_joint_state = m_node_handle.advertise<sensor_msgs::JointState>("base_feedback/joint_state", 1000);
m_publish_feedback_thread = std::thread(&KortexArmDriver::publishFeedback, this);
if (m_is_real_robot)
{
m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this);
}

// If we get here and no error was thrown we started the node correctly
ROS_INFO("%sThe Kortex driver has been initialized correctly!%s", GREEN_COLOR_CONSOLE, RESET_COLOR_CONSOLE);
Expand All @@ -44,14 +57,6 @@ KortexArmDriver::~KortexArmDriver()
m_publish_feedback_thread.join();
}

delete m_action_server_follow_joint_trajectory;
if (m_action_server_gripper_command)
{
delete m_action_server_gripper_command;
}

delete m_topic_subscribers;

delete m_actuator_config_ros_services;
delete m_base_ros_services;
delete m_control_config_ros_services;
Expand All @@ -65,100 +70,126 @@ KortexArmDriver::~KortexArmDriver()
{
delete m_vision_config_ros_services;
}

m_tcp_session_manager->CloseSession();
m_udp_session_manager->CloseSession();
m_tcp_router->SetActivationStatus(false);
m_udp_router->SetActivationStatus(false);
m_tcp_transport->disconnect();
m_udp_transport->disconnect();

delete m_actuator_config;
delete m_base;
delete m_control_config;
delete m_device_config;
delete m_device_manager;
delete m_interconnect_config;
delete m_vision_config;
delete m_base_cyclic;

delete m_tcp_session_manager;
delete m_udp_session_manager;

delete m_tcp_router;
delete m_udp_router;
delete m_tcp_transport;
delete m_udp_transport;
}

void KortexArmDriver::parseRosArguments()
{
bool use_sim_time = false;
if (ros::param::get("/use_sim_time", use_sim_time))
if (!m_is_real_robot)
{
if (use_sim_time)

delete m_action_server_follow_joint_trajectory;
if (m_action_server_gripper_command)
{
std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver.";
ROS_WARN("%s", error_string.c_str());
delete m_action_server_gripper_command;
}
}

if (!ros::param::get("~ip_address", m_ip_address))
{
std::string error_string = "IP address of the robot 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);
}
delete m_topic_subscribers;

if (!ros::param::get("~username", m_username))
{
std::string error_string = "Username for the robot session 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);
m_tcp_session_manager->CloseSession();
m_udp_session_manager->CloseSession();
m_tcp_router->SetActivationStatus(false);
m_udp_router->SetActivationStatus(false);
m_tcp_transport->disconnect();
m_udp_transport->disconnect();

delete m_actuator_config;
delete m_base;
delete m_control_config;
delete m_device_config;
delete m_device_manager;
delete m_interconnect_config;
delete m_vision_config;
delete m_base_cyclic;

delete m_tcp_session_manager;
delete m_udp_session_manager;

delete m_tcp_router;
delete m_udp_router;
delete m_tcp_transport;
delete m_udp_transport;
}
}

if (!ros::param::get("~password", m_password))
void KortexArmDriver::parseRosArguments()
{
bool sim;
if (!ros::param::get("~sim", sim))
{
std::string error_string = "Password for the robot session was not specified in the launch file, shutting down the node...";
std::string error_string = "Simulation 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);
}
m_is_real_robot = !sim;

if (!ros::param::get("~use_hard_limits", m_use_hard_limits))
// Some parameters are only necessary with a real robot
if (m_is_real_robot)
{
std::string error_string = "Usage of hard limits as soft 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);
}
bool use_sim_time = false;
if (ros::param::get("/use_sim_time", use_sim_time))
{
if (use_sim_time)
{
std::string error_string = "ROS parameter /use_sim_time is true : you may experience problems and you should set it to false and restart the driver.";
ROS_WARN("%s", error_string.c_str());
}
}

if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate))
{
std::string error_string = "Publish rate of the cyclic data 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("~ip_address", m_ip_address))
{
std::string error_string = "IP address of the robot 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("~api_rpc_timeout_ms", m_api_rpc_timeout_ms))
{
std::string error_string = "API RPC timeout duration 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("~username", m_username))
{
std::string error_string = "Username for the robot session 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("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms))
{
std::string error_string = "API session inactivity timeout duration 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("~password", m_password))
{
std::string error_string = "Password for the robot session 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("~use_hard_limits", m_use_hard_limits))
{
std::string error_string = "Usage of hard limits as soft 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("~api_rpc_timeout_ms", m_api_rpc_timeout_ms))
{
std::string error_string = "API RPC timeout duration 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("~api_session_inactivity_timeout_ms", m_api_session_inactivity_timeout_ms))
{
std::string error_string = "API session inactivity timeout duration 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("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms))
{
std::string error_string = "API connection inactivity timeout duration 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("~api_connection_inactivity_timeout_ms", m_api_connection_inactivity_timeout_ms))

if (!ros::param::get("~cyclic_data_publish_rate", m_cyclic_data_publish_rate))
{
std::string error_string = "API connection inactivity timeout duration was not specified in the launch file, shutting down the node...";
std::string error_string = "Publish rate of the cyclic data 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("~arm", m_arm_name))
{
std::string error_string = "Arm name was not specified in the launch file, shutting down the node...";
Expand Down Expand Up @@ -364,11 +395,7 @@ void KortexArmDriver::verifyProductConfiguration()
ROS_INFO("-------------------------------------------------");
ROS_INFO("Scanning all devices in robot... ");

// We suppose we don't have an interconnect or a vision module until we find it
m_is_interconnect_module_present = false;
m_is_vision_module_present = false;

// Loop through the devices to find the device ID's
// Loop through the devices to find the device ID's
auto devices = m_device_manager->ReadAllDevices();
for (auto device : devices.device_handle())
{
Expand Down Expand Up @@ -415,26 +442,53 @@ void KortexArmDriver::initRosServices()
{
ROS_INFO("-------------------------------------------------");
ROS_INFO("Initializing Kortex Driver's services...");
m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms);
m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms);
m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms);
m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms);
m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms);
if (m_is_interconnect_module_present)
{
m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms);
}
else
{
m_interconnect_config_ros_services = nullptr;
}
if (m_is_vision_module_present)
{
m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms);
if (m_is_real_robot)
{
m_actuator_config_ros_services = new ActuatorConfigRobotServices(m_node_handle, m_actuator_config, 0, m_api_rpc_timeout_ms);
m_base_ros_services = new BaseRobotServices(m_node_handle, m_base, 0, m_api_rpc_timeout_ms);
m_control_config_ros_services = new ControlConfigRobotServices(m_node_handle, m_control_config, 0, m_api_rpc_timeout_ms);
m_device_config_ros_services = new DeviceConfigRobotServices(m_node_handle, m_device_config, 0, m_api_rpc_timeout_ms);
m_device_manager_ros_services = new DeviceManagerRobotServices(m_node_handle, m_device_manager, 0, m_api_rpc_timeout_ms);
if (m_is_interconnect_module_present)
{
m_interconnect_config_ros_services = new InterconnectConfigRobotServices(m_node_handle, m_interconnect_config, m_interconnect_device_id, m_api_rpc_timeout_ms);
}
else
{
m_interconnect_config_ros_services = nullptr;
}
if (m_is_vision_module_present)
{
m_vision_config_ros_services = new VisionConfigRobotServices(m_node_handle, m_vision_config, m_vision_device_id, m_api_rpc_timeout_ms);
}
else
{
m_vision_config_ros_services = nullptr;
}
}
else
{
m_vision_config_ros_services = nullptr;
m_actuator_config_ros_services = new ActuatorConfigSimulationServices(m_node_handle);
m_base_ros_services = new BaseSimulationServices(m_node_handle);
m_control_config_ros_services = new ControlConfigSimulationServices(m_node_handle);
m_device_config_ros_services = new DeviceConfigSimulationServices(m_node_handle);
m_device_manager_ros_services = new DeviceManagerSimulationServices(m_node_handle);
if (m_is_interconnect_module_present)
{
m_interconnect_config_ros_services = new InterconnectConfigSimulationServices(m_node_handle);
}
else
{
m_interconnect_config_ros_services = nullptr;
}
if (m_is_vision_module_present)
{
m_vision_config_ros_services = new VisionConfigSimulationServices(m_node_handle);
}
else
{
m_vision_config_ros_services = nullptr;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
ROS_INFO("Kortex Driver's services initialized correctly.");
Expand Down Expand Up @@ -492,7 +546,7 @@ void KortexArmDriver::setAngularTrajectorySoftLimitsToMax()
}
}

void KortexArmDriver::publishFeedback()
void KortexArmDriver::publishRobotFeedback()
{

Kinova::Api::BaseCyclic::Feedback feedback_from_api;
Expand Down
Loading

0 comments on commit 382909f

Please sign in to comment.