actionlib support in roscs?
Hi, Does ROSCS supports actionlib and also using custom messages?
ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org |
Hi, Does ROSCS supports actionlib and also using custom messages?
Reading the documentation at http://wiki.ros.org/roscs appears to support custom messages but there does not appear to be actionlib support.
Tully is right. You need to send the msgs, which are autogenerated by actionlib and then you are missing the support of actionlib server and/or client. Here is one messy little class that did the job for us so far.
using System;
using System.Threading;
using System.Collections;
using System.Collections.Generic;
using RosCS;
using RosCS.geometry_msgs;
using RosCS.nav_msgs;
using RosCS.tmc200_msgs;
using RosCS.actionlib_msgs;
using RosCS.move_base_msgs;
using RosCS.Services;
using RosCS.Services.nav_msgs;
using Castor;
public class ArmHelper
{
public static Pose PointOfInterest { get; private set; }
public static bool NewPOI { get; set; }
public static bool ExplorationDone { get; set; }
public static GoalID CurrentGoalID{get; private set;}
public static GoalStatusArray GoalStatus {get; private set;}
public static MoveBaseActionFeedback GoalFeedback {get; private set;}
public static MoveBaseActionResult GoalResult {get; set;}
public static bool MotionEnabled { get; set; }
public static string MotionType { get; set; }
protected static Node rosNode;
protected static int id = 0;
protected static MoveBaseActionGoal MoveBaseGoal{get; private set;}
protected static SystemConfig sc = SystemConfig.LocalInstance;
protected static ArmHelper instance = null;
protected static object lockobject = new object();
public static ArmHelper Get() {
if(instance == null) {
lock(lockobject) {
if(instance == null) {
instance = new ArmHelper();
//Thread.MemoryBarrier();
}
}
}
return instance;
}
protected static Publisher motionPub;
protected static Publisher goalPositionPub;
protected static Publisher mBaseGoalPositionPub;
protected static Publisher mBaseCancelGoalPub;
protected static string GenerateUniqueId()
{
id++;
return "goal_" + id;
}
protected static GoalID GenerateActionLibGoalId()
{
CurrentGoalID = new GoalID();
CurrentGoalID.Stamp = RosSharp.Now();
CurrentGoalID.Id = GenerateUniqueId();
return CurrentGoalID;
}
public static void RosInit()
{
if(rosNode!= null) return;
rosNode = Node.MainNode;
ExplorationDone = false;
MotionEnabled = false;
MotionType = sc["ImperaHelper"].GetString("ImperaHelper.PathHelper.MotionType");
mBaseCancelGoalPub = new Publisher(Node.MainNode, "/move_base/cancel", GoalID.TypeId, 1);
goalPositionPub = new Publisher(Node.MainNode, "/move_base_simple/goal", PoseStamped.TypeId, 1);
mBaseGoalPositionPub = new Publisher(Node.MainNode, "/move_base/goal", MoveBaseActionGoal.TypeId, 1);
motionPub = new Publisher(Node.MainNode,"MotionControl",MotionControl.TypeId,1);
Node.MainNode.Subscribe("/move_base/status", OnGoalStatus);
Node.MainNode.Subscribe("/move_base/feedback", OnGoalFeedback);
Node.MainNode.Subscribe("/move_base/result", OnGoalResult);
Node.MainNode.Subscribe("/PointOfInterest", OnPOI);
Node.MainNode.Subscribe("/cmd_vel", OnTwist);
}
protected ArmHelper()
{
if(rosNode == null) {
RosInit();
}
}
protected static void OnTwist(Twist tc)
{
if (!MotionEnabled)
return;
if(MotionType.Equals("tmc"))
{
MotionControl mc = new MotionControl();
mc.Motion.Angle = Math.Atan2(tc.Linear.Y, tc.Linear.X);
mc.Motion.Translation = 1000*Math.Sqrt(tc.Linear.X * tc.Linear.X + tc.Linear.Y * tc.Linear.Y);
mc.Motion.Rotation = tc.Angular.Z;
//Console.WriteLine("Velocity : " + mc.Motion.Translation + " Turn : " + mc.Motion.Rotation + " Angle : " + mc.Motion.Angle);
motionPub.Send(mc);
}
}
protected static void OnPOI(PoseStamped ps) {
NewPOI = true;
PointOfInterest = ps.Pose;
Console.WriteLine("PH: Got new POI");
}
protected static void OnGoalStatus(GoalStatusArray gStatus)
{
GoalStatus = new GoalStatusArray();
GoalStatus.Header = gStatus.Header;
GoalStatus.Status_list = gStatus.Status_list;
//Console.WriteLine("Total Goal Count: "+GoalStatus.Status_list.Count);
}
protected static void OnGoalFeedback(MoveBaseActionFeedback feedback)
{
GoalFeedback = new MoveBaseActionFeedback();
GoalFeedback.Feedback = feedback.Feedback;
GoalFeedback.Header = feedback.Header;
GoalFeedback.Status = feedback.Status;
//Console.WriteLine("Feedback: GoalID: "+GoalFeedback.Status.Goal_id.Id+ " Status: "+GoalFeedback.Status.Status);
}
protected static void OnGoalResult(MoveBaseActionResult result)
{
GoalResult = new MoveBaseActionResult();
GoalResult.Header = result.Header;
GoalResult.Result = result.Result;
GoalResult.Status = result.Status;
Console.WriteLine("Result: GoalID: "+GoalResult.Status.Goal_id.Id ...
(more)Please start posting anonymously - your entry will be published after you log in or create a new account.
Asked: 2012-07-10 22:58:31 -0500
Seen: 414 times
Last updated: Dec 15 '13
Simulating UR5 + PR2 gripper in Gazebo
ActionServer not calling trancition callback on every goal result.
Checking goal status from an action client [closed]
How to cancel actionlib in client using C++
How do you cancel an actionlib action?
Callback Based SimpleActionClient with Python
How to make use of actionfiles between two packages?
action result is published, but callback not being called