Commit a183f051 authored by Christian Sponfeldner's avatar Christian Sponfeldner

refactoring: format all files according roscpp style guide

parent 768004d6
......@@ -10,7 +10,6 @@
#include "emros_common/kinematics_kind_configuration.h"
#include "auxiliary_drive/CommandHoming.h"
class AuxiliaryDrive
{
public:
......@@ -20,19 +19,19 @@ public:
private:
void ProcessTargetPositionCB(std_msgs::Float64::ConstPtr msg);
void DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr& msg);
bool Home17ToNegativeLimitSwitchCB(auxiliary_drive::CommandHoming::Request &req,
auxiliary_drive::CommandHoming::Response &res);
bool Home17ToNegativeLimitSwitchCB(auxiliary_drive::CommandHoming::Request& req,
auxiliary_drive::CommandHoming::Response& res);
bool Home18ToPositiveLimitSwitchCB(auxiliary_drive::CommandHoming::Request &req,
auxiliary_drive::CommandHoming::Response &res);
bool Home18ToPositiveLimitSwitchCB(auxiliary_drive::CommandHoming::Request& req,
auxiliary_drive::CommandHoming::Response& res);
bool Home33ToNegativeIndexCB(auxiliary_drive::CommandHoming::Request &req,
auxiliary_drive::CommandHoming::Response &res);
bool Home33ToNegativeIndexCB(auxiliary_drive::CommandHoming::Request& req,
auxiliary_drive::CommandHoming::Response& res);
bool Home34ToPositiveIndexCB(auxiliary_drive::CommandHoming::Request &req,
auxiliary_drive::CommandHoming::Response &res);
bool Home35ToCurrentPositionCB(auxiliary_drive::CommandHoming::Request &req,
auxiliary_drive::CommandHoming::Response &res);
bool Home34ToPositiveIndexCB(auxiliary_drive::CommandHoming::Request& req,
auxiliary_drive::CommandHoming::Response& res);
bool Home35ToCurrentPositionCB(auxiliary_drive::CommandHoming::Request& req,
auxiliary_drive::CommandHoming::Response& res);
void SendHomingCommand(Ethprotocol::Cmd homing_method);
void PublishActualPosition(const emros_dispatcher::eth::ConstPtr& msg);
EMROSCmdType::AuxiliaryController::TargetPositionsInterface* getTargetPositionsInterface();
......@@ -53,8 +52,6 @@ private:
const Robot::KindOfKinematics mKindOfKinematics;
const emros_dispatcher::eth::_id_type mID_auxiliary_device = EMROSCmdType::AuxiliaryController::DEVICE_ID;
};
#endif // AUXILIARY_DRIVE_HPP
#endif // AUXILIARY_DRIVE_HPP
This diff is collapsed.
......@@ -4,15 +4,10 @@
#include "ros/ros.h"
#include <iostream>
int main(int argc, char **argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "auxiliary_drive");
AuxiliaryDrive auxiliary_drive;
ros::spin();
return 0;
}
......@@ -21,14 +21,8 @@
#include "emros_common/attributes.h"
#include <cmath>
using namespace EMROSCmdType::Basecontroller;
class Basecontroller
{
public:
......@@ -36,7 +30,7 @@ public:
{
double x;
double y;
double z; //Workaround for auxiliary drive
double z; // Workaround for auxiliary drive
double phiz;
};
......@@ -68,7 +62,7 @@ private:
ros::Publisher mPub_twist_rx;
ros::Timer mLeakyBucketTimer;
Kinematics *mKinematic;
Kinematics* mKinematic;
Position mPosition;
Config mBaseConfig;
......@@ -76,8 +70,6 @@ private:
geometry_msgs::Twist mTarget_vel;
geometry_msgs::Twist mLast_vel;
boost::mutex mMutex;
EMROSCmdType::AllMotorsController::ActualVelocitiesInterface* mActual_vel;
EMROSCmdType::AllMotorsController::ActualVelocitiesInterface* mActual_vel;
const emros_dispatcher::eth::_id_type mID;
};
......@@ -19,9 +19,11 @@
using namespace EMROSCmdType::Basecontroller;
enum class KinematicMode {
MECANUM,DIFFERENTIAL, DIFFERENTIAL_PLUS_AUXILIARY
enum class KinematicMode
{
MECANUM,
DIFFERENTIAL,
DIFFERENTIAL_PLUS_AUXILIARY
};
struct Config
......@@ -45,9 +47,9 @@ public:
virtual ~Kinematics()
{
}
virtual void fwdKin(geometry_msgs::Twist& twist, EMROSCmdType::AllMotorsController::ActualVelocitiesInterface& vel)=0;
virtual void invKin(TargetVelocitiesInterface& wheel_vel, geometry_msgs::Twist& twist)=0;
virtual void fwdKin(geometry_msgs::Twist& twist,
EMROSCmdType::AllMotorsController::ActualVelocitiesInterface& vel) = 0;
virtual void invKin(TargetVelocitiesInterface& wheel_vel, geometry_msgs::Twist& twist) = 0;
};
///////////////////CONCRETE CLASSES///////////////////////////////////////////////
......@@ -55,10 +57,8 @@ public:
class MecanumKinematics : public Kinematics
{
public:
MecanumKinematics(Config config) :
mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
MecanumKinematics(Config config) : mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
{
}
~MecanumKinematics() override
{
......@@ -74,11 +74,10 @@ private:
//////////////////////////////////////////////////////////////////////////////////
class DifferentialKinematics: public Kinematics
class DifferentialKinematics : public Kinematics
{
public:
DifferentialKinematics(Config config) :
mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
DifferentialKinematics(Config config) : mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
{
}
~DifferentialKinematics() override
......@@ -92,16 +91,14 @@ private:
Config mConfig;
const double mWheel_radius;
};
//////////////////////////////////////////////////////////////////////////////////
class DifferentialPlusAuxiliaryKinematics: public Kinematics
class DifferentialPlusAuxiliaryKinematics : public Kinematics
{
public:
DifferentialPlusAuxiliaryKinematics(Config config) :
mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
DifferentialPlusAuxiliaryKinematics(Config config) : mConfig(config), mWheel_radius(mConfig.wheel_dia / 2)
{
}
~DifferentialPlusAuxiliaryKinematics() override
......@@ -115,7 +112,6 @@ private:
Config mConfig;
const double mWheel_radius;
};
#endif /* EMROS_BASECONTROLLER_INCLUDE_EMROS_BASECONTROLLER_KINEMATICS_HPP_ */
......@@ -5,17 +5,14 @@
* Author: speckle_s
*/
#include "emros_basecontroller/basecontroller.hpp"
#include "std_msgs/Int32.h"
namespace AllMotorsNS = EMROSCmdType::AllMotorsController;
Basecontroller::Basecontroller() :
mLeakyBucketTimeout(ros::Duration(0.3), 48, 6, 3, ros::Time::now), mPosition(), mTarget_vel(), mID(
DEVICE_ID)
Basecontroller::Basecontroller()
: mLeakyBucketTimeout(ros::Duration(0.3), 48, 6, 3, ros::Time::now), mPosition(), mTarget_vel(), mID(DEVICE_ID)
{
getParam();
switch (mBaseConfig.mode_of_operation)
{
......@@ -40,7 +37,6 @@ Basecontroller::Basecontroller() :
mSub_twist_cmd = mNodeHandler.subscribe("cmd_vel", 1, &Basecontroller::cmdVelCallback, this);
mLeakyBucketTimer = mNodeHandler.createTimer(ros::Duration(0.01), &Basecontroller::LeakyBucketControllLoop, this);
mMainLoopThread = new boost::thread(boost::bind(&Basecontroller::mainLoop, this));
}
Basecontroller::~Basecontroller()
......@@ -51,7 +47,6 @@ Basecontroller::~Basecontroller()
delete mActual_vel;
}
inline TargetVelocitiesInterface* Basecontroller::getTargetVelocitiesInstance()
{
TargetVelocitiesInterface* pReturn;
......@@ -61,7 +56,7 @@ inline TargetVelocitiesInterface* Basecontroller::getTargetVelocitiesInstance()
pReturn = new TargetWheelVelocities2WD;
break;
case KinematicMode::DIFFERENTIAL_PLUS_AUXILIARY:
case KinematicMode::DIFFERENTIAL_PLUS_AUXILIARY:
pReturn = new TargetWheelVelocities2WDplusAuxiliary;
break;
......@@ -103,7 +98,7 @@ void Basecontroller::getParam()
ros::param::param<double>("~/wheel_diameter", mBaseConfig.wheel_dia, 0.10);
ros::param::param<double>("~/controller_frequency", mBaseConfig.controller_frequency, 20.0);
ROS_INFO("Basecontroller started with %s",mode_of_operation.c_str());
ROS_INFO("Basecontroller started with %s", mode_of_operation.c_str());
if (mode_of_operation.compare("DIFFERENTIAL") == 0)
{
......@@ -116,11 +111,13 @@ void Basecontroller::getParam()
else if (mode_of_operation.compare("DIFFERENTIAL_PLUS_AUXILIARY") == 0)
{
mBaseConfig.mode_of_operation = KinematicMode::DIFFERENTIAL_PLUS_AUXILIARY;
ros::param::param<double>("~/hoisting_feed_constant_in_m_per_round", mBaseConfig.hoisting_feed_constant_in_m_per_round, 0.05);
ros::param::param<double>("~/hoisting_feed_constant_in_m_per_round",
mBaseConfig.hoisting_feed_constant_in_m_per_round, 0.05);
}
else
{
ROS_ERROR("Failed to configure Basecontroller, because mode of operation '%s' is unknown",mode_of_operation.c_str());
ROS_ERROR("Failed to configure Basecontroller, because mode of operation '%s' is unknown",
mode_of_operation.c_str());
}
}
......@@ -132,7 +129,7 @@ void Basecontroller::LeakyBucketControllLoop(const ros::TimerEvent& event)
void Basecontroller::mainLoop()
{
ros::Rate rate(mBaseConfig.controller_frequency);
TargetVelocitiesInterface &wheel_vel_Umin { *getTargetVelocitiesInstance()};
TargetVelocitiesInterface& wheel_vel_Umin{ *getTargetVelocitiesInstance() };
while (ros::ok())
{
wheel_vel_Umin.clearData();
......@@ -151,29 +148,27 @@ void Basecontroller::mainLoop()
}
vel_cmd = mTarget_vel;
}
if(!this->checkIfWithinSameRange(mTarget_vel.angular.z,mLast_vel.angular.z,0.00001)
|| !this->checkIfWithinSameRange(mTarget_vel.linear.x,mLast_vel.linear.x,0.00001)
|| !this->checkIfWithinSameRange(mTarget_vel.linear.y,mLast_vel.linear.y,0.00001)
|| !this->checkIfWithinSameRange(mTarget_vel.linear.z,mLast_vel.linear.z,0.00001))
if (!this->checkIfWithinSameRange(mTarget_vel.angular.z, mLast_vel.angular.z, 0.00001) ||
!this->checkIfWithinSameRange(mTarget_vel.linear.x, mLast_vel.linear.x, 0.00001) ||
!this->checkIfWithinSameRange(mTarget_vel.linear.y, mLast_vel.linear.y, 0.00001) ||
!this->checkIfWithinSameRange(mTarget_vel.linear.z, mLast_vel.linear.z, 0.00001))
{
mKinematic->invKin(wheel_vel_Umin, vel_cmd);
// Block one wheel for test purposes ===========
// TargetWheelVelocities2WDplusAuxiliary &velocities_temp = dynamic_cast<TargetWheelVelocities2WDplusAuxiliary&> (wheel_vel_Umin);
// velocities_temp.data().left = 0;
// TargetWheelVelocities2WDplusAuxiliary &velocities_temp =
// dynamic_cast<TargetWheelVelocities2WDplusAuxiliary&> (wheel_vel_Umin); velocities_temp.data().left = 0;
// =============================================
tx_msg.cmd = TARGET_VELOCITIES;
tx_msg.id = mID;
tx_msg.payload.resize(wheel_vel_Umin.getRawSize());
//TODO: error handling
// TODO: error handling
wheel_vel_Umin.get(&tx_msg.payload[0], tx_msg.payload.size());
mPub_sepp_dispatcher_tx.publish(tx_msg);
mLast_vel = mTarget_vel;
mLast_vel = mTarget_vel;
}
rate.sleep();
}
......@@ -182,10 +177,9 @@ void Basecontroller::mainLoop()
bool Basecontroller::checkIfWithinSameRange(double firstValue, double secondValue, double range)
{
return (std::abs(firstValue - secondValue) <= range/2);
return (std::abs(firstValue - secondValue) <= range / 2);
}
void Basecontroller::DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr& msg)
{
if (msg->id == mID)
......@@ -197,43 +191,40 @@ void Basecontroller::DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr&
ROS_WARN("I heard: [%u] this command is unknown", msg->cmd);
break;
}
} else if (msg->id == AllMotorsNS::DEVICE_ID)
}
else if (msg->id == AllMotorsNS::DEVICE_ID)
{
switch (msg->cmd)
{
case AllMotorsNS::ACTUAL_VELOCITIES:
{
ros::Duration dt;
mActual_vel->clearData();
geometry_msgs::Twist twist;
nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odomTf;
parsePayload(*mActual_vel, *msg);
dt = msg->header.stamp - mLastTime;
mLastTime = msg->header.stamp;
mKinematic->fwdKin(twist, *mActual_vel);
odom.header.stamp = msg->header.stamp;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
odomTf.header.stamp = msg->header.stamp;
odomTf.header.frame_id = "odom";
odomTf.child_frame_id = "base_footprint";
calcOdom(odom, twist, dt);
calcOdomTF(odomTf, odom);
mTransformBroadcaster.sendTransform(odomTf);
mPub_odom.publish(odom);
mPub_twist_rx.publish(twist);
break;
}
ros::Duration dt;
mActual_vel->clearData();
geometry_msgs::Twist twist;
nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odomTf;
parsePayload(*mActual_vel, *msg);
dt = msg->header.stamp - mLastTime;
mLastTime = msg->header.stamp;
mKinematic->fwdKin(twist, *mActual_vel);
odom.header.stamp = msg->header.stamp;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
odomTf.header.stamp = msg->header.stamp;
odomTf.header.frame_id = "odom";
odomTf.child_frame_id = "base_footprint";
calcOdom(odom, twist, dt);
calcOdomTF(odomTf, odom);
mTransformBroadcaster.sendTransform(odomTf);
mPub_odom.publish(odom);
mPub_twist_rx.publish(twist);
break;
}
default:
// command is not for this module
break;
}
}
}
void Basecontroller::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
......@@ -247,22 +238,21 @@ void Basecontroller::calcOdom(nav_msgs::Odometry& odom, geometry_msgs::Twist twi
{
mPosition.x += (twist.linear.x * cos(mPosition.phiz) - (twist.linear.y * sin(mPosition.phiz))) * dt.toSec();
mPosition.y += (twist.linear.x * sin(mPosition.phiz) + (twist.linear.y * cos(mPosition.phiz))) * dt.toSec();
mPosition.z += twist.linear.z * dt.toSec(); //Workaround for auxiliary drive
mPosition.z += twist.linear.z * dt.toSec(); // Workaround for auxiliary drive
mPosition.phiz += twist.angular.z * dt.toSec();
odom.pose.pose.position.x = mPosition.x;
odom.pose.pose.position.y = mPosition.y;
odom.pose.pose.position.z = 0;
odom.pose.pose.position.z = mPosition.z; //Workaround for auxiliary drive
odom.pose.pose.position.z = mPosition.z; // Workaround for auxiliary drive
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(mPosition.phiz);
odom.twist.twist.linear.x = twist.linear.x;
odom.twist.twist.linear.y = twist.linear.y;
odom.twist.twist.linear.z = 0;
odom.twist.twist.linear.z = twist.linear.z; //Workaround for auxiliary drive
odom.twist.twist.linear.z = twist.linear.z; // Workaround for auxiliary drive
odom.twist.twist.angular.x = 0;
odom.twist.twist.angular.y = 0;
odom.twist.twist.angular.z = twist.angular.z;
}
void Basecontroller::calcOdomTF(geometry_msgs::TransformStamped& odomTf, nav_msgs::Odometry odom)
......@@ -276,7 +266,7 @@ void Basecontroller::calcOdomTF(geometry_msgs::TransformStamped& odomTf, nav_msg
void Basecontroller::parsePayload(AllMotorsNS::ActualVelocitiesInterface& vel, emros_dispatcher::eth msg)
{
if (msg.payload.size() == vel.getRawSize())
{ //TODO: error handling
{ // TODO: error handling
vel.set(&msg.payload[0], msg.payload.size());
}
else
......
......@@ -10,15 +10,10 @@
#include "ros/ros.h"
#include <iostream>
int main(int argc, char **argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "basecontroller");
Basecontroller Base;
ros::spin();
return 0;
}
......@@ -11,103 +11,102 @@ namespace AllMotorsNS = EMROSCmdType::AllMotorsController;
void MecanumKinematics::invKin(TargetVelocitiesInterface& wheel_vel_milli_rad_per_sec, geometry_msgs::Twist& twist)
{
//TODO NULLPTR ?
TargetWheelVelocities4WD &vel { dynamic_cast<TargetWheelVelocities4WD&>(wheel_vel_milli_rad_per_sec)};
// TODO NULLPTR ?
TargetWheelVelocities4WD& vel{ dynamic_cast<TargetWheelVelocities4WD&>(wheel_vel_milli_rad_per_sec) };
vel.data().front_right_wheel =
int((1000 / mWheel_radius) *
(twist.linear.x + twist.linear.y + ((mConfig.length_robot + mConfig.width_robot) / 2) * twist.angular.z));
vel.data().front_right_wheel = int((1000 / mWheel_radius) * (twist.linear.x + twist.linear.y +
((mConfig.length_robot + mConfig.width_robot) / 2) * twist.angular.z));
vel.data().front_left_wheel =
int((1000 / mWheel_radius) *
(twist.linear.x - twist.linear.y - ((mConfig.length_robot + mConfig.width_robot) / 2) * twist.angular.z));
vel.data().front_left_wheel = int((1000 / mWheel_radius) * (twist.linear.x - twist.linear.y -
((mConfig.length_robot + mConfig.width_robot) / 2) * twist.angular.z));
vel.data().rear_right_wheel =
int((1000 / mWheel_radius) *
(twist.linear.x - twist.linear.y + ((mConfig.length_robot + mConfig.width_robot) / 2) * twist.angular.z));
vel.data().rear_right_wheel = int((1000 / mWheel_radius) * (twist.linear.x - twist.linear.y +
((mConfig.length_robot + mConfig.width_robot) / 2) * twist.angular.z));
vel.data().rear_left_wheel = int((1000 / mWheel_radius) * (twist.linear.x + twist.linear.y -
((mConfig.length_robot + mConfig.width_robot)) / 2 * twist.angular.z));
vel.data().rear_left_wheel =
int((1000 / mWheel_radius) *
(twist.linear.x + twist.linear.y - ((mConfig.length_robot + mConfig.width_robot)) / 2 * twist.angular.z));
}
void MecanumKinematics::fwdKin(geometry_msgs::Twist& twist, AllMotorsNS::ActualVelocitiesInterface& wheel_vel_milli_rad_per_sec)
void MecanumKinematics::fwdKin(geometry_msgs::Twist& twist,
AllMotorsNS::ActualVelocitiesInterface& wheel_vel_milli_rad_per_sec)
{
AllMotorsNS::ActualWheelVelocities4WD &vel = dynamic_cast<AllMotorsNS::ActualWheelVelocities4WD&>(wheel_vel_milli_rad_per_sec);
//TODO NULLPTR ?
AllMotorsNS::ActualWheelVelocities4WD& vel =
dynamic_cast<AllMotorsNS::ActualWheelVelocities4WD&>(wheel_vel_milli_rad_per_sec);
// TODO NULLPTR ?
twist.linear.x = (mWheel_radius / 4000) * (vel.data().front_left_wheel + vel.data().front_right_wheel +
vel.data().rear_left_wheel + vel.data().rear_right_wheel);
vel.data().rear_left_wheel + vel.data().rear_right_wheel);
twist.linear.y = (mWheel_radius / 4000) * (-vel.data().front_left_wheel + vel.data().front_right_wheel +
vel.data().rear_left_wheel - vel.data().rear_right_wheel);
vel.data().rear_left_wheel - vel.data().rear_right_wheel);
twist.linear.z = 0;
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = (mWheel_radius / 4000) * (2 / (mConfig.length_robot + mConfig.width_robot))
* (-vel.data().front_left_wheel + vel.data().front_right_wheel - vel.data().rear_left_wheel
+ vel.data().rear_right_wheel);
twist.angular.z = (mWheel_radius / 4000) * (2 / (mConfig.length_robot + mConfig.width_robot)) *
(-vel.data().front_left_wheel + vel.data().front_right_wheel - vel.data().rear_left_wheel +
vel.data().rear_right_wheel);
}
void DifferentialKinematics::invKin(TargetVelocitiesInterface& wheel_vel_milli_rad_per_sec, geometry_msgs::Twist& twist)
{
TargetWheelVelocities2WD &vel = dynamic_cast<TargetWheelVelocities2WD&> (wheel_vel_milli_rad_per_sec);
//TODO NULLPTR ?
TargetWheelVelocities2WD& vel = dynamic_cast<TargetWheelVelocities2WD&>(wheel_vel_milli_rad_per_sec);
// TODO NULLPTR ?
vel.data().left =int(1000 / mWheel_radius
* (twist.linear.x - ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().right =int(1000 / mWheel_radius
* (twist.linear.x + ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().left = int(1000 / mWheel_radius * (twist.linear.x - ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().right = int(1000 / mWheel_radius * (twist.linear.x + ((mConfig.width_robot / 2) * twist.angular.z)));
}
void DifferentialKinematics::fwdKin(geometry_msgs::Twist& twist, AllMotorsNS::ActualVelocitiesInterface& wheel_vel_milli_rad_per_sec)
void DifferentialKinematics::fwdKin(geometry_msgs::Twist& twist,
AllMotorsNS::ActualVelocitiesInterface& wheel_vel_milli_rad_per_sec)
{
AllMotorsNS::ActualWheelVelocities2WD &vel = dynamic_cast<AllMotorsNS::ActualWheelVelocities2WD&>(wheel_vel_milli_rad_per_sec);
//TODO NULLPTR ?
ROS_DEBUG_STREAM("right:"<< vel.data().right << " ; left: " << vel.data().left);
twist.linear.x = (mWheel_radius / 2000)
* (vel.data().right + vel.data().left);
AllMotorsNS::ActualWheelVelocities2WD& vel =
dynamic_cast<AllMotorsNS::ActualWheelVelocities2WD&>(wheel_vel_milli_rad_per_sec);
// TODO NULLPTR ?
ROS_DEBUG_STREAM("right:" << vel.data().right << " ; left: " << vel.data().left);
twist.linear.x = (mWheel_radius / 2000) * (vel.data().right + vel.data().left);
twist.linear.y = 0;
twist.linear.z = 0;
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = (mWheel_radius / (mConfig.width_robot*1000))
* (-vel.data().left + vel.data().right);
twist.angular.z = (mWheel_radius / (mConfig.width_robot * 1000)) * (-vel.data().left + vel.data().right);
}
void DifferentialPlusAuxiliaryKinematics::invKin(TargetVelocitiesInterface& output_vel_milli_rad_per_sec, geometry_msgs::Twist& twist)
void DifferentialPlusAuxiliaryKinematics::invKin(TargetVelocitiesInterface& output_vel_milli_rad_per_sec,
geometry_msgs::Twist& twist)
{
TargetWheelVelocities2WDplusAuxiliary &vel = dynamic_cast<TargetWheelVelocities2WDplusAuxiliary&> (output_vel_milli_rad_per_sec);
//TODO NULLPTR ?
vel.data().left =int(1000 / mWheel_radius
* (twist.linear.x - ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().right =int(1000 / mWheel_radius
* (twist.linear.x + ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().auxiliary =int(2000 * M_PI / mConfig.hoisting_feed_constant_in_m_per_round * twist.linear.z);
TargetWheelVelocities2WDplusAuxiliary& vel =
dynamic_cast<TargetWheelVelocities2WDplusAuxiliary&>(output_vel_milli_rad_per_sec);
// TODO NULLPTR ?
ROS_DEBUG_STREAM("target velocity(milli_rad_per_sec): right:"<< vel.data().right
<< " ; left: " << vel.data().left
<< " ; auxiliary: " << vel.data().auxiliary);
vel.data().left = int(1000 / mWheel_radius * (twist.linear.x - ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().right = int(1000 / mWheel_radius * (twist.linear.x + ((mConfig.width_robot / 2) * twist.angular.z)));
vel.data().auxiliary = int(2000 * M_PI / mConfig.hoisting_feed_constant_in_m_per_round * twist.linear.z);
ROS_DEBUG_STREAM("target velocity(milli_rad_per_sec): right:" << vel.data().right << " ; left: " << vel.data().left
<< " ; auxiliary: " << vel.data().auxiliary);
}
void DifferentialPlusAuxiliaryKinematics::fwdKin(geometry_msgs::Twist& twist, AllMotorsNS::ActualVelocitiesInterface& output_vel_milli_rad_per_sec)
void DifferentialPlusAuxiliaryKinematics::fwdKin(geometry_msgs::Twist& twist,
AllMotorsNS::ActualVelocitiesInterface& output_vel_milli_rad_per_sec)
{
AllMotorsNS::ActualWheelVelocities2WDplusAuxiliary &vel = dynamic_cast<AllMotorsNS::ActualWheelVelocities2WDplusAuxiliary&>(output_vel_milli_rad_per_sec);
//TODO NULLPTR ?
ROS_DEBUG_STREAM("actual velocity(milli_rad_per_sec): right:"<< vel.data().right
<< " ; left: " << vel.data().left
<< " ; auxiliary: " << vel.data().auxiliary);
twist.linear.x = (mWheel_radius / 2000)
* (vel.data().right + vel.data().left);
AllMotorsNS::ActualWheelVelocities2WDplusAuxiliary& vel =
dynamic_cast<AllMotorsNS::ActualWheelVelocities2WDplusAuxiliary&>(output_vel_milli_rad_per_sec);
// TODO NULLPTR ?
ROS_DEBUG_STREAM("actual velocity(milli_rad_per_sec): right:" << vel.data().right << " ; left: " << vel.data().left
<< " ; auxiliary: " << vel.data().auxiliary);
twist.linear.x = (mWheel_radius / 2000) * (vel.data().right + vel.data().left);
twist.linear.y = 0;
twist.linear.z = mConfig.hoisting_feed_constant_in_m_per_round / (2000 * M_PI) * vel.data().auxiliary;
twist.angular.x = 0;
twist.angular.y = 0;
twist.angular.z = (mWheel_radius / (mConfig.width_robot*1000))
* (-vel.data().left + vel.data().right);
twist.angular.z = (mWheel_radius / (mConfig.width_robot * 1000)) * (-vel.data().left + vel.data().right);
ROS_DEBUG_STREAM("odom.twist (m/s): linear.x:"<< twist.linear.x
<< " ; linear.z: " << twist.linear.z
<< " ; angular.z: " << twist.angular.z);
ROS_DEBUG_STREAM("odom.twist (m/s): linear.x:" << twist.linear.x << " ; linear.z: " << twist.linear.z
<< " ; angular.z: " << twist.angular.z);
}
......@@ -9,83 +9,81 @@
#include "emros_dispatcher/eth.h"
#include "geometry_msgs/Twist.h"
//#include "sepp_basecontroller_types.hpp"
#include<math.h>
#include <math.h>
int state = 0;
int tic =0;
int tic = 0;
ros::Publisher* TX;
geometry_msgs::Twist msg;
void timerCallback(const ros::TimerEvent&)
{
tic++;
tic++;
}
int main(int argc, char **argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_basecontroller");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
TX = &pub;
ros::Duration duration(0.5);
duration.sleep();
ros::Rate loop_rate(200);
ros::Timer timer = n.createTimer(ros::Duration(1), timerCallback);
msg.angular.z = 10*(M_PI/180);
/*while (ros::ok())
{
static geometry_msgs::Twist msg;
static int i = 0;
switch (state)
{
case 0:
//msg.angular.z = 20*M_PI/180+0.014;
msg.angular.z = 20*(M_PI/180);
//msg.angular.z = 0;
msg.linear.x = 0;
msg.linear.y = 0;
state = 1;
break;
case 1:
TX->publish(msg);
ros::init(argc, argv, "test_basecontroller");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
TX = &pub;
ros::Duration duration(0.5);
duration.sleep();
ros::Rate loop_rate(200);
ros::Timer timer = n.createTimer(ros::Duration(1), timerCallback);
msg.angular.z = 10 * (M_PI / 180);
/*while (ros::ok())
{
static geometry_msgs::Twist msg;
static int i = 0;
switch (state)
{
case 0:
//msg.angular.z = 20*M_PI/180+0.014;
msg.angular.z = 20*(M_PI/180);
//msg.angular.z = 0;
msg.linear.x = 0;
msg.linear.y = 0;
state = 1;
break;
case 1:
TX->publish(msg);
if(i == 18)
{