diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ea001c..633f888 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs sensor_msgs + std_msgs std_srvs roscpp roslib @@ -13,18 +14,23 @@ find_package(catkin REQUIRED COMPONENTS tf_conversions ) -find_package(Bullet REQUIRED) find_package(PkgConfig REQUIRED) -pkg_check_modules(Stonefish REQUIRED stonefish>=0.9.0) +pkg_check_modules(Stonefish REQUIRED stonefish>=1.1.0) + +add_message_files( + FILES + ThrusterState.msg +) add_service_files( - FILES - SonarSettings.srv - SonarSettings2.srv + FILES + SonarSettings.srv + SonarSettings2.srv ) generate_messages( DEPENDENCIES + std_msgs ) set(CMAKE_CXX_STANDARD 14) @@ -36,13 +42,14 @@ catkin_package( CATKIN_DEPENDS cola2_msgs geometry_msgs nav_msgs - sensor_msgs + sensor_msgs + std_msgs std_srvs roscpp roslib tf tf_conversions - DEPENDS BULLET Stonefish + DEPENDS Stonefish ) include_directories( @@ -57,8 +64,8 @@ src/ROSInterface.cpp src/ROSSimulationManager.cpp src/ROSScenarioParser.cpp ) -target_link_libraries(stonefish_ros ${catkin_LIBRARIES} ${BULLET_LIBRARIES} ${Stonefish_LIBRARIES}) +target_link_libraries(stonefish_ros ${catkin_LIBRARIES} ${Stonefish_LIBRARIES}) #Universal simulator with XML parser add_executable(parsed_simulator src/parsed_simulator.cpp) -target_link_libraries(parsed_simulator ${catkin_LIBRARIES} ${BULLET_LIBRARIES} ${Stonefish_LIBRARIES} stonefish_ros) +target_link_libraries(parsed_simulator ${catkin_LIBRARIES} ${Stonefish_LIBRARIES} stonefish_ros) \ No newline at end of file diff --git a/msg/ThrusterState.msg b/msg/ThrusterState.msg new file mode 100644 index 0000000..254dacf --- /dev/null +++ b/msg/ThrusterState.msg @@ -0,0 +1,5 @@ +Header header +float64[] setpoint +float64[] rpm +float64[] thrust +float64[] torque \ No newline at end of file diff --git a/package.xml b/package.xml index 70a1c6f..7204d72 100644 --- a/package.xml +++ b/package.xml @@ -1,37 +1,22 @@ - + stonefish_ros - 1.0.0 + 1.1.0 The stonefish_ros package + Patryk Cieslak Patryk Cieslak GPL-3.0 - Patryk Cieslak - catkin - roscpp - roslib - cola2_msgs - geometry_msgs - nav_msgs - sensor_msgs - std_srvs - tf - tf_conversions - Bullet - Stonefish - - roscpp - roslib - cola2_msgs - geometry_msgs - nav_msgs - sensor_msgs - std_srvs - tf - tf_conversions - Bullet - Stonefish - - - + roscpp + roslib + cola2_msgs + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + std_srvs + tf + tf_conversions + Stonefish + \ No newline at end of file diff --git a/src/ROSScenarioParser.cpp b/src/ROSScenarioParser.cpp index f46118b..4c041d8 100644 --- a/src/ROSScenarioParser.cpp +++ b/src/ROSScenarioParser.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include @@ -108,9 +109,8 @@ std::string ROSScenarioParser::SubstituteROSVars(const std::string& value) } replacedValue += param; } - else + else //Command unsupported { - ROS_ERROR("Scenario parser(ROS): substitution command '%s' not currently supported!", results[0].c_str()); return value; } @@ -243,8 +243,12 @@ bool ROSScenarioParser::ParseRobot(XMLElement* element) //Generate publishers if((item = element->FirstChildElement("ros_publisher")) != nullptr) { + const char* topicThrust = nullptr; const char* topicSrv = nullptr; + if(nThrusters > 0 && item->QueryStringAttribute("thrusters", &topicThrust) == XML_SUCCESS) + pubs[robot->getName() + "/thrusters"] = nh.advertise(std::string(topicThrust), 10); + if(nServos > 0 && item->QueryStringAttribute("servos", &topicSrv) == XML_SUCCESS) pubs[robot->getName() + "/servos"] = nh.advertise(std::string(topicSrv), 10); } diff --git a/src/ROSSimulationManager.cpp b/src/ROSSimulationManager.cpp index 36dd85a..81fc7fb 100644 --- a/src/ROSSimulationManager.cpp +++ b/src/ROSSimulationManager.cpp @@ -26,6 +26,7 @@ #include "stonefish_ros/ROSSimulationManager.h" #include "stonefish_ros/ROSScenarioParser.h" #include "stonefish_ros/ROSInterface.h" +#include "stonefish_ros/ThrusterState.h" #include #include @@ -209,40 +210,75 @@ void ROSSimulationManager::SimulationStepCompleted(Scalar timeStep) //////////////////////////////////////SERVOS(JOINTS)///////////////////////////////////////// for(size_t i=0; iservoSetpoints.size() == 0 - || pubs.find(rosRobots[i]->robot->getName() + "/servos") == pubs.end()) - continue; - - unsigned int aID = 0; - unsigned int sID = 0; - Actuator* actuator; - Servo* srv; + if(rosRobots[i]->servoSetpoints.size() != 0 + && pubs.find(rosRobots[i]->robot->getName() + "/servos") != pubs.end()) + { + unsigned int aID = 0; + unsigned int sID = 0; + Actuator* actuator; + Servo* srv; + + sensor_msgs::JointState msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = rosRobots[i]->robot->getName(); + msg.name.resize(rosRobots[i]->servoSetpoints.size()); + msg.position.resize(rosRobots[i]->servoSetpoints.size()); + msg.velocity.resize(rosRobots[i]->servoSetpoints.size()); + msg.effort.resize(rosRobots[i]->servoSetpoints.size()); + + while((actuator = rosRobots[i]->robot->getActuator(aID++)) != NULL) + { + if(actuator->getType() == ActuatorType::SERVO) + { + srv = (Servo*)actuator; + msg.name[sID] = srv->getJointName(); + msg.position[sID] = srv->getPosition(); + msg.velocity[sID] = srv->getVelocity(); + msg.effort[sID] = srv->getEffort(); + ++sID; + + if(sID == rosRobots[i]->servoSetpoints.size()) + break; + } + } - sensor_msgs::JointState msg; - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = rosRobots[i]->robot->getName(); - msg.name.resize(rosRobots[i]->servoSetpoints.size()); - msg.position.resize(rosRobots[i]->servoSetpoints.size()); - msg.velocity.resize(rosRobots[i]->servoSetpoints.size()); - msg.effort.resize(rosRobots[i]->servoSetpoints.size()); + pubs[rosRobots[i]->robot->getName() + "/servos"].publish(msg); + } - while((actuator = rosRobots[i]->robot->getActuator(aID++)) != NULL) + if(rosRobots[i]->thrusterSetpoints.size() != 0 + && pubs.find(rosRobots[i]->robot->getName() + "/thrusters") != pubs.end()) { - if(actuator->getType() == ActuatorType::SERVO) + unsigned int aID = 0; + unsigned int thID = 0; + Actuator* actuator; + Thruster* th; + + stonefish_ros::ThrusterState msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = rosRobots[i]->robot->getName(); + msg.setpoint.resize(rosRobots[i]->thrusterSetpoints.size()); + msg.rpm.resize(rosRobots[i]->thrusterSetpoints.size()); + msg.thrust.resize(rosRobots[i]->thrusterSetpoints.size()); + msg.torque.resize(rosRobots[i]->thrusterSetpoints.size()); + + while((actuator = rosRobots[i]->robot->getActuator(aID++)) != NULL) { - srv = (Servo*)actuator; - msg.name[sID] = srv->getJointName(); - msg.position[sID] = srv->getPosition(); - msg.velocity[sID] = srv->getVelocity(); - msg.effort[sID] = srv->getEffort(); - ++sID; - - if(sID == rosRobots[i]->servoSetpoints.size()) - break; + if(actuator->getType() == ActuatorType::THRUSTER) + { + th = (Thruster*)actuator; + msg.setpoint[thID] = th->getSetpoint(); + msg.rpm[thID] = th->getOmega()/(Scalar(2)*M_PI)*Scalar(60); + msg.thrust[thID] = th->getThrust(); + msg.torque[thID] = th->getTorque(); + ++thID; + + if(thID == rosRobots[i]->thrusterSetpoints.size()) + break; + } } - } - pubs[rosRobots[i]->robot->getName() + "/servos"].publish(msg); + pubs[rosRobots[i]->robot->getName() + "/thrusters"].publish(msg); + } } //////////////////////////////////////////////ACTUATORS////////////////////////////////////////// @@ -259,7 +295,6 @@ void ROSSimulationManager::SimulationStepCompleted(Scalar timeStep) { case ActuatorType::THRUSTER: ((Thruster*)actuator)->setSetpoint(rosRobots[i]->thrusterSetpoints[thID++]); - //ROS_INFO("[Thruster %d] Setpoint: %1.3lf Omega: %1.3lf Thrust: %1.3lf", thID, ((Thruster*)actuator)->getSetpoint(), ((Thruster*)actuator)->getOmega(), ((Thruster*)actuator)->getThrust()); break; case ActuatorType::PROPELLER: