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 335a8cb0..5249fe33 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 @@ -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; @@ -157,7 +160,7 @@ class KortexArmDriver // Private methods bool isGripperPresent(); void setAngularTrajectorySoftLimitsToMax(); - void publishFeedback(); + void publishRobotFeedback(); }; #endif diff --git a/kortex_driver/launch/kortex_driver.launch b/kortex_driver/launch/kortex_driver.launch index 3d6fe61d..113eaea3 100644 --- a/kortex_driver/launch/kortex_driver.launch +++ b/kortex_driver/launch/kortex_driver.launch @@ -56,6 +56,7 @@ + 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 5f57e83e..1ead99b9 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -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("base_feedback", 1000); m_pub_joint_state = m_node_handle.advertise("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); @@ -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; @@ -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..."; @@ -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()) { @@ -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."); @@ -492,7 +546,7 @@ void KortexArmDriver::setAngularTrajectorySoftLimitsToMax() } } -void KortexArmDriver::publishFeedback() +void KortexArmDriver::publishRobotFeedback() { Kinova::Api::BaseCyclic::Feedback feedback_from_api; diff --git a/kortex_gazebo/launch/spawn_kortex_robot.launch b/kortex_gazebo/launch/spawn_kortex_robot.launch index 646f42c8..57dc1cf3 100644 --- a/kortex_gazebo/launch/spawn_kortex_robot.launch +++ b/kortex_gazebo/launch/spawn_kortex_robot.launch @@ -18,6 +18,8 @@ + + @@ -119,6 +121,19 @@ + + + + + + + + + + + + + diff --git a/kortex_gazebo/launch/test.launch b/kortex_gazebo/launch/test.launch new file mode 100644 index 00000000..26999bcd --- /dev/null +++ b/kortex_gazebo/launch/test.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kortex_gazebo/package.xml b/kortex_gazebo/package.xml index 100ca5da..060b1d94 100644 --- a/kortex_gazebo/package.xml +++ b/kortex_gazebo/package.xml @@ -10,6 +10,7 @@ BSD catkin roslaunch + kortex_driver robot_state_publisher rviz joint_state_publisher