Skip to content

Commit

Permalink
Implemented thrusters state monitoring. Updated build configuration.
Browse files Browse the repository at this point in the history
  • Loading branch information
@patrykcieslak
patrykcieslak committed Sep 22, 2020
1 parent a06ad04 commit e777bf6
Show file tree
Hide file tree
Showing 5 changed files with 106 additions and 70 deletions.
25 changes: 16 additions & 9 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,25 +6,31 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
std_msgs
std_srvs
roscpp
roslib
tf
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)
Expand All @@ -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(
Expand All @@ -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)
5 changes: 5 additions & 0 deletions msg/ThrusterState.msg
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
Header header
float64[] setpoint
float64[] rpm
float64[] thrust
float64[] torque
45 changes: 15 additions & 30 deletions package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,37 +1,22 @@
<?xml version="1.0"?>
<package>
<package format="2">
<name>stonefish_ros</name>
<version>1.0.0</version>
<version>1.1.0</version>
<description>The stonefish_ros package</description>
<author email="[email protected]">Patryk Cieslak</author>
<maintainer email="[email protected]">Patryk Cieslak</maintainer>
<license>GPL-3.0</license>

<author email="[email protected]">Patryk Cieslak</author>

<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>cola2_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf_conversions</build_depend>
<build_depend>Bullet</build_depend>
<build_depend>Stonefish</build_depend>

<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>cola2_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>tf</run_depend>
<run_depend>tf_conversions</run_depend>
<run_depend>Bullet</run_depend>
<run_depend>Stonefish</run_depend>
<export>
</export>
</package>
<depend>roscpp</depend>
<depend>roslib</depend>
<depend>cola2_msgs</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tf</depend>
<depend>tf_conversions</depend>
<depend>Stonefish</depend>
</package>
8 changes: 6 additions & 2 deletions src/ROSScenarioParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@
#include <visualization_msgs/MarkerArray.h>
#include <cola2_msgs/DVL.h>
#include <cola2_msgs/Setpoints.h>
#include <stonefish_ros/ThrusterState.h>

#include <ros/package.h>

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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<stonefish_ros::ThrusterState>(std::string(topicThrust), 10);

if(nServos > 0 && item->QueryStringAttribute("servos", &topicSrv) == XML_SUCCESS)
pubs[robot->getName() + "/servos"] = nh.advertise<sensor_msgs::JointState>(std::string(topicSrv), 10);
}
Expand Down
93 changes: 64 additions & 29 deletions src/ROSSimulationManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <Stonefish/core/Robot.h>
#include <Stonefish/sensors/scalar/Pressure.h>
Expand Down Expand Up @@ -209,40 +210,75 @@ void ROSSimulationManager::SimulationStepCompleted(Scalar timeStep)
//////////////////////////////////////SERVOS(JOINTS)/////////////////////////////////////////
for(size_t i=0; i<rosRobots.size(); ++i)
{
if(rosRobots[i]->servoSetpoints.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//////////////////////////////////////////
Expand All @@ -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:
Expand Down

0 comments on commit e777bf6

Please sign in to comment.