Commit a163d4cc authored by Christian Sponfeldner's avatar Christian Sponfeldner

auxiliary drive as z-dimension in odom

parent ceabec7e
......@@ -36,6 +36,7 @@ public:
{
double x;
double y;
double z; //Workaround for auxiliary drive
double phiz;
};
......
......@@ -7,6 +7,7 @@
#include "emros_basecontroller/basecontroller.hpp"
#include "std_msgs/Int32.h"
Basecontroller::Basecontroller() :
mPosition(), mTarget_vel(), mLeakyBucketTimeout(ros::Duration(0.3), 48, 6, 3, ros::Time::now), mID(
......@@ -160,7 +161,7 @@ void Basecontroller::mainLoop()
//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,6 +183,7 @@ void Basecontroller::DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr&
geometry_msgs::Twist twist;
nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odomTf;
switch (msg->cmd)
{
case ACTUAL_VELOCITIES:
......@@ -201,6 +203,7 @@ void Basecontroller::DispatcherRxCallback(const emros_dispatcher::eth::ConstPtr&
mTransformBroadcaster.sendTransform(odomTf);
mPub_odom.publish(odom);
mPub_twist_rx.publish(twist);
//ROS_DEBUG("%d : %d : time: %f dt: %f", vel.data().left, vel.data().right, msg->header.stamp.toSec(),
// dt.toSec());
break;
......@@ -225,15 +228,18 @@ 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.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.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.angular.x = 0;
odom.twist.twist.angular.y = 0;
odom.twist.twist.angular.z = twist.angular.z;
......
......@@ -81,15 +81,21 @@ void DifferentialPlusAuxiliaryKinematics::invKin(TargetVelocitiesInterface& outp
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, ActualVelocitiesInterface& output_vel_milli_rad_per_sec)
{
ActualWheelVelocities2WDplusAuxiliary &vel = dynamic_cast<ActualWheelVelocities2WDplusAuxiliary&>(output_vel_milli_rad_per_sec);
//TODO NULLPTR ?
ROS_DEBUG_STREAM("right:"<< vel.data().right
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;
......@@ -98,4 +104,8 @@ void DifferentialPlusAuxiliaryKinematics::fwdKin(geometry_msgs::Twist& twist, Ac
twist.angular.y = 0;
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);
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment