Skip to content

Commit

Permalink
Merge pull request #56 from sonia-auv/develop
Browse files Browse the repository at this point in the history 
Develop
  • Loading branch information
@supertoto29
supertoto29 authored Aug 25, 2022
2 parents 9a0283b + 450d510 commit 72a1257
Show file tree
Hide file tree
Showing 5 changed files with 148 additions and 216 deletions.
3 changes: 3 additions & 0 deletions .github/workflows/docker-image-perception-develop.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,4 +81,7 @@ jobs:
- name: Push Image to Github Packages Registry
run: |
docker push --all-tags ${GITHUB_REMOTE_URL}/${IMAGE_NAME}
- name: Start a cronjob
run: |
curl -XPOST -u "${{ secrets.WORKFLOW_USERNAME}}:${{secrets.WORKFLOW_TOKEN}}" -H "Accept: application/vnd.github.everest-preview+json" -H "Content-Type: application/json" https://api.github.com/repos/sonia-auv/provider_actuators/dispatches --data '{"event_type": "build_application"}'
2 changes: 1 addition & 1 deletion package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>provider_power</name>
<version>2.0.3</version>
<version>2.0.5</version>
<description>The provider_provider package</description>

<maintainer email="francisalonzo29@gmail.com">Francis Alonzo</maintainer>
Expand Down
206 changes: 124 additions & 82 deletions src/provider_power/provider_power_node.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
* along with S.O.N.I.A. software. If not, see <http://www.gnu.org/licenses/>.
*/

#define TIME_BETWEEN_FLUSH 15 // seconds

#include "provider_power_node.h"

namespace provider_power {
Expand Down Expand Up @@ -81,7 +83,7 @@ namespace provider_power {
// M E T H O D S E C T I O N

void ProviderPowerNode::Spin(){
ros::Rate r(5); // 5 hz
ros::Rate r(RATE_HZ); // 5 hz

while(ros::ok())
{
Expand All @@ -99,6 +101,8 @@ namespace provider_power {
switch (receivedData->cmd)
{
case sonia_common::SendRS485Msg::CMD_VOLTAGE:
{
std::unique_lock<std::mutex> mlock(voltageMutex);
switch (receivedData->slave)
{
case sonia_common::SendRS485Msg::SLAVE_PSU0:
Expand All @@ -109,7 +113,7 @@ namespace provider_power {
}
else
{
parsedQueueVoltageSlave0.push_back(msg);
parsedQueueVoltageSlave0 = msg;
}
break;
case sonia_common::SendRS485Msg::SLAVE_PSU1:
Expand All @@ -120,7 +124,7 @@ namespace provider_power {
}
else
{
parsedQueueVoltageSlave1.push_back(msg);
parsedQueueVoltageSlave1 = msg;
}
break;
case sonia_common::SendRS485Msg::SLAVE_PSU2:
Expand All @@ -131,7 +135,7 @@ namespace provider_power {
}
else
{
parsedQueueVoltageSlave2.push_back(msg);
parsedQueueVoltageSlave2 = msg;
}
break;
case sonia_common::SendRS485Msg::SLAVE_PSU3:
Expand All @@ -142,15 +146,18 @@ namespace provider_power {
}
else
{
parsedQueueVoltageSlave3.push_back(msg);
parsedQueueVoltageSlave3 = msg;
}
break;
default:
ROS_WARN_STREAM("Unknown SLAVE to provider_power");
break;
}
}
break;
case sonia_common::SendRS485Msg::CMD_CURRENT:
{
std::unique_lock<std::mutex> mlock(currentMutex);
switch (receivedData->slave)
{
case sonia_common::SendRS485Msg::SLAVE_PSU0:
Expand All @@ -161,7 +168,7 @@ namespace provider_power {
}
else
{
parsedQueueCurrentSlave0.push_back(msg);
parsedQueueCurrentSlave0 = msg;
}
break;
case sonia_common::SendRS485Msg::SLAVE_PSU1:
Expand All @@ -172,7 +179,7 @@ namespace provider_power {
}
else
{
parsedQueueCurrentSlave1.push_back(msg);
parsedQueueCurrentSlave1 = msg;
}
break;
case sonia_common::SendRS485Msg::SLAVE_PSU2:
Expand All @@ -183,7 +190,7 @@ namespace provider_power {
}
else
{
parsedQueueCurrentSlave2.push_back(msg);
parsedQueueCurrentSlave2 = msg;
}
break;
case sonia_common::SendRS485Msg::SLAVE_PSU3:
Expand All @@ -194,33 +201,39 @@ namespace provider_power {
}
else
{
parsedQueueCurrentSlave3.push_back(msg);
parsedQueueCurrentSlave3 = msg;
}
break;
default:
ROS_WARN_STREAM("Unknown SLAVE to provider_power");
break;
}
}
break;
case sonia_common::SendRS485Msg::CMD_READ_MOTOR:
{
std::unique_lock<std::mutex> mlock(motorMutex);
switch (receivedData->slave)
{
case sonia_common::SendRS485Msg::SLAVE_PSU0:
readQueueMotorSlave0.push_back(receivedData->data);
readQueueMotorSlave0 = receivedData->data;
break;
case sonia_common::SendRS485Msg::SLAVE_PSU1:
readQueueMotorSlave1.push_back(receivedData->data);
readQueueMotorSlave1 = receivedData->data;
break;
case sonia_common::SendRS485Msg::SLAVE_PSU2:
readQueueMotorSlave2.push_back(receivedData->data);
readQueueMotorSlave2 = receivedData->data;
break;
case sonia_common::SendRS485Msg::SLAVE_PSU3:
readQueueMotorSlave3.push_back(receivedData->data);
readQueueMotorSlave3 = receivedData->data;
break;
default:
ROS_WARN_STREAM("Unknown SLAVE to provider_power");
break;
}
}
break;
case sonia_common::SendRS485Msg::CMD_KEEP_ALIVE:
break;
default:
ROS_WARN_STREAM("Unknow CMD to provider_power");
Expand All @@ -243,8 +256,10 @@ namespace provider_power {
case sonia_common::SendRS485Msg::CMD_READ_MOTOR:
ReadMotorCMD(receivedData->data, nb_motor);
break;
case sonia_common::SendRS485Msg::CMD_KEEP_ALIVE:
break;
default:
//ROS_WARN_STREAM("Unknow CMD to provider_power");
ROS_WARN_STREAM("Unknow CMD to provider_power");
break;
}
}
Expand Down Expand Up @@ -368,98 +383,125 @@ namespace provider_power {

void ProviderPowerNode::writeVoltageData()
{
ros::Rate r(RATE_HZ_MESSAGE);

while(!ros::isShuttingDown())
{
ros::Duration(0.1).sleep();
while(!parsedQueueVoltageSlave0.empty() && !parsedQueueVoltageSlave1.empty() && !parsedQueueVoltageSlave2.empty() && !parsedQueueVoltageSlave3.empty())
{
std_msgs::Float64MultiArray msg_16V;
std_msgs::Float64MultiArray msg_12V;

std::vector<double> msg_slave0 = parsedQueueVoltageSlave0.get_n_pop_front();
std::vector<double> msg_slave1 = parsedQueueVoltageSlave1.get_n_pop_front();
std::vector<double> msg_slave2 = parsedQueueVoltageSlave2.get_n_pop_front();
std::vector<double> msg_slave3 = parsedQueueVoltageSlave3.get_n_pop_front();

// Motors Voltage
msg_16V.data.push_back(msg_slave0[0]);
msg_16V.data.push_back(msg_slave1[0]);
msg_16V.data.push_back(msg_slave2[0]);
msg_16V.data.push_back(msg_slave3[0]);
msg_16V.data.push_back(msg_slave0[1]);
msg_16V.data.push_back(msg_slave1[1]);
msg_16V.data.push_back(msg_slave2[1]);
msg_16V.data.push_back(msg_slave3[1]);

// Batteries Voltage
msg_16V.data.push_back((msg_slave0[3]+msg_slave1[3])/2);
msg_16V.data.push_back((msg_slave2[3]+msg_slave3[3])/2);

// 12V Voltage
msg_12V.data.push_back(msg_slave0[2]);
msg_12V.data.push_back(msg_slave1[2]);
msg_12V.data.push_back(msg_slave2[2]);
msg_12V.data.push_back(msg_slave3[2]);

voltage16V_publisher_.publish(msg_16V);
voltage12V_publisher_.publish(msg_12V);
std_msgs::Float64MultiArray msg_16V;
std_msgs::Float64MultiArray msg_12V;

std::unique_lock<std::mutex> mlock(voltageMutex);

std::vector<double> msg_slave0 = parsedQueueVoltageSlave0;
std::vector<double> msg_slave1 = parsedQueueVoltageSlave1;
std::vector<double> msg_slave2 = parsedQueueVoltageSlave2;
std::vector<double> msg_slave3 = parsedQueueVoltageSlave3;

mlock.unlock();

if(msg_slave0.empty() || msg_slave1.empty() || msg_slave2.empty() || msg_slave3.empty())
{
r.sleep();
continue;
}

// Motors Voltage
msg_16V.data.push_back(msg_slave0[0]);
msg_16V.data.push_back(msg_slave1[0]);
msg_16V.data.push_back(msg_slave2[0]);
msg_16V.data.push_back(msg_slave3[0]);
msg_16V.data.push_back(msg_slave0[1]);
msg_16V.data.push_back(msg_slave1[1]);
msg_16V.data.push_back(msg_slave2[1]);
msg_16V.data.push_back(msg_slave3[1]);

// Batteries Voltage
msg_16V.data.push_back((msg_slave0[3]+msg_slave1[3])/2);
msg_16V.data.push_back((msg_slave2[3]+msg_slave3[3])/2);

// 12V Voltage
msg_12V.data.push_back(msg_slave0[2]);
msg_12V.data.push_back(msg_slave1[2]);
msg_12V.data.push_back(msg_slave2[2]);
msg_12V.data.push_back(msg_slave3[2]);

voltage16V_publisher_.publish(msg_16V);
voltage12V_publisher_.publish(msg_12V);

r.sleep();
}
}

void ProviderPowerNode::writeCurrentData()
{
ros::Rate r(RATE_HZ_MESSAGE);
while(!ros::isShuttingDown())
{
ros::Duration(0.1).sleep();
while(!parsedQueueCurrentSlave0.empty() && !parsedQueueCurrentSlave1.empty() && !parsedQueueCurrentSlave2.empty() && !parsedQueueCurrentSlave3.empty())
{
std_msgs::Float64MultiArray msg;
{
std_msgs::Float64MultiArray msg;

std::vector<double> msg_slave0 = parsedQueueCurrentSlave0.get_n_pop_front();
std::vector<double> msg_slave1 = parsedQueueCurrentSlave1.get_n_pop_front();
std::vector<double> msg_slave2 = parsedQueueCurrentSlave2.get_n_pop_front();
std::vector<double> msg_slave3 = parsedQueueCurrentSlave3.get_n_pop_front();
std::unique_lock<std::mutex> mlock(currentMutex);

for (int i = 0; i <= 2; i++)
{
msg.data.push_back(msg_slave0[i]);
msg.data.push_back(msg_slave1[i]);
msg.data.push_back(msg_slave2[i]);
msg.data.push_back(msg_slave3[i]);
}
std::vector<double> msg_slave0 = parsedQueueCurrentSlave0;
std::vector<double> msg_slave1 = parsedQueueCurrentSlave1;
std::vector<double> msg_slave2 = parsedQueueCurrentSlave2;
std::vector<double> msg_slave3 = parsedQueueCurrentSlave3;

mlock.unlock();

current_publisher_.publish(msg);
if(msg_slave0.empty() || msg_slave1.empty() || msg_slave2.empty() || msg_slave3.empty())
{
r.sleep();
continue;
}

for (int i = 0; i <= 2; i++)
{
msg.data.push_back(msg_slave0[i]);
msg.data.push_back(msg_slave1[i]);
msg.data.push_back(msg_slave2[i]);
msg.data.push_back(msg_slave3[i]);
}

current_publisher_.publish(msg);

r.sleep();
}
}

void ProviderPowerNode::writeMotorData()
{
while(!ros::isShuttingDown())
ros::Rate r(RATE_HZ_MESSAGE);

while(!ros::isShuttingDown())
{
ros::Duration(0.1).sleep();
while(!readQueueMotorSlave0.empty() && !readQueueMotorSlave1.empty() && !readQueueMotorSlave2.empty() && !readQueueMotorSlave3.empty())
{
std_msgs::UInt8MultiArray msg;
std_msgs::UInt8MultiArray msg;

std::vector<uint8_t> msg_slave0 = readQueueMotorSlave0.get_n_pop_front();
std::vector<uint8_t> msg_slave1 = readQueueMotorSlave1.get_n_pop_front();
std::vector<uint8_t> msg_slave2 = readQueueMotorSlave2.get_n_pop_front();
std::vector<uint8_t> msg_slave3 = readQueueMotorSlave3.get_n_pop_front();
std::unique_lock<std::mutex> mlock(motorMutex);

for (int i = 0; i < 2; i++)
{
msg.data.push_back(msg_slave0[i]);
msg.data.push_back(msg_slave1[i]);
msg.data.push_back(msg_slave2[i]);
msg.data.push_back(msg_slave3[i]);
}
std::vector<uint8_t> msg_slave0 = readQueueMotorSlave0;
std::vector<uint8_t> msg_slave1 = readQueueMotorSlave1;
std::vector<uint8_t> msg_slave2 = readQueueMotorSlave2;
std::vector<uint8_t> msg_slave3 = readQueueMotorSlave3;

mlock.unlock();

if(msg_slave0.empty() || msg_slave1.empty() || msg_slave2.empty() || msg_slave3.empty())
{
r.sleep();
continue;
}

motor_publisher_.publish(msg);
for (int i = 0; i < 2; i++)
{
msg.data.push_back(msg_slave0[i]);
msg.data.push_back(msg_slave1[i]);
msg.data.push_back(msg_slave2[i]);
msg.data.push_back(msg_slave3[i]);
}

motor_publisher_.publish(msg);

r.sleep();
}
}

}
Loading

0 comments on commit 72a1257

Please sign in to comment.