Skip to content

Tune controller for moving to artifact origin #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
iche033 wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Tune controller for moving to artifact origin #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e2bc57d
tune controller
iche033 Apr 15, 2020
3a84931
dockerfile changes
iche033 Apr 15, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions docker/subt_solution/Dockerfile
View file
Open in desktop
Copy link

@xingge520 xingge520 May 21, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I can't find the branch "urban_circuit" in the link of https://github.com/osrf/subt.git -b urban_circuit

Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ RUN apt-get update -qq \
libspnav-dev \
libusb-dev \
lsb-release \
mercurial \
python3-dbg \
python3-empy \
python3-numpy \
Expand Down Expand Up @@ -44,6 +43,7 @@ RUN /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb
&& apt-get update -qq \
&& apt-get install -y -qq \
python-catkin-tools \
python-rosdep \
python-rosinstall \
ros-melodic-desktop \
ros-melodic-joystick-drivers \
Expand Down Expand Up @@ -94,8 +94,8 @@ RUN rosdep update

RUN mkdir -p subt_solution/src \
&& cd subt_solution/src \
&& hg clone https://bitbucket.org/osrf/subt -b urban_circuit \
&& hg clone https://bitbucket.org/osrf/subt_seed
&& git clone https://github.com/osrf/subt.git -b urban_circuit \
&& git clone https://github.com/osrf/subt_seed.git

WORKDIR /home/$USERNAME/subt_solution

Expand Down
31 changes: 16 additions & 15 deletions src/subt_seed_node.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ void Controller::Update()

// Create a cmd_vel publisher to control a vehicle.
this->originClient = this->n.serviceClient<subt_msgs::PoseFromArtifact>(
"/subt/pose_from_artifact_origin");
"/subt/pose_from_artifact_origin", true);
this->originSrv.request.robot_name.data = this->name;
}
else
Expand Down Expand Up @@ -231,9 +231,11 @@ not available.");
double cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z);
auto yaw = atan2(siny_cosp, cosy_cosp);

auto yaw90 = M_PI * 0.5;

auto facingFront = abs(yaw) < 0.1;
auto facingEast = abs(yaw + M_PI * 0.5) < 0.1;
auto facingWest = abs(yaw - M_PI * 0.5) < 0.1;
auto facingEast = abs(yaw + yaw90) < 0.1;
auto facingWest = abs(yaw - yaw90) < 0.1;

auto onCenter = abs(pose.position.y) <= 1.0;
auto westOfCenter = pose.position.y > 1.0;
Expand All @@ -248,28 +250,27 @@ not available.");
msg.linear.x = linVel;
msg.angular.z = angVel * -yaw;
}
// Turn to center line
else if (!facingEast && westOfCenter)
{
msg.angular.z = -angVel;
}
else if (!facingWest && eastOfCenter)
// Center line, not facing entrance
else if (!facingFront && onCenter)
{
msg.angular.z = angVel;
msg.angular.z = angVel * -yaw;
}
// Go to center line
else if (facingEast && westOfCenter)
{
msg.linear.x = linVel;
msg.linear.x = linVel * std::min(pose.position.y, 1.0);
}
else if (facingWest && eastOfCenter)
{
msg.linear.x = linVel;
msg.linear.x = linVel * std::min(-pose.position.y, 1.0);
}
// Center line, not facing entrance
else if (onCenter && !facingFront)
else if (westOfCenter)
{
msg.angular.z = angVel * -yaw;
msg.angular.z = angVel * (-yaw90 - yaw);
}
else if (eastOfCenter)
{
msg.angular.z = angVel * (yaw90 - yaw);
}
else
{
Expand Down